geo.c

Go to the documentation of this file.

/*  Global variables and symbols                */

#include <stdio.h>
#include <math.h>
#include "geo.h"

#ifndef real
#define real double
#endif

static real PFSGeoTolerance = 0.0001 ;

#define RAD(x) ((x)*(acos(-1.0)/180.0))

#define dsin(x) sin(RAD(x))
#define dcos(x) cos(RAD(x))
#define dtan(x) tan(RAD(x))


/*      --------------------------------------------------------
**      Private entry points
*/

    /*  --------------------------------------------------------
    **  PFSGeoIntscPolyLine - returns a flag (PFSGeoTrue of PFSGeoFalse) 
    **      indicating whether a horizontal scan line in 2D space 
    **      passing at the given 2D point intersects the given line 
    **      in 2D space from minus infinite to the given point.
    */

PFSGeoPredicate PFSGeoIntscPolyLine( line, spar )

    PFSGeoSurfPar  line[] ;     /* given 2D line                    */
    PFSGeoSurfPar  *spar ;              /* 2D coordinates of pt location    */
{
    real   u0,v0,u1,v1 ;   /* line endpoint 2D coords                       */
    real   ua,va,ub,vb ;   /* corrected line endpoint coordinates           */
    real   max_u,min_u ;   /* maximum and minimum u coordinates     */
    real   max_v,min_v ;   /* maximum and minimum v coordinates     */

    u0 = line[0].u ;
    v0 = line[0].v ;
    u1 = line[1].u ;
    v1 = line[1].v ;

    /*  find the max and min coordinates                        */

    max_u = (u0 > u1) ? u0 : u1 ;
    min_u = (u0 < u1) ? u0 : u1 ;
    max_v = (v0 > v1) ? v0 : v1 ;
    min_v = (v0 < v1) ? v0 : v1 ;

/*
    the horizontal line does not intersect the line to the 
    left of location pt if:
*/
    /*  (1) if line is horizontal                                   */

    if( v0 == v1 ) return( PFSGeoFalse ) ;

    /*  (2) if the v coordinate of point is outside the range 
            max_v and min_v (but not including min_v)               */

    if( (spar->v < min_v) || (spar->v >= max_v) ) return( PFSGeoFalse ) ;

    /*  (3) if given line is vertical and u coordinate of point is 
            smaller than that of line                               */

    if( (u0 == u1) && (spar->u < u0) ) return( PFSGeoFalse ) ;

    /*  (4) if inclined line is located to the right of given point     */
    /*     (first reduce the problem to a case where vb >va, always)*/

    if( u0 != u1 ) {
        if( ((u1 > u0) && (v1 > v0)) || ((u1 < u0) && (v1 > v0)) ) {
            ua = u0 ;    va = v0 ;
            ub = u1 ;    vb = v1 ;
        }
        else {
            ua = u1 ;    va = v1 ;
            ub = u0 ;    vb = v0 ;
        }

        if( ((spar->v - va) * (ub - ua)) > ((spar->u - ua) * (vb - va)) ) {
            return( PFSGeoFalse ) ;
        }
    }

/*
    if we get here that is because the horizontal line passing 
    at the location pt intersects the given line to the left of the pt
*/

return( PFSGeoTrue ) ;
}

/*  --------------------------------------------------------------------
**   The following two routines allow the user to get the current value
**   of the tolerance being used in this module, and to change its value.
**   -sss (27-Jan-90)
*/

void PFSGeoSetTolerance( real tol )
{
    PFSGeoTolerance = tol ;
}

real PFSGeoGetTolerance()
{
    return( PFSGeoTolerance ) ;
}


    /*  --------------------------------------------------------
    **  PFSGeoIsInsidePoly - returns a flag (PFSGeoTrue of PFSGeoFalse) 
    **      indicating whether a point lying on the plane of the 
    **      given polygon is inside it.
    **      A scan line algorithm in the parametric space of the 
    **      polygon plane is used to check whether the given point 
    **      is inside the polygon.  The algorith works as follows:
    **      It gets the projection of each polygon boundary edge onto 
    **      its plane and counts the number of intersections from 
    **      minus infinite in the u direction to the given point 
    **      of a horizontal scan line in the plane parametric space 
    **      with the boundary of the polygon.  If the number of 
    **      intersections is odd the point is inside, otherwise it 
    **      is outside.
    */

PFSGeoPredicate PFSGeoIsInsidePoly( poly, spar )

    PFSGeoPoly  *poly ;         /* given polygon parametric rep.    */
    PFSGeoSurfPar  *spar ;              /* parametric values of pt location */
{
    PFSGeoSurfPar  edge[2] ;    /* projection of a polygon edge on plane    */
    PFSGeoPoint local_pt ;      /* vector from origin of local plane system 
                                    to current vertex on polygon            */
    int         n_cross = 0 ;   /* counts the number of intersections       */
    int         i ;

    for( i = 0; i < poly->n_verts; i++ ) {

        local_pt.x = poly->verts[i].x - poly->nplane.a.x ;
        local_pt.y = poly->verts[i].y - poly->nplane.a.y ;
        local_pt.z = poly->verts[i].z - poly->nplane.a.z ;
        edge[0].u = PFSGeoDotProd( &local_pt, &poly->nplane.u ) ;
        edge[0].v = PFSGeoDotProd( &local_pt, &poly->nplane.v ) ;

        local_pt.x = poly->verts[(i+1) % poly->n_verts].x - poly->nplane.a.x ;
        local_pt.y = poly->verts[(i+1) % poly->n_verts].y - poly->nplane.a.y ;
        local_pt.z = poly->verts[(i+1) % poly->n_verts].z - poly->nplane.a.z ;
        edge[1].u = PFSGeoDotProd( &local_pt, &poly->nplane.u ) ;
        edge[1].v = PFSGeoDotProd( &local_pt, &poly->nplane.v ) ;

        if( PFSGeoIntscPolyLine( edge, spar ) == PFSGeoTrue ) n_cross++ ;
    }

    if( (n_cross % 2) == 0 ) return( PFSGeoFalse ) ;
    else                     return( PFSGeoTrue ) ;
}


    /*  --------------------------------------------------------
    **  PFSGeoIsOnPolyBdry - returns a flag (PFSGeoTrue of PFSGeoFalse) 
    **      indicating whether a point in 3D space is close to 
    **      one of the edges of a given polygon.
    */

PFSGeoPredicate PFSGeoIsOnPolyBdry( poly, pt )

    PFSGeoPoly  *poly ;         /* given polygon parametric rep.    */
    PFSGeoPoint *pt ;           /* given pt location                */
{
    PFSGeoLine  edge ;          /* current polygon edge             */
    PFSGeoPoint clst ;          /* closest point on edge            */
    PFSGeoPointPar par ;                /* parametric value of point on edge*/
    int         is_on_bdry = 0 ;
    int         i ;

    for( i = 0; i < poly->n_verts; i++ ) {
        PFSGeoGenLine( &poly->verts[i], &poly->verts[(i+1) % poly->n_verts], 
                    &edge ) ;
        PFSGeoClstPtLine( &edge, pt, &clst, &par ) ;
        if( ( (real)par > - PFSGeoTolerance ) 
                    && ( (real)par < 1.0 + PFSGeoTolerance ) 
                    && ( PFSGeoDist( pt, &clst ) < PFSGeoTolerance ) ) {
            is_on_bdry = 1 ;
            break ;
        }
    }

    if( is_on_bdry == 0 ) return( PFSGeoFalse ) ;
    else                  return( PFSGeoTrue ) ;
}



/*      --------------------------------------------------------
**      Public entry points
*/

/*      --------------------------------------------------------
**      Generation Routines:

        void PFSGeoGenLine( pt0, pt1, line ) 
            PFSGeoPoint *pt0, *pt1 ;
            PFSGeoLine  *line ;

        pt0     first input point on the line.
        pt1     second input point on the line.
        line    output parametric description of the line.

        given two input points, this function creates the parametric 
        description of the line  p = D + t*E where D = pt0 and
        E = pt1 - pt0.


        void PFSGeoGenPlane( pt0, pt1, pt2, plane ) 
            PFSGeoPoint *pt0, *pt1, *pt2 ; 
            PFSGeoPlane *plane ; 

        pt0     first input point on the plane.
        pt1     second input point on the plane.
        pt2     third input point on the plane.
        plane   output parametric description of the plane.

        given three input point, this function creates the parametric
        description of the plane  p = A + u*B + v*C where A = pt0,
        B = pt1 - pt0, and C = pt2 - pt0.


        void PFSGeoGenPlanEqn( pt0, pt1, pt2, plan_eqn ) 
            PFSGeoPoint *pt0, *pt1, *pt2 ; 
            PFSGeoPlanEqn       *plan_eqn ; 

        pt0     first input point on the plane.
        pt1     second input point on the plane.
        pt2     third input point on the plane.
        plane   output general (minimal) description of the plane

        given three input points, this function creates the general
        description of the plane such that the following eqn. is satisfied
        by any point (X,Y,Z) lying on the plane:  a*X + b*Y + c*Z + d = 0.
        The unit normal to the plane is given by (a,b,c) and points in
        direction defined by  {vec(0,1) cross vec(0,2)} (see below).


                     normal |  + pt2
                            | /
                            |/ 
                            + ------+
                         pt0        pt1



        PFSGeoStatus PFSGeoGenPoly( n_verts, verts, plane )
            int          *n_verts ;
            PFSGeoPoint  *verts ; 
            PFSGeoNormPlane *plane ;

        n_verts number of polygon vertices.
        verts   a list of the polygon vertices.
        plane   output parametric description of the normalized 
                plane of the polygon.

        given a list of polygon vertex coordinates, this function creates
        the normalized parametric description of polygon plane, which 
        means that the two plane local directions are unit perpendicular 
        vectors and the unit normal vector is also contained in the 
        descriptor.
        The vertices are assumed given in clockwise direction around 
        the polygon.  If the computed normal vector is null, a PFSGeoInvalid 
        status is returned.  Otherwise a PFSGeoValid status is returned.
*/

