/*****************************************************************************
*   "Irit" - the 3d polygonal solid modeller.				     *
*									     *
* Written by:  Gershon Elber				Ver 0.2, Mar. 1990   *
******************************************************************************
*   Routines to generate transformation matrices for Translation, Rotation   *
* and Scaling for 3D universe. 						     *
*   Routines to manipulate 3D vectors.					     *
*   And some computational geometry routines.				     *
*****************************************************************************/

#include <math.h>
#include <stdio.h>
#include "program.h"
#include "allocate.h"
#include "convex.h"
#include "geomat3d.h"
#include "objects.h"
#include "primitiv.h"
#include "windows.h"
#include "graphgen.h"

/* #define DEBUG	    Print information on entry and exit of routines. */

static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes);

/*****************************************************************************
* Routine to generate a 4*4 unit matrix:                                     *
*****************************************************************************/
void MatGenUnitMat(MatrixType Mat)
{
    int i, j;

    for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
	if (i == j)
	    Mat[i][j] = 1.0;
	else
	    Mat[i][j] = 0.0;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
*****************************************************************************/
void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
     Mat[3][0] = Tx;
     Mat[3][1] = Ty;
     Mat[3][2] = Tz;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
*****************************************************************************/
void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
     Mat[0][0] = Sx;
     Mat[1][1] = Sy;
     Mat[2][2] = Sz;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotX1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = (RealType) cos((double) Teta);
    STeta = (RealType) sin((double) Teta);
    MatGenMatRotX(CTeta, STeta, Mat);
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
*****************************************************************************/
void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
     Mat[1][1] = CosTeta;
     Mat[1][2] = SinTeta;
     Mat[2][1] = -SinTeta;
     Mat[2][2] = CosTeta;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotY1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = (RealType) cos((double) Teta);
    STeta = (RealType) sin((double) Teta);
    MatGenMatRotY(CTeta, STeta, Mat);
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
*****************************************************************************/
void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
     Mat[0][0] = CosTeta;
     Mat[0][2] = -SinTeta;
     Mat[2][0] = SinTeta;
     Mat[2][2] = CosTeta;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = (RealType) cos((double) Teta);
    STeta = (RealType) sin((double) Teta);
    MatGenMatRotZ(CTeta, STeta, Mat);
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
*****************************************************************************/
void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     MatGenUnitMat(Mat);                             /* Make it unit matrix, */
     Mat[0][0] = CosTeta;
     Mat[0][1] = SinTeta;
     Mat[1][0] = -SinTeta;
     Mat[1][1] = CosTeta;
}

/*****************************************************************************
* Routine to multiply 2 4by4 matrices:                                       *
* Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
*****************************************************************************/
void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j, k;
    MatrixType MatResTemp;

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++) {
	   MatResTemp[i][j] = 0;
	   for (k = 0; k < 4; k++)
	       MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
	}
    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    MatRes[i][j] = MatResTemp[i][j];
}

/*****************************************************************************
* Routine to add 2 4by4 matrices:					     *
* Note MatRes might be one of Mat1 or Mat2.				     *
*****************************************************************************/
void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
            MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
}

/*****************************************************************************
* Routine to subtract 2 4by4 matrices:					     *
* Note MatRes might be one of Mat1 or Mat2.				     *
*****************************************************************************/
void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
	    MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
}

/*****************************************************************************
* Routine to scale a 4by4 matrix by constant:				     *
* Note MatRes might be Mat.						     *
*****************************************************************************/
void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
	    MatRes[i][j] = Mat[i][j] * (*Scale);
}

/*****************************************************************************
* Routine to multiply Vector by 4by4 matrix:                                 *
* The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
* Note VRes might be Vec as it is only updated in the end.                   *
*****************************************************************************/
void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
{
    int i, j;
    RealType
	CalcW = Mat[3][3];
    VectorType VTemp;

    for (i = 0; i < 3; i++) {
	VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
	for (j = 0; j < 3; j++) VTemp[i] += Vec[j] * Mat[j][i];
    }

    for (i = 0; i < 3; i++) CalcW += Vec[i] * Mat[i][3];
    if (CalcW == 0) CalcW = 1/INFINITY;

    for (i = 0; i < 3; i++) VRes[i] = VTemp[i]/CalcW;
}

