/******************************************************************************
* GeoMat3d.c - Trans. Matrices , Vector computation, and Comp.geom.	      *
*******************************************************************************
* Written by Gershon Elber, March 1990.					      *
******************************************************************************/

#include <math.h>
#include <stdio.h>
#include "irit_sm.h"
#include "iritprsr.h"
#include "allocate.h"
#include "convex.h"
#include "geomat3d.h"

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

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

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Routine to copy one vector to another:				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Vdst:      Destination vector.                                           M
*   Vsrc:      Source vector.                                                M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMVecCopy, copy                                                          M
*****************************************************************************/
void GMVecCopy(VectorType Vdst, VectorType Vsrc)
{
    GEN_COPY(Vdst, Vsrc, sizeof(RealType) * 3);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Routine to normalize the vector length to a unit size.                    M
*                                                                            *
* PARAMETERS:                                                                M
*   V:        To normalize.                                                  M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMVecNormalize, normalize                                                M
*****************************************************************************/
void GMVecNormalize(VectorType V)
{
    int i;
    RealType Len;

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

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Routine to compute the magnitude (length) of a given 3D vector:           M
*                                                                            *
* PARAMETERS:                                                                M
*   V:        To compute its magnitude.                                      M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:   Magnitude of V.                                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMVecLength, magnitude                                                   M
*****************************************************************************/
RealType GMVecLength(VectorType V)
{
    return sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Routine to compute the cross product of two vectors.                      M
* Note Vres may be the same as V1 or V2.                                     M
*                                                                            *
* PARAMETERS:                                                                M
*   Vres:    Result of cross product                                         M
*   V1, V2:  Two vectors of the cross product.                               M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMVecCrossProd, cross prod                                               M
*****************************************************************************/
void GMVecCrossProd(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];

    GMVecCopy(Vres, Vtemp);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Verifys the colinearity of three points.                                  M
*                                                                            *
* PARAMETERS:                                                                M
*   Pt1, Pt2, Pt3: Three points to verify for colinearity.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:      TRUE if colinear, FALSE otherwise.                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMColinear3Pts, colinearity                                              M
*****************************************************************************/
int GMColinear3Pts(PointType Pt1, PointType Pt2, PointType Pt3)
{
    VectorType V1, V2, V3;

    PT_SUB(V1, Pt1, Pt2);
    PT_SUB(V2, Pt2, Pt3);

    if (PT_SQR_LENGTH(V1) < SQR(IRIT_EPSILON) ||
	PT_SQR_LENGTH(V2) < SQR(IRIT_EPSILON))
	return TRUE;

    GMVecCrossProd(V3, V1, V2);
    return PT_SQR_LENGTH(V3) < SQR(IRIT_EPSILON);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*  Routine to compute the dot product of two vectors.                        M
*                                                                            M
* PARAMETERS:                                                                M
*   V1, V2:   Two vector to compute dot product of.                          M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:   Resulting dot product.                                       M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMVecDotProd, dot product                                                M
*****************************************************************************/
RealType GMVecDotProd(VectorType V1, VectorType V2)
{
    return  V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate rotation object around the X axis in Degree degrees:   M
*                                                                            *
* PARAMETERS:                                                                M
*   Degree:     Amount of rotation, in degrees.                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   A matrix object.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMGenMatObjectRotX, rotation, transformations                            M
*****************************************************************************/
IPObjectStruct *GMGenMatObjectRotX(RealType *Degree)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate rotation object around the Y axis in Degree degrees:   M
*                                                                            *
* PARAMETERS:                                                                M
*   Degree:     Amount of rotation, in degrees.                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   A matrix object.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMGenMatObjectRotY, rotation, transformations                            M
*****************************************************************************/
IPObjectStruct *GMGenMatObjectRotY(RealType *Degree)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate rotation object around the Z axis in Degree degrees:   M
*                                                                            *
* PARAMETERS:                                                                M
*   Degree:     Amount of rotation, in degrees.                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   A matrix object.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMGenMatObjectRotZ, rotation, transformations                            M
*****************************************************************************/
IPObjectStruct *GMGenMatObjectRotZ(RealType *Degree)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a translation object.                                  M
*                                                                            *
* PARAMETERS:                                                                M
*   Vec:     Amount of translation, in X, Y, and Z.                          M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   A matrix object.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMGenMatObjectTrans, translation, transformations                        M
*****************************************************************************/
IPObjectStruct *GMGenMatObjectTrans(VectorType Vec)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a scaling object.                                      M
*                                                                            *
* PARAMETERS:                                                                M
*   Vec:     Amount of scaling, in X, Y, and Z.                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   A matrix object.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMGenMatObjectScale, scaling, transformations                            M
*****************************************************************************/
IPObjectStruct *GMGenMatObjectScale(VectorType Vec)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to transform an object according to the transformation matrix.     M
*                                                                            *
* PARAMETERS:                                                                M
*   PObj:      Object to be transformed.                                     M
*   Mat:       Transformation matrix.                                        M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:    Transformed object.                                 M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMTransformObject, transformations                                       M
*****************************************************************************/
IPObjectStruct *GMTransformObject(IPObjectStruct *PObj, MatrixType Mat)
{
    int i;
    IPObjectStruct *NewPObj;

    if (IP_IS_POLY_OBJ(PObj)) {
    	int IsPolygon = IP_IS_POLYGON_OBJ(PObj);
    	VectorType Pin, PTemp;
    	IPPolygonStruct *Pl;
    	IPVertexStruct *V, *VFirst;

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

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

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

		MatMultVecby4by4(V -> Coord, V -> Coord, Mat);/* Update pos. */

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

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

	    if (IsPolygon)
		IritPrsrUpdatePolyPlane2(Pl, Pin);	/* Update plane eqn. */

	    Pl = Pl -> Pnext;
	}
    }
    else if (IP_IS_CRV_OBJ(PObj)) {
	CagdCrvStruct *Crv;

    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (Crv = NewPObj -> U.Crvs; Crv != NULL; Crv = Crv -> Pnext)
	    CagdCrvMatTransform(Crv, Mat);
    }
    else if (IP_IS_SRF_OBJ(PObj)) {
	CagdSrfStruct *Srf;

    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (Srf = NewPObj -> U.Srfs; Srf != NULL; Srf = Srf -> Pnext)
	    CagdSrfMatTransform(Srf, Mat);
    }
    else if (IP_IS_TRIMSRF_OBJ(PObj)) {
	TrimSrfStruct *TrimSrf;

    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (TrimSrf = NewPObj -> U.TrimSrfs;
	     TrimSrf != NULL;
	     TrimSrf = TrimSrf -> Pnext)
	    TrimSrfMatTransform(TrimSrf, Mat);
    }
    else if (IP_IS_TRIVAR_OBJ(PObj)) {
	TrivTVStruct *Trivar;

    	NewPObj = CopyObject(NULL, PObj, FALSE);

	for (Trivar = NewPObj -> U.Trivars;
	     Trivar != NULL;
	     Trivar = Trivar -> Pnext)
	    TrivTVMatTransform(Trivar, Mat);
    }
    else if (IP_IS_POINT_OBJ(PObj)) {
    	NewPObj = CopyObject(NULL, PObj, FALSE);

	MatMultVecby4by4(NewPObj -> U.Pt, NewPObj -> U.Pt, Mat);
    }
    else if (IP_IS_CTLPT_OBJ(PObj)) {
	IPObjectStruct
	    *TmpObj = IritPrsrCoerceObjectTo(PObj, CAGD_PT_P3_TYPE);
	RealType R[4];

	PT_COPY(R, &TmpObj -> U.CtlPt.Coords[1]);
	R[3] = TmpObj -> U.CtlPt.Coords[0];
	MatMultVecby4by4(R, R, Mat);
	TmpObj -> U.CtlPt.Coords[0] = R[3];
	PT_COPY(&TmpObj -> U.CtlPt.Coords[1], R);

    	NewPObj = IritPrsrCoerceObjectTo(TmpObj, PObj -> U.CtlPt.PtType);
	IPFreeObject(TmpObj);
    }
    else if (IP_IS_VEC_OBJ(PObj)) {
    	NewPObj = CopyObject(NULL, PObj, FALSE);

	MatMultVecby4by4(NewPObj -> U.Vec, NewPObj -> U.Vec, Mat);
    }
    else if (IP_IS_OLST_OBJ(PObj)) {
	IPObjectStruct *PObjTmp;

	NewPObj = IPAllocObject("", IP_OBJ_LIST_OBJ, NULL);

    	for (i = 0; (PObjTmp = ListObjectGet(PObj, i)) != NULL; i++)
    	    ListObjectInsert(NewPObj, i, GMTransformObject(PObjTmp, Mat));
	ListObjectInsert(NewPObj, i, NULL);
    }
    else {
        NewPObj = CopyObject(NULL, PObj, FALSE);
    }

    /* Make the name with prefix "_". */
    if (PObj -> Name[0] == '_')
        strcpy(NewPObj -> Name, PObj -> Name);
    else {
	NewPObj -> Name[0] = '_';
	strcpy(&NewPObj -> Name[1], PObj -> Name);
    }

    return NewPObj;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to transform an list of objects according to a transformation      M
* matrix.	                                                             M
*                                                                            *
* PARAMETERS:                                                                M
*   PObj:       Object list to transform.                                    M
*   Mat:        Transformation matrix.                                       M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:   Transformed object list.                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   GMTransformObjectList, transformations                                   M
*****************************************************************************/
IPObjectStruct *GMTransformObjectList(IPObjectStruct *PObj, MatrixType Mat)
{
    IPObjectStruct
	*PTailObj = NULL,
	*PTransObj = NULL;

    for ( ; PObj != NULL; PObj = PObj -> Pnext) {
	if (PTailObj == NULL)
	    PTailObj = PTransObj = GMTransformObject(PObj, Mat);
	else {
	    PTailObj -> Pnext = GMTransformObject(PObj, Mat);
	    PTailObj = PTailObj -> Pnext;
	}
    }

    return PTransObj;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to compute the distance between two 3d points.                   M
*                                                                            *
* PARAMETERS:                                                                M
*   P1, P2:   Two points to compute the distance between.                    M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:    Computed distance.                                          M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGDistPointPoint, point point distance                                   M
*****************************************************************************/
RealType CGDistPointPoint(PointType P1, PointType P2)
{
    RealType t;

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

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

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

     return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to construct the plane from given 3 points. If two of the points M
* are the same it returns FALSE, otherwise (successful) returns TRUE.	     M
*                                                                            *
* PARAMETERS:                                                                M
*   Plane:          To compute.                                              M
*   Pt1, Pt2, Pt3:  Three points to fit a plane through.                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:     TRUE if successful, FALSE otherwise.                            M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPlaneFrom3Points, plane                                                M
*****************************************************************************/
int CGPlaneFrom3Points(PlaneType Plane,
		       PointType Pt1,
		       PointType Pt2,
		       PointType Pt3)
{
    VectorType V1, V2;

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

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

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

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

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

    return TRUE;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to compute the closest point on a given 3d line to a given 3d    M
* point. the line is prescribed using a point on it (Pl) and vector (Vl).    M
*                                                                            *
* PARAMETERS:                                                                M
*   Point:         To find the closest to on the line.                       M
*   Pl, Vl:        Position and direction that defines the line.             M
*   ClosestPoint:  Where closest point found on the line is to be saved.     M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPointFromPointLine, point line distance                                M
*****************************************************************************/
void CGPointFromPointLine(PointType Point,
			  PointType Pl,
			  PointType Vl,
			  PointType ClosestPoint)
{
    int i;
    PointType V1, V2;
    RealType CosAlfa, VecMag;

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

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

    CosAlfa = GMVecDotProd(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 DEBUG1
    printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG1 */
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to compute the disstance between a 3d point and a 3d line.         M
*   The line is prescribed using a point on it (Pl) and vector (Vl).         M
*                                                                            *
* PARAMETERS:                                                                M
*   Point:         To find the distance to on the line.                      M
*   Pl, Vl:        Position and direction that defines the line.             M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:      The computed distance.                                    M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGDistPointLine, point line distance                                     M
*****************************************************************************/
RealType CGDistPointLine(PointType Point, PointType Pl, PointType Vl)
{
    RealType t;
    PointType Ptemp;

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

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

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

    return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to compute the distance between a Point and a Plane. The Plane   M
* is prescribed using its four coefficients : Ax + By + Cz + D = 0 given as  M
* four elements vector.							     M
*                                                                            *
* PARAMETERS:                                                                M
*   Point:         To find the distance to on the plane.                     M
*   Plane:         To find the distance to on the point.                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:      The computed distance.                                    M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGDistPointPlane, point plane distance                                   M
*****************************************************************************/
RealType CGDistPointPlane(PointType Point, PlaneType Plane)
{
    RealType t;

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

    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 DEBUG1
    printf("CGDistPointPlane exit\n");
#endif /* DEBUG1 */

    return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to find the intersection point of a line and a plane (if any).     M
*   The Plane is prescribed using four coefficients : Ax + By + Cz + D = 0   M
* given as four elements vector. The line is define via a point on it Pl and M
* a direction vector Vl. Returns TRUE only if such point exists.             M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl, Vl:        Position and direction that defines the line.             M
*   Plane:         To find the intersection with the line.                   M
*   InterPoint:    Where the intersection occured.                           M
*   t:             Parameter along the line of the intersection location     M
*                  (as Pl + Vl * t).                                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:            TRUE, if successful.                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPointFromLinePlane, line plane intersection                            M
*****************************************************************************/
int CGPointFromLinePlane(PointType Pl,
			 PointType Vl,
			 PlaneType Plane,
			 PointType InterPoint,
			 RealType *t)
{
    int i;
    RealType DotProd;

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

    /* Check to see if they are vertical - no intersection at all! */
    DotProd = GMVecDotProd(Vl, Plane);
    if (ABS(DotProd) < IRIT_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 DEBUG1
    printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG1 */

    return TRUE;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to find the intersection point of a line and a plane (if any).     M
*   The Plane is prescribed using four coefficients : Ax + By + Cz + D = 0   M
* given as four elements vector. The line is define via a point on it Pl and M
* a direction vector Vl. Returns TRUE only if such point exists.             M
*   this routine accepts solutions only for t between zero and one.          M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl, Vl:        Position and direction that defines the line.             M
*   Plane:         To find the intersection with the line.                   M
*   InterPoint:    Where the intersection occured.                           M
*   t:             Parameter along the line of the intersection location     M
*                  (as Pl + Vl * t).                                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:            TRUE, if successful and t between zero and one.          M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPointFromLinePlane01, line plane intersection                          M
*****************************************************************************/
int CGPointFromLinePlane01(PointType Pl,
			   PointType Vl,
			   PlaneType Plane,
			   PointType InterPoint,
			   RealType *t)
{
    int i;
    RealType DotProd;

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

    /* Check to see if they are vertical - no intersection at all! */
    DotProd = GMVecDotProd(Vl, Plane);
    if (ABS(DotProd) < IRIT_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 && !IRIT_APX_EQ(*t, 0.0)) ||	  /* Not in parameter range. */
	(*t > 1.0 && !IRIT_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 DEBUG1
    printf("CGPointFromLinePlane01 exit\n");
#endif /* DEBUG1 */

    return TRUE;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to find the two points Pti on the lines (Pli, Vli) ,   i = 1, 2  M
* with the minimal Euclidian distance between them. In other words, the      M
* distance between Pt1 and Pt2 is defined as distance between the two lines. M
*   The two points are calculated using the fact that if V = (Vl1 cross Vl2) M
* then these two points are the intersection point between the following:    M
* Point 1 - a plane (defined by V and line1) and the line line2.             M
* Point 2 - a plane (defined by V and line2) and the line line1.             M
*   This function returns TRUE iff the two lines are not parallel!           M
*                                                                            M
* PARAMETERS:                                                                M
*   Pl1, Vl1:  Position and direction defining the first line.               M
*   Pl2, Vl2:  Position and direction defining the second line.              M
*   Pt1:       Point on Pt1 that is closest to line 2.                       M
*   t1:        Parameter value of Pt1 as (Pl1 + Vl1 * t1).                   M
*   Pt2:       Point on Pt2 that is closest to line 1.                       M
*   t2:        Parameter value of Pt2 as (Pl2 + Vl2 * t2).                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:       TRUE, if successful.                                          M
*                                                                            *
* KEYWORDS:                                                                  M
*   CG2PointsFromLineLine, line line distance                                M
*****************************************************************************/
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 DEBUG1
    printf("CG2PointsFromLineLine entered\n");
#endif /* DEBUG1 */

    GMVecCrossProd(Vtemp, Vl1, Vl2);   /* Check to see if they are parallel. */
    if (GMVecLength(Vtemp) < IRIT_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...	     */
    GMVecCrossProd(Plane1, Vl1, Vtemp);	        /* Find the A, B, C coef.'s. */
    GMVecNormalize(Plane1);
    GMVecCrossProd(Plane2, Vl2, Vtemp);	        /* Find the A, B, C coef.'s. */
    GMVecNormalize(Plane2);

    /* and now use a point on the plane to find the 4th coef. D: */
    Plane1[3] = (-GMVecDotProd(Plane1, Pl1)); /* VecDotProd uses only first  */
    Plane2[3] = (-GMVecDotProd(Plane2, Pl2)); /* 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 DEBUG1
    printf("CG2PointsFromLineLine exit\n");
#endif /* DEBUG1 */

    return i;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to find the distance between two lines (Pli, Vli) ,  i = 1, 2.   M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl1, Vl1:  Position and direction defining the first line.               M
*   Pl2, Vl2:  Position and direction defining the second line.              M
*                                                                            *
* RETURN VALUE:                                                              M
*   RealType:   Distance between the two lines.                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGDistLineLine, line line distance                                       M
*****************************************************************************/
RealType CGDistLineLine(PointType Pl1,
			PointType Vl1,
			PointType Pl2,
			PointType Vl2)
{
    RealType t1, t2;
    PointType Ptemp1, Ptemp2;

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

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

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

    return t1;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine that implements "Jordan Theorem":                                  M
*   Fire a ray from a given point and find the number of intersections of a  M
* ray with the polygon, excluding the given point Pt (start of ray) itself,  M
* if on polygon boundary. The ray is fired in +X (Axes == 0) or +Y if        M
* (Axes == 1).                                                               M
*   Only the X/Y coordinates of the polygon are taken into account, i.e. the M
* orthogonal projection of the polygon on an X/Y parallel plane (equal to    M
* polygon itself if on X/Y parallel plane...).				     M
*   Note that if the point is on polygon boundary, the ray should not be in  M
* its edge direction.							     M
*									     M
* Algorithm:								     M
* 1. 1.1. Set NumOfIntersection = 0;					     M
*    1.2. Find vertex V not on Ray level and set AlgState to its level       M
*         (below or above the ray level). If none goto 3;		     M
*    1.3. Mark VStart = V;						     M
* 2. Do									     M
*    2.1. While State(V) == AlgState do					     M
*	    2.1.1. V = V -> Pnext;					     M
*	    2.1.2. If V == VStart goto 3;				     M
*    2.2. IntersectionMinX = INFINITY;					     M
*    2.3. While State(V) == ON_RAY do					     M
*	    2.3.1. IntersectionMin = MIN(IntersectionMin, V -> Coord[Axes]); M
*	    2.3.2. V = V -> Pnext;					     M
*    2.4. If State(V) != AlgState do					     M
*	    2.4.1. Find the intersection point between polygon edge	     M
*		   Vlast, V and the Ray and update IntersectionMin if	     M
*		   lower than it.					     M
*	    2.4.2. If IntersectionMin is greater than Pt[Axes] increase	     M
*		   the NumOfIntersection counter by 1.		  	     M
*    2.5. AlgState = State(V);						     M
*    2.6. goto 2.2.							     M
* 3. Return NumOfIntersection;						     M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl:        To compute "Jordan Theorem" for the given ray.                M
*   PtRay:     Origin of ray.                                                M
*   RayAxes:   Direction of ray. 0 for X, 1 for Y, etc.                      M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:       Number of intersections of ray with the polygon.              M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPolygonRayInter, ray polygon intersection, Jordan theorem              M
*****************************************************************************/
int CGPolygonRayInter(IPPolygonStruct *Pl, PointType PtRay, int RayAxes)
{
    int NewState, AlgState, RayOtherAxes,
	NumOfInter = 0,
	Quit = FALSE;
    RealType InterMin, Inter, t;
    IPVertexStruct *V, *VStart,
	*VLast = NULL;

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

    /* Stage 1 - find a vertex below the ray level: */
    V = VStart = Pl -> PVertex;
    do {
	if ((AlgState = CGPointRayRelation(V -> Coord, 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 -> Coord, PtRay,
				  RayOtherAxes) == AlgState) {
	    VLast = V;
	    V = V -> Pnext;
	    if (V == VStart) {
		Quit = TRUE;
		break;
	    }
	}
	InterMin = INFINITY;

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

	/* Stage 2.3. : */
	if ((NewState = CGPointRayRelation(V -> Coord, PtRay, RayOtherAxes))
								!= AlgState) {
	    /* Stage 2.3.1 Intersection with ray is in middle of edge: */
	    t = (PtRay[RayOtherAxes] - V -> Coord[RayOtherAxes]) /
		(VLast -> Coord[RayOtherAxes] - V -> Coord[RayOtherAxes]);
	    Inter = VLast -> Coord[RayAxes] * t +
		    V -> Coord[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] &&
		!IRIT_APX_EQ(InterMin, PtRay[RayAxes]))
		NumOfInter++;
	}

	AlgState = NewState;
    }

    return NumOfInter;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Routine to returns the relation between the ray level and a given point, *
* to be used in the CGPolygonRayInter routine above.			     *
*                                                                            *
* PARAMETERS:                                                                *
*   Pt:       Given point.                                                   *
*   PtRay:    Given ray.                                                     *
*   Axes:     Given axes.                                                    *
*                                                                            *
* RETURN VALUE:                                                              *
*   int:      Pt is either above below or on the ray.                        *
*****************************************************************************/
static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes)
{
    if (IRIT_APX_EQ(PtRay[Axes], Pt[Axes]))
        return ON_RAY;
    else if (PtRay[Axes] < Pt[Axes])
        return ABOVE_RAY;
    else
	return BELOW_RAY;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Same as CGPolygonRayInter but for arbitrary oriented polygon.		     M
*   The polygon is transformed into the XY plane and then CGPolygonRayInter  M
* is invoked on it.                                                          M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl:        To compute "Jordan Theorem" for the given ray.                M
*   PtRay:     Origin of ray.                                                M
*   RayAxes:   Direction of ray. 0 for X, 1 for Y, etc.                      M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:       Number of intersections of ray with the polygon.              M
*                                                                            *
* KEYWORDS:                                                                  M
*   CGPolygonRayInter3D, ray polygon intersection, Jordan theorem            M
*****************************************************************************/
int CGPolygonRayInter3D(IPPolygonStruct *Pl, PointType PtRay, int RayAxes)
{
    int i;
    MatrixType RotMat;
    IPVertexStruct *V, *VHead;
    IPPolygonStruct *RotPl;
    PointType RotPt;

    /* Make a copy of original to work on. */
    RotPl = IPAllocPolygon(1, Pl ->Tags, CopyVertexList(Pl -> PVertex), NULL);

    /* Make sure list is circular. */
    V = IritPrsrGetLastVrtx(RotPl -> PVertex);
    if (V -> Pnext == NULL);
	V -> Pnext = RotPl -> PVertex;

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

    i = CGPolygonRayInter(RotPl, RotPt, RayAxes);

    IPFreePolygonList(RotPl);

    return i;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to prepar a transformation martix to do the following (in this   M
* order): scale by Scale, rotate such that the Z axis is in direction Dir    M
* and then translate by Trans.						     M
*    Algorithm: given the Trans vector, it forms the 4th line of Mat. Dir is M
* used to form the second line (the first 3 lines set the rotation), and     M
* finally Scale is used to scale first 3 lines/columns to the needed scale:  M
*                |  Tx  Ty  Tz  0 |   A transformation which takes the coord V
*                |  Bx  By  Bz  0 |  system into T, N & B as required and    V
* [X  Y  Z  1] * |  Nx  Ny  Nz  0 |  then translate it to C. T, N, B are     V
*                |  Cx  Cy  Cz  1 |  scaled by Scale.			     V
* N is exactly Dir (unit vec) but we got freedom on T & B which must be on   M
* a plane perpendicular to N and perpendicular between them but thats all!   M
* T is therefore selected using this (heuristic ?) algorithm:		     M
* Let P be the axis of which the absolute N coefficient is the smallest.     M
* Let B be (N cross P) and T be (B cross N).				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Mat:       To place the computed transformation.                         M
*   Trans:     Translation factor.                                           M
*   Dir:       Direction to take Z axis to.                                  M
*   Scale:     Scaling factor.                                               M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*    CGGenTransMatrixZ2Dir, transformations                                  M
*****************************************************************************/
void CGGenTransMatrixZ2Dir(MatrixType Mat,
			   VectorType Trans,
			   VectorType Dir,
			   RealType Scale)
{
    int i, j;
    RealType R;
    VectorType DirN, T, B, P;
    MatrixType TempMat;

    PT_COPY(DirN, Dir);
    PT_NORMALIZE(DirN);
    PT_CLEAR(P);
    for (i = 1, j = 0, R = ABS(DirN[0]); i < 3; i++)
	if (R > ABS(DirN[i])) {
	    R = DirN[i];
	    j = i;
	}
    P[j] = 1.0;/* Now P is set to the axis with the biggest angle from DirN. */

    GMVecCrossProd(B, DirN, P);			      /* Calc the bi-normal. */
    GMVecCrossProd(T, B, DirN);				/* Calc the tangent. */

    MatGenUnitMat(Mat);
    for (i = 0; i < 3; i++) {
	Mat[0][i] = T[i];
	Mat[1][i] = B[i];
	Mat[2][i] = DirN[i];
    }
    MatGenMatUnifScale(Scale, TempMat);
    MatMultTwo4by4(Mat, TempMat, Mat);

    MatGenMatTrans(Trans[0], Trans[1], Trans[2], TempMat);
    MatMultTwo4by4(Mat, Mat, TempMat);
}
/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to prepar a transformation martix to do the following (in this   M
* order): scale by Scale, rotate such that the Z axis is in direction Dir    M
* and X axis is direction T and then translate by Trans.		     M
*    Algorithm: given the Trans vector, it forms the 4th line of Mat. Dir is M
* used to form the second line (the first 3 lines set the rotation), and     M
* finally Scale is used to scale first 3 lines/columns to the needed scale:  M
*                |  Tx  Ty  Tz  0 |   A transformation which takes the coord V
*                |  Bx  By  Bz  0 |  system into T, N & B as required and    V
* [X  Y  Z  1] * |  Nx  Ny  Nz  0 |  then translate it to C. T, N, B are     V
*                |  Cx  Cy  Cz  1 |  scaled by Scale.			     V
* N is exactly Dir (unit vec) and T is exactly Dir2.			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Mat:       To place the computed transformation.                         M
*   Trans:     Translation factor.                                           M
*   Dir:       Direction to take Z axis to.                                  M
*   Dir2:      Direction to take X axis to.                                  M
*   Scale:     Scaling factor.                                               M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*    CGGenTransMatrixZ2Dir2, transformations                                 M
*****************************************************************************/
void CGGenTransMatrixZ2Dir2(MatrixType Mat,
			    VectorType Trans,
			    VectorType Dir,
			    VectorType Dir2,
			    RealType Scale)
{
    int i;
    VectorType DirN, Dir2N, B;
    MatrixType TempMat;

    PT_COPY(DirN, Dir);
    PT_NORMALIZE(DirN);
    PT_COPY(Dir2N, Dir2);
    PT_NORMALIZE(Dir2N);

    GMVecCrossProd(B, DirN, Dir2N);		      /* Calc the bi-normal. */

    MatGenUnitMat(Mat);
    for (i = 0; i < 3; i++) {
	Mat[0][i] = Dir2N[i];
	Mat[1][i] = B[i];
	Mat[2][i] = DirN[i];
    }
    MatGenMatUnifScale(Scale, TempMat);
    MatMultTwo4by4(Mat, TempMat, Mat);

    MatGenMatTrans(Trans[0], Trans[1], Trans[2], TempMat);
    MatMultTwo4by4(Mat, Mat, TempMat);
}
