/****************************************************************************** * GeoMat3d.c - Trans. Matrices , Vector computation, and Comp.geom. * ******************************************************************************* * (C) Gershon Elber, Technion, Israel Institute of Technology * ******************************************************************************* * Written by Gershon Elber, March 1990. * ******************************************************************************/ #include #include #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_UEPS) 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_UEPS) || PT_SQR_LENGTH(V2) < SQR(IRIT_UEPS)) return TRUE; GMVecCrossProd(V3, V1, V2); return PT_SQR_LENGTH(V3) < SQR(IRIT_UEPS); } /***************************************************************************** * 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 rotation object to rotate Z to Dir: M * * * PARAMETERS: M * Dir: Vector to rotate Z axis to it. M * * * RETURN VALUE: M * IPObjectStruct *: A matrix object. M * * * KEYWORDS: M * GMGenMatObjectZ2Dir, rotation, transformations M *****************************************************************************/ IPObjectStruct *GMGenMatObjectZ2Dir(VectorType Dir) { MatrixType Mat; CGGenMatrixZ2Dir(Mat, Dir); /* Generate the matrix. */ return GenMATObject(Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate rotation object around the vector Dir in Degree degs: M * * * PARAMETERS: M * Dir: Vector to rotate Z axis to it. M * Dir2: Vector to rotate X axis to it. M * * * RETURN VALUE: M * IPObjectStruct *: A matrix object. M * * * KEYWORDS: M * GMGenMatObjectRotVec, rotation, transformations M *****************************************************************************/ IPObjectStruct *GMGenMatObjectZ2Dir2(VectorType Dir, VectorType Dir2) { MatrixType Mat; CGGenMatrixZ2Dir2(Mat, Dir, Dir2); /* Generate the matrix. */ return GenMATObject(Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate rotation object around the vector Vec in Degree degs: M * * * PARAMETERS: M * Vec: Vector to rotate along its axis. M * Degree: Amount of rotation, in degrees. M * * * RETURN VALUE: M * IPObjectStruct *: A matrix object. M * * * KEYWORDS: M * GMGenMatObjectRotVec, rotation, transformations M *****************************************************************************/ IPObjectStruct *GMGenMatObjectRotVec(VectorType Vec, RealType *Degree) { MatrixType Mat; CGGenMatrixRotVec(Mat, Vec, *Degree); /* Generate the 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 extract the translational part of a matrix or dump it. M * * * PARAMETERS: M * MatObj: To operate on. M * TransPortion: TRUE to extract translational portion out of Mat, FALSE M * to dump the translational portion from Mat. M * * * RETURN VALUE: M * IPObjectStruct *: A matrix object hold either the translational portion M * of Mat or anything but the translational part. M * * * KEYWORDS: M * GMGenMatObjectScale, translation, transformations M *****************************************************************************/ IPObjectStruct *GMGetMatTransPortion(IPObjectStruct *MatObj, int *TransPortion) { int i, j; MatrixType Mat; GEN_COPY(Mat, MatObj -> U.Mat, sizeof(MatrixType)); if (*TransPortion) { for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) Mat[i][j] = i == j; } else { for (i = 0; i < 3; i++) Mat[3][i] = 0.0; } 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); if (!PT_EQ_ZERO(V -> Normal)) 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_MODEL_OBJ(PObj)) { NewPObj = CopyObject(NULL, PObj, FALSE); MdlModelMatTransform(NewPObj -> U.Mdls, 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_TRISRF_OBJ(PObj)) { TrngTriangSrfStruct *TriSrf; NewPObj = CopyObject(NULL, PObj, FALSE); for (TriSrf = NewPObj -> U.TriSrfs; TriSrf != NULL; TriSrf = TriSrf -> Pnext) TrngTriSrfMatTransform(TriSrf, 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]; MatMultWVecby4by4(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_INSTNC_OBJ(PObj)) { NewPObj = CopyObject(NULL, PObj, FALSE); MatMultTwo4by4(NewPObj -> U.Instance -> Mat, NewPObj -> U.Instance -> Mat, 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); } strcpy(NewPObj -> Name, 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 or the three points are colinear it returns FALSE, otherwise M * (successful), it 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 (GMColinear3Pts(Pt1, Pt2, 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_UEPS) 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_UEPS) 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_UEPS) { 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 = IRIT_INFNTY; 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 * Can be either a polygon or a closed polyline (first and last M * points of polyline are equal). 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, Quit = FALSE, NumOfInter = 0; 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; } else if (V == NULL) return NumOfInter; } InterMin = IRIT_INFNTY; /* 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; else if (V == NULL) return NumOfInter; } /* 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(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 prepare 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, rotation 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. */ PT_NORMALIZE(B); 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 * Same as CGGenTransMatrixZ2Dir but with no scaling and/or translation. M * * * PARAMETERS: M * Mat: To place the computed transformation. M * Dir: Direction to take Z axis to. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * CGGenMatrixZ2Dir, transformations, rotation M *****************************************************************************/ void CGGenMatrixZ2Dir(MatrixType Mat, VectorType Dir) { VectorType Trans; PT_CLEAR(Trans); CGGenTransMatrixZ2Dir(Mat, Trans, Dir, 1.0); } /***************************************************************************** * DESCRIPTION: M * Routine to prepare 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, rotation 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); } /***************************************************************************** * DESCRIPTION: M * Same as CGGenTransMatrixZ2Dir2 but with no scaling and/or translation. M * * * PARAMETERS: M * Mat: To place the computed transformation. M * Dir: Direction to take Z axis to. M * Dir2: Direction to take X axis to. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * CGGenMatrixZ2Dir2, transformations, rotation M *****************************************************************************/ void CGGenMatrixZ2Dir2(MatrixType Mat, VectorType Dir, VectorType Dir2) { VectorType Trans; PT_CLEAR(Trans); CGGenTransMatrixZ2Dir2(Mat, Trans, Dir, Dir2, 1.0); } /***************************************************************************** * DESCRIPTION: M * Generates a transformation matrix that rotates the object around Vec, M * Angle degrees. M * * * PARAMETERS: M * Mat: To place the computed transformation. M * Vec: Vector to rotate along its axis. M * Degrees: Amount ofrotation, in degrees. M * * * RETURN VALUE: M * void M * M * KEYWORDS: M * CGGenMatrixRotVec, transformations, rotation M *****************************************************************************/ void CGGenMatrixRotVec(MatrixType Mat, VectorType Vec, RealType Degrees) { MatrixType Mat1, Mat1i; CGGenMatrixZ2Dir(Mat1, Vec); MatInverseMatrix(Mat1, Mat1i); MatGenMatRotZ1(DEG2RAD(Degrees), Mat); MatMultTwo4by4(Mat, Mat, Mat1); MatMultTwo4by4(Mat, Mat1i, Mat); } /***************************************************************************** * DESCRIPTION: M * Computes the Barycentric coordinates of given point Pt with respect to M * given Triangle Pt1 Pt2 Pt3. All points are assumed to be coplanar. M * * * PARAMETERS: M * Pt1, Pt2, Pt3: Three points forming a triangular in general position. M * Pt: A point for which the barycentric coordinates are to be M * computed. M * * * RETURN VALUE: M * RealType *: A pointers to a static space holding the three Barycentric M * coefficients, or NULL if point Pt is outside the triangle M * Pt1 Pt2 Pt3. M * * * KEYWORDS: M * CGBaryCentric3Pts M *****************************************************************************/ RealType *CGBaryCentric3Pts(PointType Pt1, PointType Pt2, PointType Pt3, PointType Pt) { static VectorType RetVal; int i; VectorType V1, V2, V3, X12, X23, X31; RealType R; PT_SUB(V1, Pt, Pt1); PT_SUB(V2, Pt, Pt2); PT_SUB(V3, Pt, Pt3); CROSS_PROD(X12, V1, V2); CROSS_PROD(X23, V2, V3); CROSS_PROD(X31, V3, V1); if (DOT_PROD(X12, X23) < 0 || DOT_PROD(X23, X31) < 0 || DOT_PROD(X31, X12) < 0) return NULL; /* Pt is out of the triangle Pt1 Pt2 Pt3. */ RetVal[0] = sqrt(DOT_PROD(X23, X23)); RetVal[1] = sqrt(DOT_PROD(X31, X31)); RetVal[2] = sqrt(DOT_PROD(X12, X12)); if ((R = RetVal[0] + RetVal[1] + RetVal[2]) > 0.0) { R = 1.0 / R; PT_SCALE(RetVal, R); } for (i = 0; i < 3; i++) if (RetVal[i] < -IRIT_EPS || RetVal[i] > 1.0 + IRIT_EPS) return NULL; /* Pt is out of the triangle Pt1 Pt2 Pt3. */ return RetVal; }