/*****************************************************************************
* Procedure to calculate the INVERSE of a given matrix M which is not        *
* modified. The matrix is assumed to be 4 by 4 (transformation matrix).      *
* Return TRUE if inverted matrix (InvM) do exists.			     *
*****************************************************************************/
int MatInverseMatrix(MatrixType M, MatrixType InvM)
{
    MatrixType A;
    int i, j, k;
    RealType V;

    MAT_COPY(A, M);			/* Prepare temporary copy of M in A. */
    MatGenUnitMat(InvM);			     /* Make it unit matrix. */

    for (i = 0; i < 4; i++) {
	V = A[i][i];				      /* Find the new pivot. */
	k = i;
	for (j = i + 1; j < 4; j++)
	    if (ABS(A[j][i]) > ABS(V)) {
	        /* Find maximum on col i, row i+1..n */
	        V = A[j][i];
	        k = j;
	    }
	j = k;

	if (i != j)
	    for (k = 0; k < 4; k++) {
		SWAP(RealType, A[i][k], A[j][k]);
		SWAP(RealType, InvM[i][k], InvM[j][k]);
            }

	for (j = i + 1; j < 4; j++) {	 /* Eliminate col i from row i+1..n. */
            V = A[j][i] / A[i][i];
	    for (k = 0; k < 4; k++) {
                A[j][k]    -= V * A[i][k];
                InvM[j][k] -= V * InvM[i][k];
	    }
	}
    }

    for (i = 3; i >= 0; i--) {			       /* Back Substitution. */
	if (A[i][i] == 0) return FALSE;				   /* Error. */

	for (j = 0; j < i; j++) {	 /* Eliminate col i from row 1..i-1. */
            V = A[j][i] / A[i][i];
	    for (k = 0; k < 4; k++) {
                /* A[j][k] -= V * A[i][k]; */
                InvM[j][k] -= V * InvM[i][k];
	    }
	}
    }

    for (i = 0; i < 4; i++)		    /* Normalize the inverse Matrix. */
	for (j = 0; j < 4; j++)
            InvM[i][j] /= A[i][i];

    return TRUE;
}

/*****************************************************************************
*  Routine to copy one vector to another:				     *
*****************************************************************************/
void VecCopy(VectorType Vdst, VectorType Vsrc)
{
    int i;

    for (i = 0; i < 3; i++)
	Vdst[i] = Vsrc[i];
}

/*****************************************************************************
*  Routine to normalize the vector length to a unit length:                  *
*****************************************************************************/
void VecNormalize(VectorType V)
{
    int i;
    RealType Len;

    Len = VecLength(V);
    if (Len > 0.0)
        for (i = 0; i < 3; i++) {
	    V[i] /= Len;
	    if (ABS(V[i]) < EPSILON) V[i] = 0.0;
	}
}

/*****************************************************************************
*  Routine to calculate the magnitude (length) of a given 3D vector:         *
*****************************************************************************/
RealType VecLength(VectorType V)
{
    return sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
}

/*****************************************************************************
*  Routine to calculate the cross product of two vectors:                    *
* Note Vres might be the same as V1 or V2 !                                  *
*****************************************************************************/
void VecCrossProd(VectorType Vres, VectorType V1, VectorType V2)
{
    VectorType Vtemp;

    Vtemp[0] = V1[1] * V2[2] - V2[1] * V1[2];
    Vtemp[1] = V1[2] * V2[0] - V2[2] * V1[0];
    Vtemp[2] = V1[0] * V2[1] - V2[0] * V1[1];

    VecCopy(Vres, Vtemp);
}

/*****************************************************************************
*  Routine to calculate the dot product of two vectors:                      *
*****************************************************************************/
RealType VecDotProd(VectorType V1, VectorType V2)
{
    return  V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2];
}