void PFSGeoGenLine( pt0, pt1, line ) 
    PFSGeoPoint *pt0, *pt1 ;
    PFSGeoLine  *line ;
{                          
    line->d = *pt0 ;
    line->e.x = pt1->x - pt0->x ;
    line->e.y = pt1->y - pt0->y ;
    line->e.z = pt1->z - pt0->z ;
}


void PFSGeoGenPlane( pt0, pt1, pt2, plane ) 
    PFSGeoPoint *pt0, *pt1, *pt2 ; 
    PFSGeoPlane *plane ; 
{
    plane->a = *pt0 ;
    plane->b.x = pt1->x - pt0->x ;
    plane->b.y = pt1->y - pt0->y ;
    plane->b.z = pt1->z - pt0->z ;
    plane->c.x = pt2->x - pt0->x ;
    plane->c.y = pt2->y - pt0->y ;
    plane->c.z = pt2->z - pt0->z ;
}


void PFSGeoGenPlanEqn( pt0, pt1, pt2, plan_eqn ) 
    PFSGeoPoint *pt0, *pt1, *pt2 ; 
    PFSGeoPlanEqn       *plan_eqn ; 
{
    PFSGeoPoint vec1, vec2, normal ;
    real        length ;

    vec1.x =  pt1->x - pt0->x ;
    vec1.y =  pt1->y - pt0->y ;
    vec1.z =  pt1->z - pt0->z ;
    vec2.x =  pt2->x - pt0->x ;
    vec2.y =  pt2->y - pt0->y ;
    vec2.z =  pt2->z - pt0->z ;
 
    normal.x = (vec1.y * vec2.z) - (vec1.z * vec2.y) ; 
    normal.y = (vec1.z * vec2.x) - (vec1.x * vec2.z) ; 
    normal.z = (vec1.x * vec2.y) - (vec1.y * vec2.x) ; 

    length = sqrt(     normal.x*normal.x
                    +  normal.y*normal.y
                    +  normal.z*normal.z  ) ;

    plan_eqn->a = normal.x / length ;
    plan_eqn->b = normal.y / length ;
    plan_eqn->c = normal.z / length ;
    plan_eqn->d = (  - plan_eqn->a * pt0->x
                     - plan_eqn->b * pt0->y
                     - plan_eqn->c * pt0->z  ) ;

return;
}


PFSGeoStatus PFSGeoGenPoly( n_verts, verts, plane )
    int          *n_verts ;
    PFSGeoPoint  *verts ; 
    PFSGeoNormPlane *plane ;
{
    real  e_size ;                  /* length of last polygon edge  */

    if ( PFSGeoPolyNormal( n_verts, verts, &plane->n ) != PFSGeoValid )
        return( PFSGeoInvalid ) ;

    e_size = PFSGeoDist( &verts[0], &verts[*n_verts-1] ) ;
    plane->a = verts[0] ;

    plane->u.x = (verts[*n_verts-1].x - verts[0].x) / e_size ;
    plane->u.y = (verts[*n_verts-1].y - verts[0].y) / e_size ;
    plane->u.z = (verts[*n_verts-1].z - verts[0].z) / e_size ;

    PFSGeoCrossProd( &plane->n, &plane->u, &plane->v ) ;
    return( PFSGeoValid );
}


/*      --------------------------------------------------------
**      Evaluation Routines:

        void PFSGeoEvalLine( par, line, pt ) 
            PFSGeoPointPar          *par ;
            PFSGeoLine      *line ;
            PFSGeoPoint     *pt ;

        par     input parametric value along the line.
        line    input parametric description of a line.
        pt      output evaluated cartesian point.

        given a parametric description of a line and a parameter
        value along the line, this routine generates the cartesian
        point coresponding to the given parametric value.


        void PFSGeoEvalPlane( par, plane, pt )  
            PFSGeoSurfPar       *par ;
            PFSGeoPlane *plane ;
            PFSGeoPoint *pt ;

        par     input parametric surface values.
        plane   input parametric representation of a plane.
        pt      output evaluated cartesian point.

        given a parametric description of a plane and surface
        parameter values (u and v), this routine generates the
        cartesian point coresponding to the given surface parameter.


*/

void PFSGeoEvalLine( par, line, pt ) 
    PFSGeoPointPar          *par ;
    PFSGeoLine      *line ;
    PFSGeoPoint     *pt ;
{
    pt->x = line->d.x + (*par * line->e.x) ;   
    pt->y = line->d.y + (*par * line->e.y) ;   
    pt->z = line->d.z + (*par * line->e.z) ;   
    return ;
}


void PFSGeoEvalPlane( par, plane, pt )  
    PFSGeoSurfPar       *par ;
    PFSGeoPlane *plane ;
    PFSGeoPoint *pt ;
{
    pt->x = plane->a.x + (par->u * plane->b.x) + (par->v * plane->c.x) ;   
    pt->y = plane->a.y + (par->u * plane->b.y) + (par->v * plane->c.y) ;   
    pt->z = plane->a.z + (par->u * plane->b.z) + (par->v * plane->c.z) ;   
}


/*      --------------------------------------------------------
**      Vector Routines:


        real PFSGeoDotProd( vec0, vec1 )        
            PFSGeoPoint *vec0, *vec1 ;

        vec0    first input vector.
        vec1    second input vector.

        The function value is the value of the dot product of
        the two three element input vectors.


        void PFSGeoCrossProd( in0, in1, out )
            PFSGeoPoint *in0, *in1, *out ;

        in0     first input vector.
        in1     second input vector.
        out     output cross product vector.

        This function computes the cross product of the two
        three element input vectors.  The cross product components
        are returned as the third argument.


        real PFSGeoTripleProd( vec0, vec1, vec2 ) 
            PFSGeoPoint    *vec0, *vec1, *vec2 ;

        vec0    first input vector.
        vec1    second input vector.
        vec2    third input vector.
        
        The returned function value is the value of the triple
        product of the three input vectors.


        PFSGeoPredicate PFSGeoNullVec( vec, tolerance )
            PFSGeoPoint *vec ;
            real        *tolerance ;

        vec             input vector.
        tolerance       check tolerance.

        This function return a value of PFSGeoTrue if all absolute values 
        of the three elements of vec are less the the value of tolerance.  
        Otherwise a value of PFSGeoFalse is returned.


        real PFSGeoVecLen( vec )
            PFSGeoPoint *vec ;

        vec     input vector.

        The value of this function is the length of the input
        vector.


        void PFSGeoVecNormalize( vec, n )
            PFSGeoPoint *vec ;
            PFSGeoPoint *n ;

        vec     input vector.
        n       normalized vector.

        This function normalizes a vector (i.e. finds a vector
        with the same direction but a magnitude of one).


        real    PFSGeoDist( in0, in1 )
            PFSGeoPoint *in0 ;
            PFSGeoPoint *in1 ;

        in0     input point0
        in1     input point1

        The value of this function is the distance between the
        two given points.


        void    PFSGeoDiffVec( in0, in1, out)
            PFSGeoPoint *in0, *in1, *out ;

        in0, in1 : input vectors
        out : output vector = in0 - in1 ;

        computes the difference between two vectors 


        void    PFSGeoSumVec( in0, in1, out)
            PFSGeoPoint    *in0, *in1, *out ;

        in0, in1 : input vectors
        out : output vector = in0 + in1 ;

        computes the sum of two vectors

 
        void PFSGeoProdScalVec ( in, scalar, out )
            PFSGeoPoint    *in, *out ;
            real       *csi ;

        in:     input vector
        out :   output vector = csi * in ;

        computes the product of a vector by a scalar

        void    PFSGeoLinCombVec( in0, csi0, in1, csi1, out )
            PFSGeoPoint    *in1, *in2, *out ; 
            real        *csi0, *csi1 ;
        in0, in1, :     input vectors ;
        csi0, csi1 :    coefficients corresponding to in0 and in1
        out :   output vector = csi0 * in0 + cs1 * in1 ;

        computes the linear combination of two vectors

*/

real PFSGeoDotProd( vec0, vec1 )        
    PFSGeoPoint *vec0, *vec1 ;
{
    return ( (vec0->x * vec1->x) + 
             (vec0->y * vec1->y) +
             (vec0->z * vec1->z) ) ;
}    

void PFSGeoCrossProd( in0, in1, out )
    PFSGeoPoint *in0, *in1, *out ;
{
    out->x = (in0->y * in1->z) - (in0->z * in1->y) ; 
    out->y = (in0->z * in1->x) - (in0->x * in1->z) ; 
    out->z = (in0->x * in1->y) - (in0->y * in1->x) ; 
}    

real PFSGeoTripleProd( vec0, vec1, vec2 ) 
    PFSGeoPoint    *vec0, *vec1, *vec2 ;
{
    PFSGeoPoint tmp ;

    PFSGeoCrossProd( vec0, vec1, &tmp ) ;
    return( PFSGeoDotProd( &tmp, vec2 ) ) ;
}

