home *** CD-ROM | disk | FTP | other *** search
/ Mega CD-ROM 1 / megacd_rom_1.zip / megacd_rom_1 / IRIT / IRITS.ZIP / GEOMAT3D.C < prev    next >
C/C++ Source or Header  |  1990-05-05  |  30KB  |  798 lines
  1. /*****************************************************************************
  2. *   "Irit" - the 3d polygonal solid modeller.                     *
  3. *                                         *
  4. * Written by:  Gershon Elber                Ver 0.2, Mar. 1990   *
  5. ******************************************************************************
  6. *   Routines to generate transformation matrices for Translation, Rotation   *
  7. * and Scaling for 3D universe.                              *
  8. *   Routines to manipulate 3D vectors.                         *
  9. *   And some computational geometry routines.                     *
  10. *****************************************************************************/
  11.  
  12. #include <math.h>
  13. #include <stdio.h>
  14. #include "program.h"
  15. #include "objects.h"
  16. #include "geomat3d.h"
  17. #include "primitvg.h"
  18.  
  19. /* #define DEBUG        Print information on entry and exit of routines. */
  20.  
  21. static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes);
  22.  
  23. /*****************************************************************************
  24. * Routine to generate a 4*4 unit matrix:                                     *
  25. *****************************************************************************/
  26. void MatGenUnitMat(MatrixType Mat)
  27. {
  28.     int i, j;
  29.  
  30.     for (i=0; i<4; i++) for (j=0; j<4; j++)
  31.         if (i==j) Mat[i][j] = 1.0;
  32.         else      Mat[i][j] = 0.0;
  33. }
  34.  
  35. /*****************************************************************************
  36. * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
  37. *****************************************************************************/
  38. void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
  39. {
  40.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  41.      Mat[3][0] = Tx;
  42.      Mat[3][1] = Ty;
  43.      Mat[3][2] = Tz;
  44. }
  45.  
  46. /*****************************************************************************
  47. * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
  48. *****************************************************************************/
  49. void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
  50. {
  51.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  52.      Mat[0][0] = Sx;
  53.      Mat[1][1] = Sy;
  54.      Mat[2][2] = Sz;
  55. }
  56.  
  57. /*****************************************************************************
  58. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  59. * Note Teta must be given in radians.                                        *
  60. *****************************************************************************/
  61. void MatGenMatRotX1(RealType Teta, MatrixType Mat)
  62. {
  63.     RealType CTeta, STeta;
  64.  
  65.     CTeta = (RealType) cos((double) Teta);
  66.     STeta = (RealType) sin((double) Teta);
  67.     MatGenMatRotX(CTeta, STeta, Mat);
  68. }
  69.  
  70. /*****************************************************************************
  71. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  72. *****************************************************************************/
  73. void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  74. {
  75.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  76.      Mat[1][1] = CosTeta;
  77.      Mat[1][2] = SinTeta;
  78.      Mat[2][1] = -SinTeta;
  79.      Mat[2][2] = CosTeta;
  80. }
  81.  
  82. /*****************************************************************************
  83. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  84. * Note Teta must be given in radians.                                        *
  85. *****************************************************************************/
  86. void MatGenMatRotY1(RealType Teta, MatrixType Mat)
  87. {
  88.     RealType CTeta, STeta;
  89.  
  90.     CTeta = (RealType) cos((double) Teta);
  91.     STeta = (RealType) sin((double) Teta);
  92.     MatGenMatRotY(CTeta, STeta, Mat);
  93. }
  94.  
  95. /*****************************************************************************
  96. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  97. *****************************************************************************/
  98. void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  99. {
  100.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  101.      Mat[0][0] = CosTeta;
  102.      Mat[0][2] = -SinTeta;
  103.      Mat[2][0] = SinTeta;
  104.      Mat[2][2] = CosTeta;
  105. }
  106.  
  107. /*****************************************************************************
  108. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  109. * Note Teta must be given in radians.                                        *
  110. *****************************************************************************/
  111. void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
  112. {
  113.     RealType CTeta, STeta;
  114.  
  115.     CTeta = (RealType) cos((double) Teta);
  116.     STeta = (RealType) sin((double) Teta);
  117.     MatGenMatRotZ(CTeta, STeta, Mat);
  118. }
  119.  
  120. /*****************************************************************************
  121. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  122. *****************************************************************************/
  123. void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  124. {
  125.      MatGenUnitMat(Mat);                             /* Make it unit matrix, */
  126.      Mat[0][0] = CosTeta;
  127.      Mat[0][1] = SinTeta;
  128.      Mat[1][0] = -SinTeta;
  129.      Mat[1][1] = CosTeta;
  130. }
  131.  
  132. /*****************************************************************************
  133. * Routine to multiply 2 4by4 matrices:                                       *
  134. * Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
  135. *****************************************************************************/
  136. void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  137. {
  138.     int i, j, k;
  139.     MatrixType MatResTemp;
  140.  
  141.     for (i=0; i<4; i++) for (j=0; j<4; j++) {
  142.         MatResTemp[i][j] = 0;
  143.         for (k=0; k<4; k++) MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
  144.     }
  145.     for (i=0; i<4; i++) for (j=0; j<4; j++) MatRes[i][j] = MatResTemp[i][j];
  146. }
  147.  
  148. /*****************************************************************************
  149. * Routine to add 2 4by4 matrices:                         *
  150. * Note MatRes might be one of Mat1 or Mat2.                     *
  151. *****************************************************************************/
  152. void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  153. {
  154.     int i, j;
  155.  
  156.     for (i=0; i<4; i++) for (j=0; j<4; j++)
  157.         MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
  158. }
  159.  
  160. /*****************************************************************************
  161. * Routine to subtract 2 4by4 matrices:                         *
  162. * Note MatRes might be one of Mat1 or Mat2.                     *
  163. *****************************************************************************/
  164. void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  165. {
  166.     int i, j;
  167.  
  168.     for (i=0; i<4; i++) for (j=0; j<4; j++)
  169.         MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
  170. }
  171.  
  172. /*****************************************************************************
  173. * Routine to scale a 4by4 matrix by constant:                     *
  174. * Note MatRes might be Mat.                             *
  175. *****************************************************************************/
  176. void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
  177. {
  178.     int i, j;
  179.  
  180.     for (i=0; i<4; i++) for (j=0; j<4; j++)
  181.         MatRes[i][j] = Mat[i][j] * (*Scale);
  182. }
  183.  
  184. /*****************************************************************************
  185. * Routine to multiply Vector by 4by4 matrix:                                 *
  186. * The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
  187. * Note VRes might be Vec as it is only updated in the end.                   *
  188. *****************************************************************************/
  189. void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
  190. {
  191.     int i, j;
  192.     RealType CalcW = Mat[3][3];
  193.     VectorType VTemp;
  194.  
  195.     for (i=0; i<3; i++) {
  196.         VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
  197.         for (j=0; j<3; j++) VTemp[i] += Vec[j] * Mat[j][i];
  198.     }
  199.  
  200.     for (i=0; i<3; i++) CalcW += Vec[i] * Mat[i][3];
  201.     if (CalcW == 0) CalcW = 1/INFINITY;
  202.  
  203.     for (i=0; i<3; i++) VRes[i] = VTemp[i]/CalcW;
  204. }
  205.  
  206. /*****************************************************************************
  207. * Procedure to calculate the INVERSE of a given matrix M which is not        *
  208. * modified. The matrix is assumed to be 4 by 4 (transformation matrix).      *
  209. * Return TRUE if inverted matrix (InvM) do exists.                 *
  210. *****************************************************************************/
  211. int MatInverseMatrix(MatrixType M, MatrixType InvM)
  212. {
  213.     MatrixType A;
  214.     int i, j, k;
  215.     RealType V;
  216.  
  217.     MAT_COPY(A, M);            /* Prepare temporary copy of M in A. */
  218.     MatGenUnitMat(InvM);                 /* Make it unit matrix. */
  219.  
  220.     for (i=0; i<4; i++) {
  221.     V = A[i][i];                      /* Find the new pivot. */
  222.     k = i;
  223.     for (j=i+1; j<4; j++) if (ABS(A[j][i]) > ABS(V)) {
  224.         /* Find maximum on col i, row i+1..n */
  225.         V = A[j][i];
  226.         k = j;
  227.     }
  228.     j = k;
  229.  
  230.     if (i != j) for (k=0; k<4; k++) {
  231.         SWAP(A[i][k], A[j][k], RealType);
  232.         SWAP(InvM[i][k], InvM[j][k], RealType);
  233.     }
  234.  
  235.     for (j=i+1; j<4; j++) {         /* Eliminate col i from row i+1..n. */
  236.             V = A[j][i] / A[i][i];
  237.         for (k=0; k<4; k++) {
  238.                 A[j][k]    -= V * A[i][k];
  239.                 InvM[j][k] -= V * InvM[i][k];
  240.         }
  241.     }
  242.     }
  243.  
  244.     for (i=3; i>=0; i--) {                   /* Back Substitution. */
  245.     if (A[i][i] == 0) return FALSE;                   /* Error. */
  246.  
  247.     for (j=0; j<i; j++) {         /* Eliminate col i from row 1..i-1. */
  248.             V = A[j][i] / A[i][i];
  249.         for (k=0; k<4; k++) {
  250.                 /* A[j][k] -= V * A[i][k]; */
  251.                 InvM[j][k] -= V * InvM[i][k];
  252.         }
  253.     }
  254.     }
  255.  
  256.     for (i=0; i<4; i++)                /* Normalize the inverse Matrix. */
  257.     for (j=0; j<4; j++)
  258.             InvM[i][j] /= A[i][i];
  259.  
  260.     return TRUE;
  261. }
  262.  
  263. /*****************************************************************************
  264. *  Routine to copy one vector to another:                     *
  265. *****************************************************************************/
  266. void VecCopy(VectorType Vdst, VectorType Vsrc)
  267. {
  268.     int i;
  269.  
  270.     for (i=0; i<3; i++) Vdst[i] = Vsrc[i];
  271. }
  272.  
  273. /*****************************************************************************
  274. *  Routine to normalize the vector length to a unit length:                  *
  275. *****************************************************************************/
  276. void VecNormalize(VectorType V)
  277. {
  278.     int i;
  279.     RealType Len;
  280.  
  281.     Len = VecLength(V);
  282.     if (Len > 0.0) for (i=0; i<3; i++) {
  283.     V[i] /= Len;
  284.     if (ABS(V[i]) < EPSILON) V[i] = 0.0;
  285.     }
  286. }
  287.  
  288. /*****************************************************************************
  289. *  Routine to calculate the magnitude (length) of a given 3D vector:         *
  290. *****************************************************************************/
  291. RealType VecLength(VectorType V)
  292. {
  293.     return sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
  294. }
  295.  
  296. /*****************************************************************************
  297. *  Routine to calculate the cross product of two vectors:                    *
  298. * Note Vres might be the same as V1 or V2 !                                  *
  299. *****************************************************************************/
  300. void VecCrossProd(VectorType Vres, VectorType V1, VectorType V2)
  301. {
  302.     VectorType Vtemp;
  303.  
  304.     Vtemp[0] = V1[1] * V2[2] - V2[1] * V1[2];
  305.     Vtemp[1] = V1[2] * V2[0] - V2[2] * V1[0];
  306.     Vtemp[2] = V1[0] * V2[1] - V2[0] * V1[1];
  307.  
  308.     VecCopy(Vres, Vtemp);
  309. }
  310.  
  311. /*****************************************************************************
  312. *  Routine to calculate the dot product of two vectors:                      *
  313. *****************************************************************************/
  314. RealType VecDotProd(VectorType V1, VectorType V2)
  315. {
  316.     return  V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2];
  317. }
  318.  
  319. /*****************************************************************************
  320. * Routine to generate rotation object around the X axis in Degree degrees:   *
  321. *****************************************************************************/
  322. struct ObjectStruct *GenMatObjectRotX(RealType *Degree)
  323. {
  324.     MatrixType Mat;
  325.  
  326.     MatGenMatRotX1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  327.  
  328.     return GenMatObject("", Mat, NULL);
  329. }
  330.  
  331. /*****************************************************************************
  332. * Routine to generate rotation object around the Y axis in Degree degrees:   *
  333. *****************************************************************************/
  334. struct ObjectStruct *GenMatObjectRotY(RealType *Degree)
  335. {
  336.     MatrixType Mat;
  337.  
  338.     MatGenMatRotY1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  339.  
  340.     return GenMatObject("", Mat, NULL);
  341. }
  342.  
  343. /*****************************************************************************
  344. * Routine to generate rotation object around the Z axis in Degree degrees:   *
  345. *****************************************************************************/
  346. struct ObjectStruct *GenMatObjectRotZ(RealType *Degree)
  347. {
  348.     MatrixType Mat;
  349.  
  350.     MatGenMatRotZ1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  351.  
  352.     return GenMatObject("", Mat, NULL);
  353. }
  354.  
  355. /*****************************************************************************
  356. * Routine to generate translation object according to XYZ vector Vec.         *
  357. *****************************************************************************/
  358. struct ObjectStruct * GenMatObjectTrans(VectorType Vec)
  359. {
  360.     MatrixType Mat;
  361.  
  362.     /* Generate the transformation matrix */
  363.     MatGenMatTrans(Vec[0], Vec[1], Vec[2], Mat);
  364.  
  365.     return GenMatObject("", Mat, NULL);
  366. }
  367.  
  368. /*****************************************************************************
  369. * Routine to scale an object according to XYZ scaling vector Vec.         *
  370. *****************************************************************************/
  371. struct ObjectStruct * GenMatObjectScale(VectorType Vec)
  372. {
  373.     MatrixType Mat;
  374.  
  375.     /* Generate the transformation matrix */
  376.     MatGenMatScale(Vec[0], Vec[1], Vec[2], Mat);
  377.  
  378.     return GenMatObject("", Mat, NULL);
  379. }
  380.  
  381. /*****************************************************************************
  382. * Routine to transform an object according to the transformation matrix.     *
  383. *****************************************************************************/
  384. struct ObjectStruct *TransformObject(ObjectStruct *PObj, MatrixType Mat)
  385. {
  386.     VectorType Pin;
  387.     struct PolygonStruct *Pl = PObj -> U.Pl;
  388.     struct VertexStruct *V, *VFirst;
  389.  
  390.     if (!IS_GEOM_OBJ(PObj))
  391.     FatalError("TransfromObject: object is not geometric\n");
  392.  
  393.     while (Pl != NULL) {
  394.     V = VFirst = Pl -> V;
  395.     PT_ADD(Pin, V -> Pt, Pl -> Plane);  /* Prepare point in the IN side. */
  396.     MatMultVecby4by4(Pin, Pin, Mat); /* transformed relative to new pos. */
  397.  
  398.     do {
  399.         MatMultVecby4by4(V -> Pt, V -> Pt, Mat);
  400.         V = V -> Pnext;
  401.     }
  402.     while (V != VFirst && V != NULL);           /* VList is circular! */
  403.  
  404.     if (!IS_POLYLINE_GEOM_OBJ(PObj))
  405.         UpdatePolyPlane(Pl, Pin);   /* Update plane eqn. using IN point. */
  406.  
  407.     Pl = Pl -> Pnext;
  408.     }
  409.  
  410.     return PObj;
  411. }
  412.  
  413. /*****************************************************************************
  414. *   Routine to calc the distance between two 3d points                       *
  415. *****************************************************************************/
  416. RealType CGDistPointPoint(PointType P1, PointType P2)
  417. {
  418.     RealType t;
  419.  
  420. #ifdef DEBUG
  421.     printf("CGDistPointPoint entered\n");
  422. #endif /* DEBUG */
  423.  
  424.     t = sqrt(SQR(P2[0]-P1[0]) + SQR(P2[1]-P1[1]) + SQR(P2[2]-P1[2]));
  425.  
  426. #ifdef DEBUG
  427.     printf("CGDistPointPoint exit\n");
  428. #endif /* DEBUG */
  429.  
  430.      return t;
  431. }
  432.  
  433. /*****************************************************************************
  434. *   Routine to create the plane from given 3 points. if two of the points    *
  435. * are the same it returns FALSE, otherwise (succesfull) returns TRUE.         *
  436. *****************************************************************************/
  437. int CGPlaneFrom3Points(PlaneType Plane, PointType Pt1, PointType Pt2,
  438.                             PointType Pt3)
  439. {
  440.     VectorType V1, V2;
  441.  
  442. #ifdef DEBUG
  443.     printf("CGPlaneFrom3Points entered\n");
  444. #endif /* DEBUG */
  445.  
  446.     if (PT_EQ(Pt1, Pt2) || PT_EQ(Pt2, Pt3) || PT_EQ(Pt1, Pt3)) return FALSE;
  447.  
  448.     PT_SUB(V1, Pt2, Pt1);
  449.     PT_SUB(V2, Pt3, Pt2);
  450.     VecCrossProd(Plane, V1, V2);
  451.     PT_NORMALIZE(Plane);
  452.  
  453.     Plane[3] = -DOT_PROD(Plane, Pt1);
  454.  
  455. #ifdef DEBUG
  456.     printf("CGPlaneFrom3Points exit\n");
  457. #endif /* DEBUG */
  458.  
  459.     return TRUE;
  460. }
  461.  
  462. /*****************************************************************************
  463. *   Routine to calc the closest 3d point to a given 3d line. the line is     *
  464. * given as a point on it (Pl) and vector (Vl).                               *
  465. *****************************************************************************/
  466. void CGPointFromPointLine(PointType Point, PointType Pl, PointType Vl,
  467.                             PointType ClosestPoint)
  468. {
  469.     int i;
  470.     PointType V1, V2;
  471.     RealType CosAlfa, VecMag;
  472.  
  473. #ifdef DEBUG
  474.     printf("CGPointFromLinePlane entered\n");
  475. #endif /* DEBUG */
  476.  
  477.     for (i=0; i<3; i++) {
  478.         V1[i] = Point[i] - Pl[i];
  479.         V2[i] = Vl[i];
  480.     }
  481.     VecMag = VecLength(V1);
  482.     VecNormalize(V1); /* Normalized vector from Point to a point on line Pl. */
  483.     VecNormalize(V2);                   /* Normalized line direction vector. */
  484.  
  485.     CosAlfa = VecDotProd(V1, V2); /* Find the angle between the two vectors. */
  486.  
  487.     /* Find P1 - the closest point to Point on the line: */
  488.     for (i=0; i<3; i++) ClosestPoint[i] = Pl[i] + V2[i] * CosAlfa * VecMag;
  489.  
  490. #ifdef DEBUG
  491.     printf("CGPointFromLinePlane exit\n");
  492. #endif /* DEBUG */
  493. }
  494.  
  495. /*****************************************************************************
  496. *   Routine to calc the distance between 3d point and 3d line. the line is   *
  497. * given as a point on it (Pl) and vector (Vl).                               *
  498. *****************************************************************************/
  499. RealType CGDistPointLine(PointType Point, PointType Pl, PointType Vl)
  500. {
  501.     RealType t;
  502.     PointType Ptemp;
  503.  
  504. #ifdef DEBUG
  505.     printf("CGDistPointLine entered\n");
  506. #endif /* DEBUG */
  507.  
  508.     CGPointFromPointLine(Point, Pl, Vl, Ptemp);/* Find closest point on line.*/
  509.     t = CGDistPointPoint(Point, Ptemp);
  510.  
  511. #ifdef DEBUG
  512.     printf("CGDistPointLine exit\n");
  513. #endif /* DEBUG */
  514.  
  515.     return t;
  516. }
  517.  
  518. /*****************************************************************************
  519. *   Routine to calc the distance between a Point and a Plane. The Plane is   *
  520. * defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 elements vector.    *
  521. *****************************************************************************/
  522. RealType CGDistPointPlane(PointType Point, PlaneType Plane)
  523. {
  524.     RealType t;
  525.  
  526. #ifdef DEBUG
  527.     printf("CGDistPointPlane entered\n");
  528. #endif /* DEBUG */
  529.  
  530.     t = ((Plane[0] * Point [0] +
  531.       Plane[1] * Point [1] +
  532.       Plane[2] * Point [2] +
  533.       Plane[3]) /
  534.      sqrt(SQR(Plane[0]) + SQR(Plane[1]) + SQR(Plane[2])));
  535.  
  536. #ifdef DEBUG
  537.     printf("CGDistPointPlane exit\n");
  538. #endif /* DEBUG */
  539.  
  540.     return t;
  541. }
  542.  
  543. /*****************************************************************************
  544. *   Routine to find the intersection point of a line and a plane (if any)    *
  545. *   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
  546. * elements vector. The line is define via a point on it Pl and a direction   *
  547. * vector Vl. Return TRUE only if such point exists.                          *
  548. *****************************************************************************/
  549. int CGPointFromLinePlane(PointType Pl, PointType Vl, PlaneType Plane,
  550.                     PointType InterPoint, RealType *t)
  551. {
  552.     int i;
  553.     RealType DotProd;
  554.  
  555. #ifdef DEBUG
  556.     printf("CGPointFromLinePlane entered\n");
  557. #endif /* DEBUG */
  558.  
  559.     /* Check to see if they are vertical - no intersection at all! */
  560.     DotProd = VecDotProd(Vl, Plane);
  561.     if (ABS(DotProd) < EPSILON) return FALSE;
  562.  
  563.     /* Else find t in line such that the plane equation plane is satisfied: */
  564.     *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
  565.                                 / DotProd;
  566.  
  567.     /* And so find the intersection point which is at that t: */
  568.     for (i=0; i<3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
  569.  
  570. #ifdef DEBUG
  571.     printf("CGPointFromLinePlane exit\n");
  572. #endif /* DEBUG */
  573.  
  574.     return TRUE;
  575. }
  576.  
  577. /*****************************************************************************
  578. *   Routine to find the intersection point of a line and a plane (if any)    *
  579. *   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
  580. * elements vector. The line is define via a point on it Pl and a direction   *
  581. * vector Vl: Line == Pl + Vl * t, where t is the line parameter.         *
  582. *   Return TRUE only if such point exists, in the t parameter range [0..1].  *
  583. *****************************************************************************/
  584. int CGPointFromLinePlane01(PointType Pl, PointType Vl, PlaneType Plane,
  585.                     PointType InterPoint, RealType *t)
  586. {
  587.     int i;
  588.     RealType DotProd;
  589.  
  590. #ifdef DEBUG
  591.     printf("CGPointFromLinePlane01 entered\n");
  592. #endif /* DEBUG */
  593.  
  594.     /* Check to see if they are vertical - no intersection at all! */
  595.     DotProd = VecDotProd(Vl, Plane);
  596.     if (ABS(DotProd) < EPSILON) return FALSE;
  597.  
  598.     /* Else find t in line such that the plane equation plane is satisfied: */
  599.     *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
  600.                                 / DotProd;
  601.  
  602.     if ((*t < 0.0 && !APX_EQ(*t, 0.0)) ||      /* Not in parameter range. */
  603.     (*t > 1.0 && !APX_EQ(*t, 1.0))) return FALSE;
  604.  
  605.     /* And so find the intersection point which is at that t : */
  606.     for (i=0; i<3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
  607.  
  608. #ifdef DEBUG
  609.     printf("CGPointFromLinePlane01 exit\n");
  610. #endif /* DEBUG */
  611.  
  612.     return TRUE;
  613. }
  614.  
  615. /*****************************************************************************
  616. *   Routine to find the two point Pti on the lines (Pli, Vli) ,   i = 1, 2   *
  617. * with the minimal euclidian distance between them. In other words the       *
  618. * distance between Pt1 and Pt2 is defined as distance between the two lines. *
  619. *   The two points are calculated using the fact that if V = (Vl1 cross Vl2) *
  620. * then these two points are the intersection point between the following:    *
  621. * point 1 - a plane (defined by V and line1) and the line line2.             *
  622. * point 2 - a plane (defined by V and line2) and the line line1.             *
  623. *   This function returns TRUE iff the two lines are not parallel!           *
  624. *****************************************************************************/
  625. int CG2PointsFromLineLine(PointType Pl1, PointType Vl1,
  626.               PointType Pl2, PointType Vl2,
  627.               PointType Pt1, RealType *t1,
  628.               PointType Pt2, RealType *t2)
  629. {
  630.     int i;
  631.     PointType Vtemp;
  632.     PlaneType Plane1, Plane2;
  633.  
  634. #ifdef DEBUG
  635.     printf("CG2PointsFromLineLine entered\n");
  636. #endif /* DEBUG */
  637.  
  638.     VecCrossProd(Vtemp, Vl1, Vl2);     /* Check to see if they are parallel. */
  639.     if (VecLength(Vtemp) < EPSILON) {
  640.         for (i=0; i<3; i++) Pt1[i] = Pl1[i]; /* Pick point on line1 and find */
  641.     CGPointFromPointLine(Pl1, Pl2, Vl2, Pt2); /* closest point on line2. */
  642.         return FALSE;
  643.     }
  644.  
  645.     /* Define the two planes: 1) Vl1, Pl1, Vtemp and 2) Vl2, Pl2, Vtemp         */
  646.     /* Note this sets the first 3 elements A, B, C out of the 4...         */
  647.     VecCrossProd(Plane1, Vl1, Vtemp);           /* Find the A, B, C coef.'s. */
  648.     VecNormalize(Plane1);
  649.     VecCrossProd(Plane2, Vl2, Vtemp);           /* Find the A, B, C coef.'s. */
  650.     VecNormalize(Plane2);
  651.  
  652.     /* and now use a point on the plane to find the 4th coef. D: */
  653.     Plane1[3] = (-VecDotProd(Plane1, Pl1)); /* Note VecDotProd uses only the */
  654.     Plane2[3] = (-VecDotProd(Plane2, Pl2)); /* first three elements in vec.  */
  655.  
  656.     /* Thats it! now we should solve for the intersection point between a    */
  657.     /* line and a plane but we already familiar with this problem...         */
  658.     i = CGPointFromLinePlane(Pl1, Vl1, Plane2, Pt1, t1) &&
  659.     CGPointFromLinePlane(Pl2, Vl2, Plane1, Pt2, t2);
  660.  
  661. #ifdef DEBUG
  662.     printf("CG2PointsFromLineLine exit\n");
  663. #endif /* DEBUG */
  664.  
  665.     return i;
  666. }
  667.  
  668. /*****************************************************************************
  669. *   Routine to find the distance between two lines (Pli, Vli) ,  i = 1, 2.   *
  670. *****************************************************************************/
  671. RealType CGDistLineLine(PointType Pl1, PointType Vl1,
  672.             PointType Pl2, PointType Vl2)
  673. {
  674.     RealType t1, t2;
  675.     PointType Ptemp1, Ptemp2;
  676.  
  677. #ifdef DEBUG
  678.     printf("CGDistLineLine entered\n");
  679. #endif /* DEBUG */
  680.  
  681.     CG2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, Ptemp1, &t1, Ptemp2, &t2);
  682.     t1 = CGDistPointPoint(Ptemp1, Ptemp2);
  683.  
  684. #ifdef DEBUG
  685.     printf("CGDistLineLine exit\n");
  686. #endif /* DEBUG */
  687.  
  688.     return t1;
  689. }
  690.  
  691. /*****************************************************************************
  692. *   Routine implements Jordan Theorem: Fire a ray from a given point and find*
  693. * number of intersections of ray with the polygon, excluding the given point *
  694. * Pt (start of ray) itself, if on polygon boundary. The ray is fired in +X   *
  695. * (Axes == 0) or +Y (Axes == 1). And only the X/Y coordinates of the polygon *
  696. * are taken into account, i.e. the orthogonal projection of the polygon on   *
  697. * a X/Y parallel plane (equal to polygon itself if on X/Y parallel plane...) *
  698. *   Note that if the point is on polygon boundary, the ray should not be in  *
  699. * its edge direction                                 *
  700. *                                         *
  701. *   Algorithm:                                     *
  702. * 1. Set NumOfIntersection = 0;                             *
  703. *    Find vertex V not on Ray level and set AlgState to its level (below or  *
  704. *    above the ray level). If none goto 3                     *
  705. *    Mark VStart = V;                                 *
  706. * 2. 2.1. While State(V) == AlgState do                         *
  707. *        2.1.1. V = V -> Pnext;                         *
  708. *        2.1.2. If V == VStart goto 3                     *
  709. *      end;                                     *
  710. *      IntersectionMinX = INFINITY;                         *
  711. *    2.2. While State(V) == ON_RAY do                         *
  712. *        IntersectionMin = MIN(IntersectionMin, V -> Pt[Axes]);         *
  713. *        V = V -> Pnext;                             *
  714. *         end;                                     *
  715. *    2.3. If State(V) != AlgState do                         *
  716. *        2.3.1. Find the intersection point between polygon edge      *
  717. *               Vlast, V and the Ray and update IntersectionMin if    *
  718. *               lower than it.                         *
  719. *        2.3.2. If IntersectionMin is greater than Pt[Axes] increase  *
  720. *               the NumOfIntersection counter by 1.             *
  721. *      end;                                     *
  722. *    2.4. AlgState = State(V);                             *
  723. *    2.5. goto 2.1.                                 *
  724. * 3. return NumOfIntersection;                             *
  725. *                                         *
  726. *****************************************************************************/
  727. int CGPolygonRayInter(PolygonStruct *Pl, PointType PtRay, int RayAxes)
  728. {
  729.     int NewState, AlgState, RayOtherAxes, NumOfInter = 0, Quit = FALSE;
  730.     RealType InterMin, Inter, t;
  731.     VertexStruct *V, *VStart, *VLast;
  732.  
  733.     RayOtherAxes = (RayAxes == 1 ? 0 : 1);     /* Other dir: X -> Y, Y -> X. */
  734.  
  735.     /* Stage 1 - find a vertex below the ray level: */
  736.     V = VStart = Pl -> V;
  737.     do {
  738.     if ((AlgState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
  739.                             != ON_RAY) break;
  740.     V = V -> Pnext;
  741.     }
  742.     while (V != VStart && V != NULL);
  743.     if (AlgState == ON_RAY) return 0;
  744.     VStart = V; /* Vertex Below Ray level */
  745.  
  746.     /* Stage 2 - scan the vertices and count number of intersections. */
  747.     while (!Quit) {
  748.     /* Stage 2.1. : */
  749.     while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == AlgState) {
  750.         VLast = V;
  751.         V = V -> Pnext;
  752.         if (V == VStart) {
  753.         Quit = TRUE;
  754.         break;
  755.         }
  756.     }
  757.     InterMin = INFINITY;
  758.  
  759.     /* Stage 2.2. : */
  760.     while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == ON_RAY) {
  761.         InterMin = MIN(InterMin, V -> Pt[RayAxes]);
  762.         VLast = V;
  763.         V = V -> Pnext;
  764.         if (V == VStart) Quit = TRUE;
  765.     }
  766.  
  767.     /* Stage 2.3. : */
  768.     if ((NewState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
  769.                                 != AlgState) {
  770.         /* Stage 2.3.1 Intersection with ray is in middle of edge: */
  771.         t = (PtRay[RayOtherAxes] - V -> Pt[RayOtherAxes]) /
  772.         (VLast -> Pt[RayOtherAxes] - V -> Pt[RayOtherAxes]);
  773.         Inter = VLast -> Pt[RayAxes] * t + V -> Pt[RayAxes] * (1.0 - t);
  774.         InterMin = MIN(InterMin, Inter);
  775.  
  776.         /* Stage 2.3.2. comp. with ray base and inc. # of inter if above.*/
  777.         if (InterMin > PtRay[RayAxes] &&
  778.         !APX_EQ(InterMin, PtRay[RayAxes])) NumOfInter++;
  779.     }
  780.  
  781.     AlgState = NewState;
  782.     }
  783.  
  784.     return NumOfInter;
  785. }
  786.  
  787. /*****************************************************************************
  788. *   Routine to return the relation between the ray level and a given point,  *
  789. * to be used in the CGPolygonRayInter routine above.                 *
  790. *****************************************************************************/
  791. static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes)
  792. {
  793.     if (APX_EQ(PtRay[Axes], Pt[Axes])) return ON_RAY;
  794.     else if (PtRay[Axes] < Pt[Axes]) return ABOVE_RAY;
  795.      else return BELOW_RAY;
  796. }
  797.  
  798.