/*****************************************************************************
* Routine to generate rotation object around the X axis in Degree degrees:   *
*****************************************************************************/
ObjectStruct *GenMatObjectRotX(RealType *Degree)
{
    MatrixType Mat;

    MatGenMatRotX1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */

    return GenMatObject("", Mat, NULL);
}

/*****************************************************************************
* Routine to generate rotation object around the Y axis in Degree degrees:   *
*****************************************************************************/
ObjectStruct *GenMatObjectRotY(RealType *Degree)
{
    MatrixType Mat;

    MatGenMatRotY1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */

    return GenMatObject("", Mat, NULL);
}

/*****************************************************************************
* Routine to generate rotation object around the Z axis in Degree degrees:   *
*****************************************************************************/
ObjectStruct *GenMatObjectRotZ(RealType *Degree)
{
    MatrixType Mat;

    MatGenMatRotZ1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */

    return GenMatObject("", Mat, NULL);
}

/*****************************************************************************
* Routine to generate translation object according to XYZ vector Vec.	     *
*****************************************************************************/
ObjectStruct * GenMatObjectTrans(VectorType Vec)
{
    MatrixType Mat;

    /* Generate the transformation matrix */
    MatGenMatTrans(Vec[0], Vec[1], Vec[2], Mat);

    return GenMatObject("", Mat, NULL);
}

/*****************************************************************************
* Routine to scale an object according to XYZ scaling vector Vec.	     *
*****************************************************************************/
ObjectStruct * GenMatObjectScale(VectorType Vec)
{
    MatrixType Mat;

    /* Generate the transformation matrix */
    MatGenMatScale(Vec[0], Vec[1], Vec[2], Mat);

    return GenMatObject("", Mat, NULL);
}

/*****************************************************************************
* Routine to transform an object according to the transformation matrix.     *
*****************************************************************************/
ObjectStruct *TransformObject(ObjectStruct *PObj, MatrixType Mat)
{
    int i, j;
    ObjectStruct *NewPObj;
    CagdMatStruct CagdMat;

    if (IS_POLY_OBJ(PObj)) {
    	int IsPolygon = !IS_POLYLINE_OBJ(PObj);
    	VectorType Pin, PTemp;
    	PolygonStruct *Pl;
    	VertexStruct *V, *VFirst;

    	NewPObj = CopyObject(NULL, PObj, FALSE);
	Pl = NewPObj -> U.Pl.P;

	while (Pl != NULL) {
	    V = VFirst = Pl -> V;
	    PT_ADD(Pin, V -> Pt, Pl -> Plane);     /* Prepare point IN side. */
	    MatMultVecby4by4(Pin, Pin, Mat); /* Transformed relative to new. */

	    do {
		if (IsPolygon) PT_ADD(PTemp, V -> Pt, V -> Normal);

		MatMultVecby4by4(V -> Pt, V -> Pt, Mat); /* Update position. */

		if (IsPolygon) {
		    MatMultVecby4by4(PTemp, PTemp, Mat);   /* Update normal. */
		    PT_SUB(V -> Normal, PTemp, V -> Pt);
		    PT_NORMALIZE(V -> Normal);
		}

		V = V -> Pnext;
	    }
	    while (V != VFirst && V != NULL);	       /* VList is circular! */

	    if (IsPolygon)
		UpdatePolyPlane(Pl, Pin);/* Update plane eqn. using IN point.*/

	    Pl = Pl -> Pnext;
	}
    }
    else if (IS_SRF_OBJ(PObj)) {
    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (i = 0; i < 4; i++)
            for (j = 0; j < 4; j++)
		CagdMat.Mat[i][j] = Mat[i][j];

        CagdSrfMatTransform(NewPObj -> U.Srf.Srf, &CagdMat);
    }
    else if (IS_CRV_OBJ(PObj)) {
    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (i = 0; i < 4; i++)
            for (j = 0; j < 4; j++)
		CagdMat.Mat[i][j] = Mat[i][j];

        CagdCrvMatTransform(NewPObj -> U.Crv.Crv, &CagdMat);
    }
    else if (IS_OLST_OBJ(PObj)) {
	NewPObj = AllocObject("", OBJ_LIST_OBJ, NULL);

    	for (i = 0; PObj -> U.PObjList[i] != NULL && i < MAX_OBJ_LIST; i++) {
    	    NewPObj -> U.PObjList[i] = TransformObject(PObj -> U.PObjList[i],
						       Mat);
    	}
    	NewPObj -> U.PObjList[i] = NULL;
    }
    else {
	WndwInputWindowPutStr("None transformable object ignored.");
        NewPObj = CopyObject(NULL, PObj, FALSE);
    }

    return NewPObj;
}