PFSGeoPredicate PFSGeoNullVec( vec, tolerance )
    PFSGeoPoint *vec ;
    real        *tolerance ;
{
    if (( fabs( vec->x ) < *tolerance ) &&  
        ( fabs( vec->y ) < *tolerance ) &&  
        ( fabs( vec->z ) < *tolerance ))
        return( PFSGeoTrue ) ;
    else
        return( PFSGeoFalse );
}

real PFSGeoVecLen( vec )
    PFSGeoPoint *vec ;
{
    return ( sqrt( (vec->x * vec->x) +
                   (vec->y * vec->y) +
                   (vec->z * vec->z) ) ) ;
}


void  PFSGeoVecNormalize( vec, n )
    PFSGeoPoint *vec ;
    PFSGeoPoint *n ;
{
    real        len ;

    len = PFSGeoVecLen( vec ) ;

    n->x = vec->x / len ;
    n->y = vec->y / len ;
    n->z = vec->z / len ;
}


real    PFSGeoDist( in0,in1 )
    PFSGeoPoint *in0 ;
    PFSGeoPoint *in1 ;
{
    PFSGeoPoint tmp ;

    tmp.x = in1->x - in0->x ;
    tmp.y = in1->y - in0->y ;
    tmp.z = in1->z - in0->z ;

    return( PFSGeoVecLen( &tmp ) ) ;
}

void    PFSGeoDiffVec( in0, in1, out)
    PFSGeoPoint *in0, *in1, *out ;
    
{
    out->x = in0->x - in1->x ;
    out->y = in0->y - in1->y ;
    out->z = in0->z - in1->z ;
    return ;
} 

void    PFSGeoSumVec( in0, in1, out)
    PFSGeoPoint *in0, *in1, *out ;
    
{
    out->x = in0->x + in1->x ;
    out->y = in0->y + in1->y ;
    out->z = in0->z + in1->z ;
    return ;
} 


void PFSGeoProdScalVec ( in, csi, out )
    PFSGeoPoint *in, *out ;
    real        *csi ;
{
    out->x = in->x * (*csi) ;
    out->y = in->y * (*csi) ;
    out->z = in->z * (*csi) ;
    return ;
}


void    PFSGeoLinCombVec( in0, csi0, in1, csi1, out )
    PFSGeoPoint    *in0, *in1, *out ; 
    real        *csi0, *csi1 ;
{
    out->x = in0->x * (*csi0) + in1->x * (*csi1) ;
    out->y = in0->y * (*csi0) + in1->y * (*csi1) ;
    out->z = in0->z * (*csi0) + in1->z * (*csi1) ;

    return ;

}

/*      --------------------------------------------------------
**      Rotation Routines:


        void PFSGeoRotateX( angle, in, out ) 
            real        *angle ;
            PFSGeoPoint *in, *out ;

        angle   rotation angle (in degrees).
        in      input cartesian point.
        out     rotated cartesian point.

        This function takes an input and rotates it about the X
        axis by the number of degrees specified by angle.  The
        new point position is returned as the third argument.


        void PFSGeoRotateY( angle, in, out ) 
            real        *angle ;
            PFSGeoPoint *in, *out ;

        angle   rotation angle (in degrees).
        in      input cartesian point.
        out     rotated cartesian point.

        This function takes an input and rotates it about the Y
        axis by the number of degrees specified by angle.  The
        new point position is returned as the third argument.


        void PFSGeoRotateZ( angle, in, out ) 
            real        *angle ;
            PFSGeoPoint *in, *out ;

        angle   rotation angle (in degrees).
        in      input cartesian point.
        out     rotated cartesian point.

        This function takes an input and rotates it about the Z
        axis by the number of degrees specified by angle.  The
        new point position is returned as the third argument.


        void PFSGeoRotateAxis( axis, angle, in, out ) 
            PFSGeoPoint *axis ;
            real        *angle ;
            PFSGeoPoint *in, *out ;

        axis    asix about which to rotate.
        angle   rotation angle (in degrees).
        in      input cartesian point.
        out     rotated cartesian point.

        This function takes an input vector and rotates it about
        a given axis (passing through the origin) by the number 
        of degrees specified by angle.  The new point position
        is returned as the fourth argument.

*/

void PFSGeoRotateX( angle, in, out ) 
    real        *angle ;
    PFSGeoPoint *in, *out ;
{
    real        s, c ;

    s = dsin( *angle) ;   c = dcos( *angle ) ;
    out->x = in->x ;
    out->y = (in->y * c) - (in->z * s) ;
    out->z = (in->y * s) + (in->z * c) ;
}


void PFSGeoRotateY( angle, in, out ) 
    real        *angle ;
    PFSGeoPoint *in, *out ;
{         
    real        s, c ;

    s = dsin( *angle) ;   c = dcos( *angle ) ;
    out->x = (in->x * c) - (in->z * s) ;
    out->y = in->y ;
    out->z = (in->x * s) + (in->z * c) ;
}


void PFSGeoRotateZ( angle, in, out ) 
    real        *angle ;
    PFSGeoPoint *in, *out ;
{
    real        s, c ;

    s = dsin( *angle ) ;    c = dcos( *angle ) ;
    out->x = (in->x * c) - (in->y * s) ;
    out->y = (in->x * s) + (in->y * c) ;
    out->z = in->z ;
}


void PFSGeoRotateAxis( axis, angle, in, out ) 
    PFSGeoPoint *axis ;
    real        *angle ;
    PFSGeoPoint *in, *out ;
{
    PFSGeoPoint u ;
    real        s, c ;
    real        r[3][3] ;

    s = dsin( *angle ) ;    c = dcos( *angle ) ;

    PFSGeoVecNormalize( axis, &u ) ;

    r[0][0] = u.x*u.x + c * (1.0 - u.x*u.x) ;
    r[0][1] = u.x*u.y*(1.0 - c) - u.z*s ;
    r[0][2] = u.z*u.x*(1.0 - c) + u.y*s ;

    r[1][0] = u.x*u.y*(1.0 - c) + u.z*s ;
    r[1][1] = u.y*u.y + c * (1.0 - u.y*u.y) ;
    r[1][2] = u.y*u.z*(1.0 - c) - u.x*s ;

    r[2][0] = u.z*u.x*(1.0 - c) - u.y*s ;
    r[2][1] = u.y*u.z*(1.0 - c) + u.x*s ;
    r[2][2] = u.z*u.z + c * (1.0 - u.z*u.z) ;

    out->x = r[0][0]*in->x + r[0][1]*in->y + r[0][2]*in->z ;
    out->y = r[1][0]*in->x + r[1][1]*in->y + r[1][2]*in->z ;
    out->z = r[2][0]*in->x + r[2][1]*in->y + r[2][2]*in->z ;
}


/*      --------------------------------------------------------
**      Intersection Routines:


        PFSGeoStatus PFSGeoIntscLinePlane( line, plane, pt, ptpar, spar ) 
            PFSGeoLine  *line ;
            PFSGeoPlane *plane ;
            PFSGeoPoint *pt ;
            PFSGeoPointPar      *ptpar ;
            PFSGeoSurfPar  *spar ;

        line    input parametric description of a line.
        plane   input parametric description of a plane.
        pt      output intersection point.
        ptpar   output parametric value along the line.
        spar    output surface parameters for the plane.

        This function computes the intersection point of a line and a
        plane both given as parametric representations.  The parametric
        value of the intersection along the line as well as the 
        parametric values of the intersection point on the plane are
        also returned.


        PFSGeoStatus PFSGeoIntscLinePoly( line, poly, pt, ptpar, spar ) 
            PFSGeoLine  *line ;
            PFSGeoPoly  *poly ;
            PFSGeoPoint *pt ;
            PFSGeoPointPar      *ptpar ;
            PFSGeoSurfPar  *spar ;

        line    input parametric description of a line.
        poly    input parametric description of a polygon.
        pt      output intersection point.
        ptpar   output parametric value along the line.
        spar    output surface parameters for the plane.

        This function computes the intersection point of a line and a
        planar polygon both given as parametric representations.  The 
        parametric value of the intersection along the line as well as the 
        parametric values of the intersection point on the poly are
        also returned.
        First it is found whether the line intersects the plane which 
        contains the polygon, then, using a scan line algorithm in the 
        parametric space of that plane, it is checked whether the 
        intersection point is inside the polygon.
*/

PFSGeoStatus PFSGeoIntscLinePlane( line, plane, pt, ptpar, spar ) 
    PFSGeoLine  *line ;
    PFSGeoPlane *plane ;
    PFSGeoPoint *pt ;
    PFSGeoPointPar      *ptpar ;
    PFSGeoSurfPar  *spar ;
{
    real        size ;
    PFSGeoPoint b_cross_c, c_cross_e, b_cross_e ;
    real        b_x_c_dot_e, c_x_e_dot_b, b_x_e_dot_c ;

    size = PFSGeoVecLen( &plane->b ) ;

    PFSGeoCrossProd( &plane->b, &plane->c, &b_cross_c ) ;
    PFSGeoCrossProd( &plane->c, &line->e,  &c_cross_e ) ;
    PFSGeoCrossProd( &plane->b, &line->e,  &b_cross_e ) ;
         
    b_x_c_dot_e = PFSGeoDotProd( &b_cross_c, &line->e ) ;
    c_x_e_dot_b = PFSGeoDotProd( &c_cross_e, &plane->b ) ;
    b_x_e_dot_c = PFSGeoDotProd( &b_cross_e, &plane->c ) ;

    if (( fabs(b_x_c_dot_e) < PFSGeoTolerance*size ) || 
        ( fabs(c_x_e_dot_b) < PFSGeoTolerance*size ) || 
        ( fabs(b_x_e_dot_c) < PFSGeoTolerance*size )) 
        return( PFSGeoInvalid ) ;
    else {
        *ptpar = ( PFSGeoDotProd( &b_cross_c, &plane->a ) -
                   PFSGeoDotProd( &b_cross_c, &line->d ) ) /  
                   b_x_c_dot_e ;
        PFSGeoEvalLine( ptpar, line, pt ) ;
        spar->u = ( PFSGeoDotProd( &c_cross_e, &line->d ) -
                    PFSGeoDotProd( &c_cross_e, &plane->a ) ) /
                    c_x_e_dot_b ;
        spar->v = ( PFSGeoDotProd( &b_cross_e, &line->d ) -
                    PFSGeoDotProd( &b_cross_e, &plane->a ) ) /
                    b_x_e_dot_c ;
        return( PFSGeoValid ) ;
    }
}



