/******************************************************************************
* 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);

/*****************************************************************************
*  Routine to copy one vector to another:				     *
*****************************************************************************/
void GMVecCopy(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 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]) < EPSILON)
		V[i] = 0.0;
	}
}

/*****************************************************************************
*  Routine to calculate the magnitude (length) of a given 3D vector:         *
*****************************************************************************/
RealType GMVecLength(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 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);
}

/*****************************************************************************
*  Routine to calculate the dot product of two vectors:                      *
*****************************************************************************/
RealType GMVecDotProd(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:   *
*****************************************************************************/
IPObjectStruct *GMGenMatObjectRotX(RealType *Degree)
{
    MatrixType Mat;

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

    return GenMATObject(Mat);
}

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

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

    return GenMATObject(Mat);
}

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

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

    return GenMATObject(Mat);
}

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

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

    return GenMATObject(Mat);
}

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

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

    return GenMATObject(Mat);
}

/*****************************************************************************
* Routine to transform an object according to the transformation matrix.     *
*****************************************************************************/
IPObjectStruct *GMTransformObject(IPObjectStruct *PObj, MatrixType Mat)
{
    int i;
    IPObjectStruct *NewPObj;

    if (IP_IS_POLY_OBJ(PObj)) {
    	int IsPolygon = !IP_IS_POLYLINE_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_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_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_POINT_OBJ(PObj)) {
    	NewPObj = CopyObject(NULL, PObj, FALSE);

	MatMultVecby4by4(NewPObj -> U.Pt, NewPObj -> U.Pt, Mat);
    }
    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);
    }

    return NewPObj;
}

/*****************************************************************************
* Routine to transform an object list according to transformation matrix.    *
*****************************************************************************/
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;
}

/*****************************************************************************
*   Routine to calc the distance between two 3d points                       *
*****************************************************************************/
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;
}

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

    if (PT_APX_EQ(Pt1, Pt2) || PT_APX_EQ(Pt2, Pt3) || 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;
}

/*****************************************************************************
*   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 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 */
}

/*****************************************************************************
*   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 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;
}

/*****************************************************************************
*   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 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;
}

/*****************************************************************************
*   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 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) < 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;
}

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

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

    GMVecCrossProd(Vtemp, Vl1, Vl2);   /* Check to see if they are parallel. */
    if (GMVecLength(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...	     */
    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;
}

/*****************************************************************************
*   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 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;
}

/*****************************************************************************
*   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 -> Coord[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(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] &&
		!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(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);

    /* 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 != NULL && V != VHead);
    MatMultVecby4by4(RotPt, PtRay, RotMat);

    i = CGPolygonRayInter(RotPl, RotPt, RayAxes);

    IPFreePolygonList(RotPl);

    return i;
}