/*****************************************************************************
*   Routine to calc the distance between two 3d points                       *
*****************************************************************************/
RealType CGDistPointPoint(PointType P1, PointType P2)
{
    RealType t;

#ifdef DEBUG
    printf("CGDistPointPoint entered\n");
#endif /* DEBUG */

    t = sqrt(SQR(P2[0]-P1[0]) + SQR(P2[1]-P1[1]) + SQR(P2[2]-P1[2]));

#ifdef DEBUG
    printf("CGDistPointPoint exit\n");
#endif /* DEBUG */

     return t;
}

/*****************************************************************************
*   Routine to create the plane from given 3 points. if two of the points    *
* are the same it returns FALSE, otherwise (succesfull) returns TRUE.	     *
*****************************************************************************/
int CGPlaneFrom3Points(PlaneType Plane, PointType Pt1, PointType Pt2,
							PointType Pt3)
{
    VectorType V1, V2;

#ifdef DEBUG
    printf("CGPlaneFrom3Points entered\n");
#endif /* DEBUG */

    if (PT_EQ(Pt1, Pt2) || PT_EQ(Pt2, Pt3) || PT_EQ(Pt1, Pt3)) return FALSE;

    PT_SUB(V1, Pt2, Pt1);
    PT_SUB(V2, Pt3, Pt2);
    VecCrossProd(Plane, V1, V2);
    PT_NORMALIZE(Plane);

    Plane[3] = -DOT_PROD(Plane, Pt1);

#ifdef DEBUG
    printf("CGPlaneFrom3Points exit\n");
#endif /* DEBUG */

    return TRUE;
}

/*****************************************************************************
*   Routine to calc the closest 3d point to a given 3d line. the line is     *
* given as a point on it (Pl) and vector (Vl).                               *
*****************************************************************************/
void CGPointFromPointLine(PointType Point, PointType Pl, PointType Vl,
							PointType ClosestPoint)
{
    int i;
    PointType V1, V2;
    RealType CosAlfa, VecMag;

#ifdef DEBUG
    printf("CGPointFromLinePlane entered\n");
#endif /* DEBUG */

    for (i = 0; i < 3; i++) {
        V1[i] = Point[i] - Pl[i];
        V2[i] = Vl[i];
    }
    VecMag = VecLength(V1);
    VecNormalize(V1); /* Normalized vector from Point to a point on line Pl. */
    VecNormalize(V2);                   /* Normalized line direction vector. */

    CosAlfa = VecDotProd(V1, V2); /* Find the angle between the two vectors. */

    /* Find P1 - the closest point to Point on the line: */
    for (i = 0; i < 3; i++)
	ClosestPoint[i] = Pl[i] + V2[i] * CosAlfa * VecMag;

#ifdef DEBUG
    printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG */
}

/*****************************************************************************
*   Routine to calc the distance between 3d point and 3d line. the line is   *
* given as a point on it (Pl) and vector (Vl).                               *
*****************************************************************************/
RealType CGDistPointLine(PointType Point, PointType Pl, PointType Vl)
{
    RealType t;
    PointType Ptemp;

#ifdef DEBUG
    printf("CGDistPointLine entered\n");
#endif /* DEBUG */

    CGPointFromPointLine(Point, Pl, Vl, Ptemp);/* Find closest point on line.*/
    t = CGDistPointPoint(Point, Ptemp);

#ifdef DEBUG
    printf("CGDistPointLine exit\n");
#endif /* DEBUG */

    return t;
}