PFSGeoStatus PFSGeoIntscLinePoly( line, poly, pt, ptpar, spar ) 
    PFSGeoLine  *line ;
    PFSGeoPoly  *poly ;
    PFSGeoPoint *pt ;
    PFSGeoPointPar      *ptpar ;
    PFSGeoSurfPar  *spar ;
{
    PFSGeoPlane plane ;

    /*  first find if line intersects the plane which 
        contains the polygon                                            */

    plane.a = poly->nplane.a ;
    plane.b = poly->nplane.u ;
    plane.c = poly->nplane.v ;
    if( PFSGeoIntscLinePlane( line, &plane, pt, ptpar, spar ) == PFSGeoValid ) {

        /*  then check to see if intersection point is inside polygon
            or on the boundary of polygon                               */

        if( PFSGeoIsInsidePoly( poly, spar ) == PFSGeoTrue ) {
            return( PFSGeoValid ) ;
        }
        else {
            if( PFSGeoIsOnPolyBdry( poly, pt ) == PFSGeoTrue ) {
                return( PFSGeoValid ) ;
            }
            else {
                return( PFSGeoInvalid ) ;
            }
        }
    }
    else return( PFSGeoInvalid ) ;
}



/*      --------------------------------------------------------
**      Find Normal Routines:

        PFSGeoStatus PFSGeoPolyNormal( n_verts, verts, n )
            int         *n_verts ; 
            PFSGeoPoint *verts ;
            PFSGeoPoint *n ;

        n_verts input number of polgon vertices.
        verts   input polygon vertices.
        n       ouput polygon normal.

        This function computes a unit vector normal of polygon given 
        by a list of vertex coordinates.  The vertices are assumed
        given in a clockwise order around the polygon.  If the computed
        normal vector is null, a PFSGeoInvalid status is returned. Otherwise
        a value of PFSGeoValid is returned.

        The algorithm for the routine is as follows:
            Traverse the loop of coordinates, assuming that it is in
            clockwise order, computing the components of the "area" of the
            enclosed polygon.  The total "area" components are computed by
            adding "area" components (cross product components) of
            triangles sides formed by the first, previous, and current
            vertices.  If the loop is not convex, some of the triangle
            areas might be negative, but those will be compensated by other
            positive triangle areas so that the final area is positive.
            (area here is actually twice the area).
            (positive here means in the direction of the face normal).


        PFSGeoStatus  PFSGeoLstSqrPlanEqn( num_pts, pts, plan_eqn ) 
            int         *num_pts ;
            PFSGeoPoint *pts ;
            PFSGeoPlanEqn       *plan_eqn ;

        num_pts     number of points
        pts         list of points
        plan_eqn    plane equation

        Given a list of points this function computes the equation 
        of the plane which correspond to the least square fit 
        through the points.
        The equation is given in the form:
            a * x  +  b * y  +  c * z  +  d  = 0

*/


PFSGeoStatus PFSGeoPolyNormal( n_verts, verts, n )
    int         *n_verts ; 
    PFSGeoPoint *verts ;
    PFSGeoPoint *n ;
{
    real        e_size;         /* length of first edge         */
    real        n_size;         /* normal length                */
    int         i;              /* loop counter                 */

    n->x = 0.0 ; n->y = 0.0 ; n->z = 0.0 ;
             
    for ( i=2 ; i < *n_verts ; i++ ) {
        n->x +=  (verts[i  ].y - verts[0].y) * 
                 (verts[i-1].z - verts[0].z) -
                 (verts[i  ].z - verts[0].z) * 
                 (verts[i-1].y - verts[0].y) ;

        n->y += -(verts[i  ].x - verts[0].x) * 
                 (verts[i-1].z - verts[0].z) +
                 (verts[i  ].z - verts[0].z) * 
                 (verts[i-1].x - verts[0].x) ;

        n->z +=  (verts[i  ].x - verts[0].x) * 
                 (verts[i-1].y - verts[0].y) -
                 (verts[i  ].y - verts[0].y) * 
                 (verts[i-1].x - verts[0].x) ;
    }

/*  normalize                                                   */
/*  get the length of the first edge (to compute ratio norm_size/edge_size) */

    e_size = PFSGeoDist( &verts[0], &verts[1] ) ;
    n_size = PFSGeoVecLen( n ) ;

    if (n_size/e_size > PFSGeoTolerance) {
        n->x /= n_size ;
        n->y /= n_size ;
        n->z /= n_size ;
        return( PFSGeoValid ) ;
    }
    else 
        return( PFSGeoInvalid );
}


PFSGeoStatus  PFSGeoLstSqrPlanEqn( num_pts, pts, plan_eqn ) 
    int         *num_pts ;      /* number of points     */
    PFSGeoPoint *pts ;          /* list of points       */
    PFSGeoPlanEqn       *plan_eqn ;     /* plane equation       */

{
    real        a11,a12,a13,a22,a23,a33 ;       /* A matrix         */
    real        b11,b12,b13,b22,b23,b33 ;       /* A inverse matrix */
    real        sum_x,sum_y,sum_z ;             /* summed values    */
    real        div,norm ;                      /* auxiliar coeff.  */
    PFSGeoPoint    seg1, seg2, cross ;
    int         i ;                             /* counter          */

    /*  loop through the points and calculate the sums and products  */

    a11 = 0.0 ;     a12 = 0.0 ;     a13 = 0.0 ;
    a22 = 0.0 ;     a23 = 0.0 ;     a33 = 0.0 ;

    sum_x = 0.0 ;   sum_y = 0.0 ;   sum_z = 0.0 ;

    for ( i = 0; i < *num_pts; i++ ) {
        a11 = a11 + pts[i].x * pts[i].x ;
        a12 = a12 + pts[i].x * pts[i].y ;
        a13 = a13 + pts[i].x * pts[i].z ;
        a22 = a22 + pts[i].y * pts[i].y ;
        a23 = a23 + pts[i].y * pts[i].z ;
        a33 = a33 + pts[i].z * pts[i].z ;

        sum_x = sum_x + pts[i].x ;
        sum_y = sum_y + pts[i].y ;
        sum_z = sum_z + pts[i].z ;
    }

    /*  compute the inverse of A                            */

    div = a11 * ( a22*a33 - a23*a23 ) + a12 * ( a13*a23 - a12*a33 ) +
          a13 * ( a12*a23 - a13*a22 ) ;

    /* Check for possible trouble.  Either colinear points or
       all pts. lie near to a plane which passes through origin resulting
       in the A matrix to be singular, e.g., if only three pts. then
       third row is linear combo. of top two rows.
    */

    if ( fabs(div) < PFSGeoTolerance ) {

        /* Find first two segments which are non-colinear.
           Use these to form the plane eqn.
        */
        for( i = 0; i < *num_pts - 2; i++ ) {
            PFSGeoDiffVec( &pts[i+1], &pts[i], &seg1 ) ;
            PFSGeoDiffVec( &pts[i+2], &pts[i+1], &seg2 ) ;
            PFSGeoCrossProd( &seg1, &seg2, &cross ) ;
            if( PFSGeoNullVec( &cross, &PFSGeoTolerance ) == PFSGeoFalse ) {
                PFSGeoGenPlanEqn( &pts[i], &pts[i+1], &pts[i+2], plan_eqn ) ;
                return( PFSGeoValid ) ;
            }
        }

        return( PFSGeoInvalid ) ;
    }
    else {
    /*  compute the inverse of A coefficients               */

        b11 = ( a22*a33 - a23*a23 ) / div ;
        b12 = ( a13*a23 - a12*a33 ) / div ;
        b13 = ( a12*a23 - a13*a22 ) / div ;
        b22 = ( a11*a33 - a13*a13 ) / div ;
        b23 = ( a12*a13 - a11*a23 ) / div ;
        b33 = ( a11*a22 - a12*a12 ) / div ;

    /*  compute the equation coefficients                   */

        plan_eqn->a = b11*sum_x + b12*sum_y + b13*sum_z ;
        plan_eqn->b = b12*sum_x + b22*sum_y + b23*sum_z ;
        plan_eqn->c = b13*sum_x + b23*sum_y + b33*sum_z ;

    /*  normalize the equation coefficients                 */

        norm = sqrt( plan_eqn->a * plan_eqn->a + plan_eqn->b * plan_eqn->b + 
                     plan_eqn->c * plan_eqn->c ) ;

        plan_eqn->a /= norm ;
        plan_eqn->b /= norm ;
        plan_eqn->c /= norm ;
        plan_eqn->d = - 1.0 / norm ;
    }

return( PFSGeoValid ) ;
}