/*****************************************************************************
*   Routine to calc the distance between a Point and a Plane. The Plane is   *
* defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 elements vector.    *
*****************************************************************************/
RealType CGDistPointPlane(PointType Point, PlaneType Plane)
{
    RealType t;

#ifdef DEBUG
    printf("CGDistPointPlane entered\n");
#endif /* DEBUG */

    t = ((Plane[0] * Point [0] +
	  Plane[1] * Point [1] +
	  Plane[2] * Point [2] +
	  Plane[3]) /
	 sqrt(SQR(Plane[0]) + SQR(Plane[1]) + SQR(Plane[2])));

#ifdef DEBUG
    printf("CGDistPointPlane exit\n");
#endif /* DEBUG */

    return t;
}

/*****************************************************************************
*   Routine to find the intersection point of a line and a plane (if any)    *
*   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
* elements vector. The line is define via a point on it Pl and a direction   *
* vector Vl. Return TRUE only if such point exists.                          *
*****************************************************************************/
int CGPointFromLinePlane(PointType Pl, PointType Vl, PlaneType Plane,
					PointType InterPoint, RealType *t)
{
    int i;
    RealType DotProd;

#ifdef DEBUG
    printf("CGPointFromLinePlane entered\n");
#endif /* DEBUG */

    /* Check to see if they are vertical - no intersection at all! */
    DotProd = VecDotProd(Vl, Plane);
    if (ABS(DotProd) < EPSILON) return FALSE;

    /* Else find t in line such that the plane equation plane is satisfied: */
    *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
								/ DotProd;

    /* And so find the intersection point which is at that t: */
    for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];

#ifdef DEBUG
    printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG */

    return TRUE;
}

/*****************************************************************************
*   Routine to find the intersection point of a line and a plane (if any)    *
*   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
* elements vector. The line is define via a point on it Pl and a direction   *
* vector Vl: Line == Pl + Vl * t, where t is the line parameter.	     *
*   Return TRUE only if such point exists, in the t parameter range [0..1].  *
*****************************************************************************/
int CGPointFromLinePlane01(PointType Pl, PointType Vl, PlaneType Plane,
					PointType InterPoint, RealType *t)
{
    int i;
    RealType DotProd;

#ifdef DEBUG
    printf("CGPointFromLinePlane01 entered\n");
#endif /* DEBUG */

    /* Check to see if they are vertical - no intersection at all! */
    DotProd = VecDotProd(Vl, Plane);
    if (ABS(DotProd) < EPSILON) return FALSE;

    /* Else find t in line such that the plane equation plane is satisfied: */
    *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
								/ DotProd;

    if ((*t < 0.0 && !APX_EQ(*t, 0.0)) ||	  /* Not in parameter range. */
	(*t > 1.0 && !APX_EQ(*t, 1.0))) return FALSE;

    /* And so find the intersection point which is at that t : */
    for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];

#ifdef DEBUG
    printf("CGPointFromLinePlane01 exit\n");
#endif /* DEBUG */

    return TRUE;
}

/*****************************************************************************
*   Routine to find the two point Pti on the lines (Pli, Vli) ,   i = 1, 2   *
* with the minimal euclidian distance between them. In other words the       *
* distance between Pt1 and Pt2 is defined as distance between the two lines. *
*   The two points are calculated using the fact that if V = (Vl1 cross Vl2) *
* then these two points are the intersection point between the following:    *
* point 1 - a plane (defined by V and line1) and the line line2.             *
* point 2 - a plane (defined by V and line2) and the line line1.             *
*   This function returns TRUE iff the two lines are not parallel!           *
*****************************************************************************/
int CG2PointsFromLineLine(PointType Pl1, PointType Vl1,
			  PointType Pl2, PointType Vl2,
			  PointType Pt1, RealType *t1,
			  PointType Pt2, RealType *t2)
{
    int i;
    PointType Vtemp;
    PlaneType Plane1, Plane2;

#ifdef DEBUG
    printf("CG2PointsFromLineLine entered\n");
#endif /* DEBUG */

    VecCrossProd(Vtemp, Vl1, Vl2);     /* Check to see if they are parallel. */
    if (VecLength(Vtemp) < EPSILON) {
	for (i = 0; i < 3; i++)
	    Pt1[i] = Pl1[i];		     /* Pick point on line1 and find */
	CGPointFromPointLine(Pl1, Pl2, Vl2, Pt2); /* closest point on line2. */
        return FALSE;
    }

    /* Define the two planes: 1) Vl1, Pl1, Vtemp and 2) Vl2, Pl2, Vtemp	     */
    /* Note this sets the first 3 elements A, B, C out of the 4...	     */
    VecCrossProd(Plane1, Vl1, Vtemp);           /* Find the A, B, C coef.'s. */
    VecNormalize(Plane1);
    VecCrossProd(Plane2, Vl2, Vtemp);           /* Find the A, B, C coef.'s. */
    VecNormalize(Plane2);

    /* and now use a point on the plane to find the 4th coef. D: */
    Plane1[3] = (-VecDotProd(Plane1, Pl1)); /* Note VecDotProd uses only the */
    Plane2[3] = (-VecDotProd(Plane2, Pl2)); /* first three elements in vec.  */

    /* Thats it! now we should solve for the intersection point between a    */
    /* line and a plane but we already familiar with this problem...         */
    i = CGPointFromLinePlane(Pl1, Vl1, Plane2, Pt1, t1) &&
	CGPointFromLinePlane(Pl2, Vl2, Plane1, Pt2, t2);

#ifdef DEBUG
    printf("CG2PointsFromLineLine exit\n");
#endif /* DEBUG */

    return i;
}

/*****************************************************************************
*   Routine to find the distance between two lines (Pli, Vli) ,  i = 1, 2.   *
*****************************************************************************/
RealType CGDistLineLine(PointType Pl1, PointType Vl1,
			PointType Pl2, PointType Vl2)
{
    RealType t1, t2;
    PointType Ptemp1, Ptemp2;

#ifdef DEBUG
    printf("CGDistLineLine entered\n");
#endif /* DEBUG */

    CG2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, Ptemp1, &t1, Ptemp2, &t2);
    t1 = CGDistPointPoint(Ptemp1, Ptemp2);

#ifdef DEBUG
    printf("CGDistLineLine exit\n");
#endif /* DEBUG */

    return t1;
}