/*      --------------------------------------------------------
**      Closest Point Routines:

        void PFSGeoClstPtLine( line, pt, clst, par ) 
            PFSGeoLine   *line ;
            PFSGeoPoint  *pt, *clst ;
            PFSGeoPointPar  *par ;

        line    input parametric description of a line.
        pt      input cartesian point.
        clst    output closest point.
        par     parametric value of the point.

        Given the parametric description of a line, and a cartesian
        point not on the line, this function computes the cartesian
        coordinates of a point on the line which is closest to the
        input point.  The function also returns the parametric value
        of this point along the input line.


        void PFSGeoClstPtNormPlane( plane, pt, clst, spar ) 
            PFSGeoNormPlane *plane ;
            PFSGeoPoint  *pt, *clst ;
            PFSGeoSurfPar   *spar ;

        plane   input normalized parametric description of a plane.
        pt      input cartesian point.
        clst    output closest point.
        spar    parametric value of the point.

        Given the normalized parametric description of a plane, and a 
        cartesian point not necessarely on the plane, this function 
        computes the cartesian coordinates of a point on the plane 
        which is closest to the input point.  
        The function also returns the parametric values of this point on 
        the plane.  
        It is assumed that the two local vectors on the plane, 
        "u" and "v", are perpendicular-to-each-other unit vectors.


        PFSGeoStatus PFSGeoClstPtPoly( poly, pt, clst, spar ) 
            PFSGeoPoly  *poly ;
            PFSGeoPoint *pt ;
            PFSGeoPoint *clst ;
            PFSGeoSurfPar  *spar ;

        poly    input parametric description of a polygon.
        pt      input cartesian point.
        clst    output closest point.
        spar    parametric value of the point.

        Given the normalized parametric description of a polygon, and a 
        cartesian point not necessarely on its plane, this function 
        computes the cartesian coordinates of a point on the plane 
        which is closest to the input point.  
        The function also returns the parametric values of this point on 
        the plane.  
        First it is obtained the closest point on the plane which
        contains the polygon, then, using a scan line algorithm in the 
        parametric space of that plane, it is checked whether the 
        closest point is inside the polygon.  If the point is inside 
        the polygon, the function returns a PFSGeoValid status, otherwise 
        it returns PFSGeoInvalid.
        It is assumed that the two local vectors on the polygon plane, 
        "u" and "v", are perpendicular-to-each-other unit vectors.


        PFSGeoPredicate PFSGeoIsOnPlane( PFSGeoPoint  *pt, PFSGeoNormPlane  *pla )

        This function determines if the given point, pt,  lies on the
        given plane, pla. THE PLANE SHOULD BE GIVEN AS A POINTER 
        TO A PLANE NORMALIZED DESCRIPTION.
*/


void PFSGeoClstPtLine( line, pt, clst, par ) 
    PFSGeoLine  *line ;
    PFSGeoPoint *pt, *clst ;
    PFSGeoPointPar *par ;
{
    PFSGeoPoint local_pt  ;

    local_pt.x = pt->x - line->d.x ;
    local_pt.y = pt->y - line->d.y ;
    local_pt.z = pt->z - line->d.z ;

    *par = PFSGeoDotProd( &local_pt, &line->e ) / 
           PFSGeoDotProd( &line->e,  &line->e ) ;

    PFSGeoEvalLine( par, line, clst ) ;
}


void PFSGeoClstPtNormPlane( plane, pt, clst, spar ) 
    PFSGeoNormPlane *plane ;
    PFSGeoPoint  *pt, *clst ;
    PFSGeoSurfPar   *spar ;
{
    PFSGeoPoint local_pt ;

    local_pt.x = pt->x - plane->a.x ;
    local_pt.y = pt->y - plane->a.y ;
    local_pt.z = pt->z - plane->a.z ;

    spar->u = - PFSGeoTripleProd( &local_pt, &plane->n, &plane->v ) ;
    spar->v =   PFSGeoTripleProd( &local_pt, &plane->n, &plane->u ) ;

    PFSGeoEvalPlane( spar, (PFSGeoPlane *)plane, clst ) ;
}



PFSGeoStatus PFSGeoClstPtPoly( poly, pt, clst, spar ) 
    PFSGeoPoly  *poly ;
    PFSGeoPoint *pt, *clst ;
    PFSGeoSurfPar  *spar ;
{
    /*  first get the closest point on the plane which 
        contains the polygon                                    */

    PFSGeoClstPtNormPlane( &(poly->nplane), pt, clst, spar ) ;

    /*  then check to see if closest point is inside polygon 
        or on the boundary of polygon                           */

    if( PFSGeoIsInsidePoly( poly, spar ) == PFSGeoTrue ) {
        return( PFSGeoValid ) ;
    }
    else {
        if( PFSGeoIsOnPolyBdry( poly, pt ) == PFSGeoTrue ) {
            return( PFSGeoValid ) ;
        }
        else {
            return( PFSGeoInvalid ) ;
        }
    }
}



PFSGeoPredicate PFSGeoIsOnPlane( pt, pla )

PFSGeoPoint     *pt ;
PFSGeoNormPlane *pla ;

{
PFSGeoLine              line ;          /* line from "pt" to origin of "pla"        */
real            proj ;          /* projection of above line on plane normal */
real            toler ;         /* tolerance for closeness of point on plane*/
real            line_len ;      /* length of line                           */

    /*  Generate a line connecting the given point to the  
        origin of the given normalized plane.                   */

    PFSGeoGenLine( pt, &pla->a, &line ) ;

    line_len = PFSGeoVecLen( &line.e ) ;

    /*  Get the projection of this line in the direction of the unit 
        normal of the given plane. (take the dot product of the two 
        vectors).  The division by line_len is to normalize the vector 
        line.e to unit size.                                    */

    if( fabs( line_len ) < PFSGeoTolerance ) return( PFSGeoTrue ) ;
    proj = fabs( PFSGeoDotProd( &pla->n, &line.e ) / line_len ) ;

    /*  If the projection is smaller than an arbitrary value,
        the point can be assumed to be on the plane.            */

    toler = 0.01 ;      /* 0.01 ~= cos(1.56 radians) == cos(89.4 degrees) */
    if( proj < toler ) {
        return( PFSGeoTrue ) ;
    }
    else {
        return( PFSGeoFalse ) ;
    }
}


/*      --------------------------------------------------------
**      Misc Routines:

        PFSGeoStatus  PFSGeoInvert2x2( a, b ) 
            real        a[2][2] ;
            real        b[2][2] ;

        a       input 2 x 2 array of floats.
        b       output 2 x 2 inverse of a.

        This function computes the inverse of a 2 x 2 array.
        If the matrix is singular a value of PFSGeoInvalid is
        returned.  Otherwise a value of PFSGeoValid is returned


        PFSGeoStatus  PFSGeoInvert3x3( a, b ) 
            real        a[3][3] ;
            real        b[3][3] ;

        a       input 3 x 3 array of real.
        b       output 3 x 3 inverse of a.

        This function computes the inverse of a 3 x 3 array.
        If the matrix is singular a value of PFSGeoInvalid is
        returned.  Otherwise a value of PFSGeoValid is returned


        PFSGeoStatus  PFSGeodInvert3x3( a, b ) 
            double      a[3][3] ;
            double      b[3][3] ;

        a       input 3 x 3 array of doubles.
        b       output 3 x 3 inverse of a.

        This function computes the inverse of a 3 x 3 array
        in double precision.
        If the matrix is singular a value of PFSGeoInvalid is
        returned.  Otherwise a value of PFSGeoValid is returned


        void    PFSGeoCentroid( num_pts, pts, cntrd )
            int         *num_pts ;
            PFSGeoPoint pts[*num_pts] ;
            PFSGeoPoint *cntrd ;

        num_pts - number of points input
        pts     - coordinates of the points
        cntrd   - returned value of the centroid of the given list of points

        This function calculates the centroid of the given list of points
        such that cntrd->k =  sum{ pts[j]->k } / num_pts.


        PFSGeoStatus PFSGeoPerpLine( pl1, pl2, pl3, u, v, w ) 
            PFSGeoLine  *pl1, *pl2, *pl3 ; 
            PFSGeoPointPar *u, *v, *w ;

        pl1     first input parametric line.
        pl2     second input parametric line.
        pl3     output parametric line.
        u       output parametric value along pl1
        v       output parametric value along pl2
        w       output parametric value along pl3

        Given the parametric description of two input lines
        this function computes the parametric description of the
        line which is perpendicular to the two given lines.
        It also returns the parametric values for the intersection
        points along the three lines.  The w paramenter is the 
        parametric value of the intersection point of pl3 and pl2
        along pl3.  The parametric value of the intersection point
        of pl3 and pl1 measured along pl3 is always 0.0.


        void    PFSGeoPtNormToParPlan( pt, normal, plane ) 
            PFSGeoPoint  *pt, *normal ;
            PFSGeoNormPlane *plane ;

        pt          point on plane
        normal      normal to plane
        plane       normalized plane parametric description

        Given a point on a plane and the plane normal direction, 
        this function finds a parametric representation of the plane.
        The representation is given as a PFSGeoNormPlane structure, which 
        means that the two plane local directions are unit perpendicular 
        vectors and the unit normal vector is also contained in the 
        descriptor.
        The origin of the plane local system is the given point, and the 
        two unit directions on the plane are arbitrarely chosen.

        void    PFSGeoPlanToNormPlan( plane, normplane )
            PFSGeoPlane     *plane ;
            PFSGeoNormPlane    *normplane ;
            
        plane       PFSGeoPlane description of plane             (in)
        normplane   retrieved PFSGeoNormPlane description of plane (out)

        converts plane description from PFSGeoPlane to PFSGeoNormPlane. 
        The vector normplane->u is plane->d normalized and normplane->v
        is the normalized difference between plane.e and its projection
        onto the direction of normplane->u.  


        PFSGeoStatus   PFSGeo3PointsToNormPlane( in0, in1, in2, plane )
            PFSGeoPoint    *in0 ;
            PFSGeoPoint    *in1 ;
            PFSGeoPoint    *in2 ;
            PFSGeoNormPlane *plane ;

        in0     input point0
        in1     input point1
        in2     input point2
        plane   output normalized plane

        given 3 points, if they are not colinear, determine a normalized 
        description for the plane containing them and return a PFSGeoValid 
        status, othewise return PFSGeoInvalid. 

        void    PFSGeoPlanEqnToParPlan( plan_eqn, plane ) 
            PFSGeoPlanEqn        *plan_eqn ;
            PFSGeoNormPlane *plane ;

        plan_eqn    plane equation
        plane       normalized plane parametric description

        Given a plane equation in the form:
            a * x  +  b * y  +  c * z  +  d  = 0
        this function finds a parametric representation of the plane.
        The representation is given as a PFSGeoNormPlane structure, which 
        means that the two plane local directions are unit perpendicular 
        vectors and the unit normal vector is also contained in the 
        descriptor.
        The origin of the plane local system and the two unit directions 
        on the plane are arbitrarely chosen.


        PFSGeoPtTest   PFSGeoSideofPlan( plan_eqn, pt )
            PFSGeoPlanEqn       *plan_eqn ;
            PFSGeoPoint *pt ;

        Given a plane equation of the form:  a*x + b*y + c*z + d = 0
        (which was created via call to PFSGeoGenPlanEqn), whose top side
        is taken to be that side in which the normal vector points;
        this function will return one of three values.
                PFSGeoAbove -- given pt. is above the plane
                PFSGeoBelow -- given pt. is below the plane
                PFSGeoOn         -- given pt. is on (within PFSGeoTolerance) the plane


        void    PFSGeoSurfInterp( num_pts, map_func, pts_val, interp_val ) 
            int         *num_pts ;
            real        map_func[*num_pts] ;
            PFSGeoSurfPar       pts_val[*num_pts] ;
            PFSGeoSurfPar       *interp_val ;

        num_pts     number of points which describe the mapping
        map_func[]  mapping function evaluated at target point
        pts_val[]   known surface values at interpolation points
        interp_val  returned surface values at target point

        This function interpolates a set of known surface values.
        The mapping function is given evaluated at the target point.
        Basically this function performs the dot product of the 
        given mapping function with the known interpolation point surface 
        values, which results in the target interpolated values.


        void    PFSGeoSurfJacob( num_pts, map_deriv_s, map_deriv_t, 
                                        pts_val, jac ) 
            int         *num_pts ;
            real        map_deriv_s[*num_pts] ;
            real        map_deriv_t[*num_pts] ;
            PFSGeoSurfPar       pts_val[*num_pts] ;
            real        jac[2][2] ;

        num_pts         number of points which describe the mapping
        map_deriv_s[]   mapping derivative wrt s evaluated at target point
        map_deriv_t[]   mapping derivative wrt t evaluated at target point
        pts_val[]       known surface values at interpolation points
        jac[2][2]       returned mapping jacobian evaluated at target point

        This function computes the jacobian matrix of a surface mapping at 
        a target point.
        The partial derivatives of the mapping function are given evaluated at 
        a the target point.
        Basically this function performs the dot product of the given mapping 
        function derivatives with the known interpolation point surface 
        values, which results in jacobian matrix.


        real    PFSGeoAngVecAboutLine( l, v0, v1 ) 
            PFSGeoLine  *l ;
            PFSGeoPoint *v0, *v1 ;

        l               given line
        v0, v1          reference and angle vectors normal to line

        This function determines the angle about a given line between two 
        planes that meet at this line and contain the two given vectors
        (presumed normal to line).
        The angle is measured in clockwise order going from the plane passing 
        at the second (angle) vector to the first (reference) plane.
        Clockwise is given by the right hand rule with the thumb 
        pointing in the direction of the line.


        real    PFSGeoAngPtAboutLine( l, p0, p1 ) 
            PFSGeoLine  *l ;
            PFSGeoPoint *p0, *p1 ;

        l               given line
        p0, p1          reference and angle points

        This function determines the angle about a given line between two 
        planes that meet at this line and pass at two given points 
        (presumed not on the line).
        The angle is measured in clockwise order going from the plane passing 
        at the second (angle) point to the first (reference) plane.
        Clockwise is given by the right hand rule with the thumb 
        pointing in the direction of the line.


        real    PFSGeoAngPtAboutPt( pivot, p0, p1 ) 
            PFSGeoPoint *pivot ;
            PFSGeoPoint *p0, *p1 ;

        pivot           pivot point
        p0, p1          reference and angle points

        This function determines the angle about a given pivot point 
        between two given points.
        The angle is measured in clockwise order going from the second 
        (angle) point to the first (reference) point.


        PFSGeoStatus PFSGeoProjVecPlane( vec, plane, proj )
            PFSGeoPoint *vec ; 
            PFSGeoPlane *plane ;
            PFSGeoPoint *proj ; 

        vec     vector direction
        plane   given plane
        proj    projection of vector direction onto plane

        given a vector direction and a plane, this function finds the 
        vector direction which corresponds to the projection of the 
        vector onto the plane.
        If the given vector is normal to the plane, a PFSGeoInvalid 
        status is returned.  Otherwise a PFSGeoValid status is returned.


        real PFSGeoDistPtNormPlane( plane, pt )
            PFSGeoNormPlane    *plane ;
            PFSGeoPoint     *pt ;

        plane   input normalized parametric description of a plane.
        pt      input cartesian point.

        returns the distance from the point to the plane


        void PFSGeoDyadProd( vec0, vec1, matr )

            PFSGeoPoint *vec0, *vec1 ;
            real        matr[3][3] ;

        vec0, vec1  input vectors
        matr        output matrix, dyadic product  matr = v1 @ v2

        performs tensorial, or dyadic, product of two vectors


        void PFSGeoProdMatrVec( matr, vec0, vec1 ) 

        real        matr[3][3] ;
        PFSGeoPoint    *vec0 ;
        PFSGeoPoint    *vec1 ;
        
        matr    input matrix
        vec0    input vector
        vec1    output vector   v1 = matr x v0

*/

PFSGeoStatus  PFSGeoInvert2x2( a, b ) 
    real        a[2][2] ;       /* matrix to invert     */
    real        b[2][2] ;       /* inverse matrix       */
{
    real        det ;       /* determinant of given matrix  */
    int         i,j ;       /* counters                     */

/*  clear inverse matrix                        */

    for( i = 0; i < 2; i++) 
        for( j = 0; j < 2; j++) b[i][j] = 0.0 ;

/*  compute matrix determinant                  */

    det = a[0][0] * a[1][1] - a[0][1] * a[1][0] ;

    if (det == 0.0) 
        return( PFSGeoInvalid ) ;
    else {
        b[0][0] =  a[1][1] / det ;
        b[0][1] = -a[0][1] / det ;
        b[1][0] = -a[1][0] / det ;
        b[1][1] =  a[0][0] / det ;
        return( PFSGeoValid );
    }
}


PFSGeoStatus  PFSGeoInvert3x3( a, b ) 
    real        a[3][3] ;       /* matrix to invert     */
    real        b[3][3] ;       /* inverse matrix       */
{
    real        det ;       /* determinant of given matrix  */
    int         i,j ;       /* counters                     */

/*  clear inverse matrix                        */

    for( i = 0; i < 3; i++) 
        for( j = 0; j < 3; j++) b[i][j] = 0.0 ;

/*  compute matrix determinant                  */

    det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) - 
          a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + 
          a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]) ;

    if (det == 0.0) 
        return( PFSGeoInvalid ) ;
    else {
        b[0][0] =  (a[1][1] * a[2][2] - a[1][2] * a[2][1]) / det ;
        b[0][1] = -(a[0][1] * a[2][2] - a[0][2] * a[2][1]) / det ;
        b[0][2] =  (a[0][1] * a[1][2] - a[0][2] * a[1][1]) / det ;
        b[1][0] = -(a[1][0] * a[2][2] - a[1][2] * a[2][0]) / det ;
        b[1][1] =  (a[0][0] * a[2][2] - a[0][2] * a[2][0]) / det ;
        b[1][2] = -(a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det ;
        b[2][0] =  (a[1][0] * a[2][1] - a[1][1] * a[2][0]) / det ;
        b[2][1] = -(a[0][0] * a[2][1] - a[0][1] * a[2][0]) / det ;
        b[2][2] =  (a[0][0] * a[1][1] - a[0][1] * a[1][0]) / det ;
        return( PFSGeoValid );
    }
}