/*****************************************************************************
*   Routine implements Jordan Theorem: Fire a ray from a given point and find*
* number of intersections of ray with the polygon, excluding the given point *
* Pt (start of ray) itself, if on polygon boundary. The ray is fired in +X   *
* (Axes == 0) or +Y (Axes == 1). And only the X/Y coordinates of the polygon *
* are taken into account, i.e. the orthogonal projection of the polygon on   *
* a X/Y parallel plane (equal to polygon itself if on X/Y parallel plane...) *
*   Note that if the point is on polygon boundary, the ray should not be in  *
* its edge direction							     *
*									     *
*   Algorithm:								     *
* 1. Set NumOfIntersection = 0;						     *
*    Find vertex V not on Ray level and set AlgState to its level (below or  *
*    above the ray level). If none goto 3				     *
*    Mark VStart = V;							     *
* 2. 2.1. While State(V) == AlgState do					     *
*		2.1.1. V = V -> Pnext;					     *
*		2.1.2. If V == VStart goto 3				     *
*	  end;								     *
*	  IntersectionMinX = INFINITY;					     *
*    2.2. While State(V) == ON_RAY do					     *
*		IntersectionMin = MIN(IntersectionMin, V -> Pt[Axes]);	     *
*		V = V -> Pnext;						     *
*         end;								     *
*    2.3. If State(V) != AlgState do					     *
*		2.3.1. Find the intersection point between polygon edge      *
*		       Vlast, V and the Ray and update IntersectionMin if    *
*		       lower than it.					     *
*		2.3.2. If IntersectionMin is greater than Pt[Axes] increase  *
*		       the NumOfIntersection counter by 1.		     *
*	  end;								     *
*    2.4. AlgState = State(V);						     *
*    2.5. goto 2.1.							     *
* 3. return NumOfIntersection;						     *
*									     *
*****************************************************************************/
int CGPolygonRayInter(PolygonStruct *Pl, PointType PtRay, int RayAxes)
{
    int NewState, AlgState, RayOtherAxes,
	NumOfInter = 0,
	Quit = FALSE;
    RealType InterMin, Inter, t;
    VertexStruct *V, *VStart, *VLast;

    RayOtherAxes = (RayAxes == 1 ? 0 : 1);     /* Other dir: X -> Y, Y -> X. */

    /* Stage 1 - find a vertex below the ray level: */
    V = VStart = Pl -> V;
    do {
	if ((AlgState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
							!= ON_RAY) break;
	V = V -> Pnext;
    }
    while (V != VStart && V != NULL);
    if (AlgState == ON_RAY) return 0;
    VStart = V; /* Vertex Below Ray level */

    /* Stage 2 - scan the vertices and count number of intersections. */
    while (!Quit) {
	/* Stage 2.1. : */
	while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == AlgState) {
	    VLast = V;
	    V = V -> Pnext;
	    if (V == VStart) {
		Quit = TRUE;
		break;
	    }
	}
	InterMin = INFINITY;

	/* Stage 2.2. : */
	while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == ON_RAY) {
	    InterMin = MIN(InterMin, V -> Pt[RayAxes]);
	    VLast = V;
	    V = V -> Pnext;
	    if (V == VStart) Quit = TRUE;
	}

	/* Stage 2.3. : */
	if ((NewState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
								!= AlgState) {
	    /* Stage 2.3.1 Intersection with ray is in middle of edge: */
	    t = (PtRay[RayOtherAxes] - V -> Pt[RayOtherAxes]) /
		(VLast -> Pt[RayOtherAxes] - V -> Pt[RayOtherAxes]);
	    Inter = VLast -> Pt[RayAxes] * t + V -> Pt[RayAxes] * (1.0 - t);
	    InterMin = MIN(InterMin, Inter);

	    /* Stage 2.3.2. comp. with ray base and inc. # of inter if above.*/
	    if (InterMin > PtRay[RayAxes] &&
		!APX_EQ(InterMin, PtRay[RayAxes])) NumOfInter++;
	}

	AlgState = NewState;
    }

    return NumOfInter;
}

/*****************************************************************************
*   Routine to return the relation between the ray level and a given point,  *
* to be used in the CGPolygonRayInter routine above.			     *
*****************************************************************************/
static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes)
{
    if (APX_EQ(PtRay[Axes], Pt[Axes]))
        return ON_RAY;
    else if (PtRay[Axes] < Pt[Axes])
        return ABOVE_RAY;
    else
	return BELOW_RAY;
}


/*****************************************************************************
* Same as CGPolygonRayInter but for arbitrary oriented polygon.		     *
* The polygon (and the point) is first rotated to a XY parallel plane.	     *
*****************************************************************************/
int CGPolygonRayInter3D(PolygonStruct *Pl, PointType PtRay, int RayAxes)
{
    int i;
    MatrixType RotMat;
    VertexStruct *V, *VHead;
    PolygonStruct *RotPl;
    PointType RotPt;

    /* Make a copy of original to work on. */
    RotPl = AllocPolygon(1, Pl ->Tags, CopyVList(Pl -> V), NULL);

    /* Bring the polygon to a XY parallel plane by rotation. */
    GenRotateMatrix(RotMat, Pl -> Plane);
    V = VHead = RotPl -> V;
    do {				    /* Transform the polygon itself. */
	MatMultVecby4by4(V -> Pt, V -> Pt, RotMat);
	V = V -> Pnext;
    }
    while (V != NULL && V != VHead);
    MatMultVecby4by4(RotPt, PtRay, RotMat);

    i = CGPolygonRayInter(RotPl, RotPt, RayAxes);

    MyFree((char *) RotPl, ALLOC_POLYGON);

    return i;
}