PFSGeoStatus  PFSGeodInvert3x3( a, b ) 
    double      a[3][3] ;       /* matrix to invert     */
    double      b[3][3] ;       /* inverse matrix       */
{
    double      det ;       /* determinant of given matrix  */
    int         i,j ;       /* counters                     */

/*  clear inverse matrix                        */

    for( i = 0; i < 3; i++) 
        for( j = 0; j < 3; j++) b[i][j] = 0.0 ;

/*  compute matrix determinant                  */

    det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) - 
          a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + 
          a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]) ;

    if (det == 0.0) 
        return( PFSGeoInvalid ) ;
    else {
        b[0][0] =  (a[1][1] * a[2][2] - a[1][2] * a[2][1]) / det ;
        b[0][1] = -(a[0][1] * a[2][2] - a[0][2] * a[2][1]) / det ;
        b[0][2] =  (a[0][1] * a[1][2] - a[0][2] * a[1][1]) / det ;
        b[1][0] = -(a[1][0] * a[2][2] - a[1][2] * a[2][0]) / det ;
        b[1][1] =  (a[0][0] * a[2][2] - a[0][2] * a[2][0]) / det ;
        b[1][2] = -(a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det ;
        b[2][0] =  (a[1][0] * a[2][1] - a[1][1] * a[2][0]) / det ;
        b[2][1] = -(a[0][0] * a[2][1] - a[0][1] * a[2][0]) / det ;
        b[2][2] =  (a[0][0] * a[1][1] - a[0][1] * a[1][0]) / det ;
        return( PFSGeoValid );
    }
}


void    PFSGeoCentroid( num_pts, pts, cntrd )
            int         *num_pts ;   /* number of points            */
            PFSGeoPoint pts[] ;     /* list of points       */
            PFSGeoPoint *cntrd ;    /* returned centroid    */

{
    PFSGeoPoint sum ;
    int         i ;

    sum.x = sum.y = sum.z = 0.0 ;
    for( i=0; i<(*num_pts); i++ ) {

        sum.x += pts[i].x ;
        sum.y += pts[i].y ;
        sum.z += pts[i].z ;
    }

    cntrd->x = sum.x / *num_pts ;
    cntrd->y = sum.y / *num_pts ;
    cntrd->z = sum.z / *num_pts ;

return ;
}


PFSGeoStatus PFSGeoPerpLine( pl1, pl2, pl3, u, v, w ) 
    PFSGeoLine  *pl1, *pl2, *pl3 ; 
    PFSGeoPointPar *u, *v, *w ;
{
    real        coef[3][3], cinv[3][3];
    PFSGeoPoint dvec ;

    /* comput the direction of the perpendicular line   */

    PFSGeoCrossProd( &pl1->e, &pl2->e, &pl3->e ) ;

    /* build and invert the coef matrix */

    coef[0][0] = pl1->e.x ; coef[0][1] = pl2->e.x ; coef[0][2] = pl3->e.x ;
    coef[1][0] = pl1->e.y ; coef[1][1] = pl2->e.y ; coef[1][2] = pl3->e.y ;
    coef[2][0] = pl1->e.z ; coef[2][1] = pl2->e.z ; coef[2][2] = pl3->e.z ;

    if ( PFSGeoInvert3x3( coef, cinv ) != PFSGeoValid ) 
        return( PFSGeoInvalid ) ;
    else {
        /* find the scale factors */

        dvec.x = pl2->d.x - pl1->d.x ;
        dvec.y = pl2->d.y - pl1->d.y ;
        dvec.z = pl2->d.z - pl1->d.z ;

        *u = cinv[0][0]*dvec.x + cinv[0][1]*dvec.y + cinv[0][2]*dvec.z ;
        *v = cinv[1][0]*dvec.x + cinv[1][1]*dvec.y + cinv[1][2]*dvec.z ;
        *w = cinv[2][0]*dvec.x + cinv[2][1]*dvec.y + cinv[2][2]*dvec.z ;
        *v = -*v ;

        PFSGeoEvalLine( u, pl1, &pl3->d ) ;
        return(PFSGeoValid) ;
    }
}


void    PFSGeoPtNormToParPlan( pt, normal, plane ) 
    PFSGeoPoint  *pt, *normal ; /* point on plane and normal    */
    PFSGeoNormPlane *plane ;    /* plane parametric description */

{
    PFSGeoPoint    x_dir ;                      /* x direction              */
    PFSGeoPoint    y_dir ;                      /* y direction              */
    PFSGeoPoint    z_dir ;                      /* z direction              */
    real        size ;                  /* size of a vector         */
    real        tolerance ;             /* tolerance for checking   */

    x_dir.x = 1.0 ;     x_dir.y = 0.0 ;     x_dir.z = 0.0 ;
    y_dir.x = 0.0 ;     y_dir.y = 1.0 ;     y_dir.z = 0.0 ;
    z_dir.x = 0.0 ;     z_dir.y = 0.0 ;     z_dir.z = 1.0 ;

    /*  normalize given normal                          */

    size = PFSGeoVecLen( normal ) ;

    plane->n.x = normal->x / size ;
    plane->n.y = normal->y / size ;
    plane->n.z = normal->z / size ;

    /*  choose the given point as an origin for the local system on plane */

    plane->a = *pt ;

    /*  arbitrarely choose two unit directions for the local system on plane */

    tolerance = PFSGeoTolerance ;
    PFSGeoCrossProd( &plane->n, &x_dir, &plane->u ) ;
    if (PFSGeoNullVec( &plane->u, &tolerance ) == PFSGeoTrue) {
        PFSGeoCrossProd( &plane->n, &y_dir, &plane->u ) ;
        if (PFSGeoNullVec( &plane->u, &tolerance ) == PFSGeoTrue) {
            PFSGeoCrossProd( &plane->n, &z_dir, &plane->u ) ;
        }
    }
    size = PFSGeoVecLen( &plane->u ) ;
    plane->u.x /= size ;    plane->u.y /= size ;    plane->u.z /= size ;
    PFSGeoCrossProd( &plane->n, &plane->u, &plane->v ) ;

return ;
}

void    PFSGeoPlanToNormPlan( plane, normplane )
    PFSGeoPlane     *plane ;
    PFSGeoNormPlane    *normplane ;


{
    normplane->a = plane->a ;
    PFSGeoVecNormalize( &plane->b, &normplane->u ) ;
    PFSGeoCrossProd( &normplane->u, &plane->c, &normplane->n ) ;
    PFSGeoVecNormalize( &normplane->n, &normplane->n) ;
    PFSGeoCrossProd( &normplane->n, &normplane->u, &normplane->v ) ;
}

PFSGeoStatus  PFSGeo3PointsToNormPlane( in0, in1, in2, plane )
    PFSGeoPoint    *in0 ;
    PFSGeoPoint    *in1 ;
    PFSGeoPoint    *in2 ;
    PFSGeoNormPlane *plane ;

{
    plane->a = *in0 ;
    PFSGeoDiffVec( in1, in0, &plane->u ) ;
    PFSGeoDiffVec( in2, in0, &plane->v ) ;
    
    PFSGeoCrossProd( &plane->u, &plane->v, &plane->n ) ;

    if (PFSGeoNullVec( &plane->n, &PFSGeoTolerance ) ) return(PFSGeoInvalid) ;
    
    PFSGeoPlanToNormPlan( (PFSGeoPlane *)plane, plane ) ;    

    return(PFSGeoValid) ;
}
        


void    PFSGeoPlanEqnToParPlan( plan_eqn, plane ) 
    PFSGeoPlanEqn        *plan_eqn ;    /* plane equation               */
    PFSGeoNormPlane *plane ;    /* plane parametric description */

{
    PFSGeoPoint    x_dir ;                      /* x direction              */
    PFSGeoPoint    y_dir ;                      /* y direction              */
    PFSGeoPoint    z_dir ;                      /* z direction              */
    real        tolerance ;             /* tolerance for checking   */
    real        size ;                  /* size of a vector         */

    x_dir.x = 1.0 ;     x_dir.y = 0.0 ;     x_dir.z = 0.0 ;
    y_dir.x = 0.0 ;     y_dir.y = 1.0 ;     y_dir.z = 0.0 ;
    z_dir.x = 0.0 ;     z_dir.y = 0.0 ;     z_dir.z = 1.0 ;

    /*  normal to plane is obtained directly from its equation  */

    size = sqrt( plan_eqn->a * plan_eqn->a + plan_eqn->b * plan_eqn->b + 
                 plan_eqn->c * plan_eqn->c ) ;

    plane->n.x = plan_eqn->a /size ;
    plane->n.y = plan_eqn->b /size ;
    plane->n.z = plan_eqn->c /size ;

    /*  arbitrarely choose an origin for the local system on plane */

    if (fabs(plan_eqn->a) > PFSGeoTolerance) {
        plane->a.x = - plan_eqn->d / plan_eqn->a ;
        plane->a.y = 0.0 ;
        plane->a.z = 0.0 ;
    }
    else if (fabs(plan_eqn->b) > PFSGeoTolerance) {
        plane->a.x = 0.0 ;
        plane->a.y = - plan_eqn->d / plan_eqn->b ;
        plane->a.z = 0.0 ;
    }
    else {
        plane->a.x = 0.0 ;
        plane->a.y = 0.0 ;
        plane->a.z = - plan_eqn->d / plan_eqn->c ;
    }

    /*  arbitrarely choose two unit directions for the local system on plane */

    if (fabs( plan_eqn->d ) == 0.0) tolerance = PFSGeoTolerance ;
    else                        tolerance = PFSGeoTolerance * fabs( plan_eqn->d ) ;
    PFSGeoCrossProd( &plane->n, &x_dir, &plane->u ) ;
    if (PFSGeoNullVec( &plane->u, &tolerance ) == PFSGeoTrue) {
        PFSGeoCrossProd( &plane->n, &y_dir, &plane->u ) ;
        if (PFSGeoNullVec( &plane->u, &tolerance ) == PFSGeoTrue) {
            PFSGeoCrossProd( &plane->n, &z_dir, &plane->u ) ;
        }
    }
    size = PFSGeoVecLen( &plane->u ) ;
    plane->u.x /= size ;    plane->u.y /= size ;    plane->u.z /= size ;
    PFSGeoCrossProd( &plane->n, &plane->u, &plane->v ) ;

return ;
}


PFSGeoPtTest  PFSGeoSideofPlan( plan_eqn, pt )
    PFSGeoPlanEqn       *plan_eqn ;
    PFSGeoPoint *pt ;

{
    real  result ;

    result =  (     plan_eqn->a * pt->x
                 +  plan_eqn->b * pt->y
                 +  plan_eqn->c * pt->z
                 +  plan_eqn->d          ) ;

    if( result > PFSGeoTolerance )
        return( PFSGeoAbove ) ;
    else if( result < -PFSGeoTolerance )
        return( PFSGeoBelow ) ;
    else
        return( PFSGeoOn ) ;
}


void    PFSGeoSurfInterp( num_pts, map_func, pts_val, interp_val ) 
    int         *num_pts ;      /* number of points which describe the mapping*/
    real        map_func[] ;    /* mapping function evaluated at target point */
    PFSGeoSurfPar       pts_val[] ;     /* known surface values at interpolation pts  */
    PFSGeoSurfPar       *interp_val ;   /* returned surface values at target point    */

{
    int     i;

    interp_val->u = 0.0 ;
    interp_val->v = 0.0 ;

    for( i = 0; i < *num_pts; i++ ) {
        interp_val->u += map_func[i] * pts_val[i].u ;
        interp_val->v += map_func[i] * pts_val[i].v ;
    }

return ;
}


void    PFSGeoSurfJacob( num_pts, map_deriv_s, map_deriv_t, pts_val, jac ) 
    int         *num_pts ;      /* number of points which describe the mapping*/
    real        map_deriv_s[] ; /* mapping deriv. wrt s evaluated at target pt*/
    real        map_deriv_t[] ; /* mapping deriv. wrt t evaluated at target pt*/
    PFSGeoSurfPar       pts_val[] ;     /* known surface values at interpolation pts  */
    real        jac[2][2] ;     /* mapping jacobian evaluated at target point */

{
    int     i;

    jac[0][0] = 0.0 ;       jac[0][1] = 0.0 ;
    jac[1][0] = 0.0 ;       jac[1][1] = 0.0 ;

    for( i = 0; i < *num_pts; i++ ) {
        jac[0][0] += map_deriv_s[i] * pts_val[i].u ;
        jac[0][1] += map_deriv_s[i] * pts_val[i].v ;
        jac[1][0] += map_deriv_t[i] * pts_val[i].u ;
        jac[1][1] += map_deriv_t[i] * pts_val[i].v ;
    }

return ;
}


real    PFSGeoAngVecAboutLine( l, v0, v1 ) 
    PFSGeoLine  *l ;            /* given line                                */
    PFSGeoPoint *v0, *v1 ;      /* reference and angle vectors normal to line*/
{
    real        cosang ;            /* cosine of target angle           */
    PFSGeoPoint cross ;             /* cross product of normal vectors  */
    real        pi = fabs( acos(-1.0) ) ;
    real        len, toler ;

    /* normalize the given vectors                          */

    len = PFSGeoVecLen ( v0 ) ;
    if( len == 0.0 ) return( 0.0 ) ;
    v0->x /= len ;   v0->y /= len ;   v0->z /= len ;
    len = PFSGeoVecLen ( v1 ) ;
    if( len == 0.0 ) return( 0.0 ) ;
    v1->x /= len ;   v1->y /= len ;   v1->z /= len ;

    /* cosine of angle between two vectors is obtained from dot product */

    cosang = PFSGeoDotProd( v1, v0 ) ;

    /* get the cross product of these vectors.  if the cross product vector 
       has the same direction as the given line, then desired angle is 
       in the range [0 .. pi], otherwise it is in the range [pi .. 2pi] */

    PFSGeoCrossProd( v1, v0, &cross ) ;

    toler = PFSGeoTolerance ;
    if( PFSGeoNullVec( &cross, &toler ) == PFSGeoFalse ) {
        if( PFSGeoDotProd( &l->e, &cross ) > 0.0 ) {
            return( acos( cosang ) ) ;
        }
        else {
            return( 2.0 * pi - acos( cosang ) ) ;
        }
    }
    else {
        if( cosang > 0.0 ) {
            return( 0.0 ) ;
        }
        else {
            return( pi ) ;
        }
    }
}


real    PFSGeoAngPtAboutLine( l, p0, p1 ) 
    PFSGeoLine  *l ;            /* given line                   */
    PFSGeoPoint *p0, *p1 ;      /* reference and angle points   */
{
    PFSGeoPoint clstp0, clstp1 ;    /* points on line which are closest 
                                       to given points respectively     */
    PFSGeoPoint v0, v1 ;            /* vectors normal to given line 
                                       passing on p0 and p1 respectively*/
    PFSGeoPointPar dum_par ;

    /* get the two points on line which are closest to given points */

    PFSGeoClstPtLine  ( l, p0, &clstp0, &dum_par ) ;
    PFSGeoClstPtLine  ( l, p1, &clstp1, &dum_par ) ;

    /* generate two vectors normal to line passing at the given points  */

    v0.x = p0->x - clstp0.x ;
    v0.y = p0->y - clstp0.y ;
    v0.z = p0->z - clstp0.z ;
    v1.x = p1->x - clstp1.x ;
    v1.y = p1->y - clstp1.y ;
    v1.z = p1->z - clstp1.z ;

    return( PFSGeoAngVecAboutLine( l, &v0, &v1 ) ) ;
}


real    PFSGeoAngPtAboutPt( pivot, p0, p1 ) 
    PFSGeoPoint *pivot ;        /* pivot point                  */
    PFSGeoPoint *p0, *p1 ;      /* reference and angle points   */
{
    PFSGeoLine  l ;              /* line normal to plane formed by 3 points*/
    PFSGeoPoint v0, v1 ;         /* vectors from pivot to given points  */
    real        pi = fabs( acos(-1.0) ) ;
    real        toler ;

    /* first the vectors from the pivot to the two other points.
       get the cross product of these vectors.                              */

    v0.x = p0->x - pivot->x; v0.y = p0->y - pivot->y; v0.z = p0->z - pivot->z;
    v1.x = p1->x - pivot->x; v1.y = p1->y - pivot->y; v1.z = p1->z - pivot->z;

    /* get a line which is normal to the three points.
       if the cross product vector is null, the angle is either 0 or pi.    */

    l.d = *pivot ;
    PFSGeoCrossProd( &v1, &v0, &l.e ) ;

    toler = PFSGeoTolerance ;
    if( PFSGeoNullVec( &l.e, &toler ) == PFSGeoTrue ) {
        if( PFSGeoDotProd( &v0, &v1 ) > 0.0 ) {
            return( 0.0 ) ;
        }
        else {
            return( pi ) ;
        }
    }
    else {

    /* return angle between two vectors             */

        return( PFSGeoAngVecAboutLine( &l, &v0, &v1 ) ) ;
    }
}


PFSGeoStatus PFSGeoProjVecPlane( vec, plane, proj )
    PFSGeoPoint *vec ; 
    PFSGeoPlane *plane ;
    PFSGeoPoint *proj ; 
{
    real        vec_dot_b, vec_dot_c ;

    vec_dot_b = PFSGeoDotProd( vec, &plane->b ) ;
    vec_dot_c = PFSGeoDotProd( vec, &plane->c ) ;

    proj->x = vec_dot_b * plane->b.x + vec_dot_c * plane->c.x ;
    proj->y = vec_dot_b * plane->b.y + vec_dot_c * plane->c.y ;
    proj->z = vec_dot_b * plane->b.z + vec_dot_c * plane->c.z ;

    if( (fabs(vec_dot_b) < PFSGeoTolerance) && (fabs(vec_dot_c) < PFSGeoTolerance) ) {
        return( PFSGeoInvalid ) ;
    }
    else {
        return( PFSGeoValid ) ;
    }
}

real PFSGeoDistPtNormPlane( plane, pt )
    PFSGeoNormPlane    *plane ;
    PFSGeoPoint     *pt ;

{
    PFSGeoPoint    clst ;
    PFSGeoSurfPar  spar ;

    PFSGeoClstPtNormPlane( plane, pt, &clst, &spar ) ;

    return( PFSGeoDist( pt, &clst ) ) ;
}

void PFSGeoDyadProd( vec0, vec1, matr )

    PFSGeoPoint *vec0, *vec1 ;
    real        matr[3][3] ;
{

    matr[0][0] = vec0->x * vec1->x ;
    matr[0][1] = vec0->x * vec1->y ;
    matr[0][2] = vec0->x * vec1->z ;
    matr[1][0] = vec0->y * vec1->x ;
    matr[1][1] = vec0->y * vec1->y ;
    matr[1][2] = vec0->y * vec1->z ;
    matr[2][0] = vec0->z * vec1->x ;
    matr[2][1] = vec0->z * vec1->y ;
    matr[2][2] = vec0->z * vec1->z ;

    return ;
}

void PFSGeoProdMatrVec( matr, vec0, vec1 ) 
    real        matr[3][3] ;
    PFSGeoPoint *vec0 ;
    PFSGeoPoint *vec1 ;

{
    vec1->x = matr[0][0] * vec0->x + matr[0][1] * vec0->y + 
                                     matr[0][2] * vec0->z ;
    vec1->y = matr[1][0] * vec0->x + matr[1][1] * vec0->y + 
                                     matr[1][2] * vec0->z ;
    vec1->z = matr[2][0] * vec0->x + matr[2][1] * vec0->y + 
                                     matr[2][2] * vec0->z ;
    return ;
}

---