/*****************************************************************************
* "Irit" - the 3d polygonal solid modeller. *
* *
* Written by: Gershon Elber Ver 0.2, Mar. 1990 *
******************************************************************************
* Routines to generate transformation matrices for Translation, Rotation *
* and Scaling for 3D universe. *
* Routines to manipulate 3D vectors. *
* And some computational geometry routines. *
*****************************************************************************/
#include <math.h>
#include <stdio.h>
#include "program.h"
#include "allocate.h"
#include "convex.h"
#include "geomat3d.h"
#include "objects.h"
#include "primitiv.h"
#include "windows.h"
#include "graphgen.h"
/* #define DEBUG Print information on entry and exit of routines. */
static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes);
/*****************************************************************************
* Routine to generate a 4*4 unit matrix: *
*****************************************************************************/
void MatGenUnitMat(MatrixType Mat)
{
int i, j;
for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
if (i == j)
Mat[i][j] = 1.0;
else
Mat[i][j] = 0.0;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts. *
*****************************************************************************/
void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[3][0] = Tx;
Mat[3][1] = Ty;
Mat[3][2] = Tz;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts. *
*****************************************************************************/
void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = Sx;
Mat[1][1] = Sy;
Mat[2][2] = Sz;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void MatGenMatRotX1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = (RealType) cos((double) Teta);
STeta = (RealType) sin((double) Teta);
MatGenMatRotX(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta. *
*****************************************************************************/
void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[1][1] = CosTeta;
Mat[1][2] = SinTeta;
Mat[2][1] = -SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void MatGenMatRotY1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = (RealType) cos((double) Teta);
STeta = (RealType) sin((double) Teta);
MatGenMatRotY(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta. *
*****************************************************************************/
void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = CosTeta;
Mat[0][2] = -SinTeta;
Mat[2][0] = SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = (RealType) cos((double) Teta);
STeta = (RealType) sin((double) Teta);
MatGenMatRotZ(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta. *
*****************************************************************************/
void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix, */
Mat[0][0] = CosTeta;
Mat[0][1] = SinTeta;
Mat[1][0] = -SinTeta;
Mat[1][1] = CosTeta;
}
/*****************************************************************************
* Routine to multiply 2 4by4 matrices: *
* Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end! *
*****************************************************************************/
void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j, k;
MatrixType MatResTemp;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
MatResTemp[i][j] = 0;
for (k = 0; k < 4; k++)
MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
}
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = MatResTemp[i][j];
}
/*****************************************************************************
* Routine to add 2 4by4 matrices: *
* Note MatRes might be one of Mat1 or Mat2. *
*****************************************************************************/
void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
}
/*****************************************************************************
* Routine to subtract 2 4by4 matrices: *
* Note MatRes might be one of Mat1 or Mat2. *
*****************************************************************************/
void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
}
/*****************************************************************************
* Routine to scale a 4by4 matrix by constant: *
* Note MatRes might be Mat. *
*****************************************************************************/
void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat[i][j] * (*Scale);
}
/*****************************************************************************
* Routine to multiply Vector by 4by4 matrix: *
* The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1 *
* Note VRes might be Vec as it is only updated in the end. *
*****************************************************************************/
void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
{
int i, j;
RealType
CalcW = Mat[3][3];
VectorType VTemp;
for (i = 0; i < 3; i++) {
VTemp[i] = Mat[3][i]; /* Initiate it with the weight factor. */
for (j = 0; j < 3; j++) VTemp[i] += Vec[j] * Mat[j][i];
}
for (i = 0; i < 3; i++) CalcW += Vec[i] * Mat[i][3];
if (CalcW == 0) CalcW = 1/INFINITY;
for (i = 0; i < 3; i++) VRes[i] = VTemp[i]/CalcW;
}
/*****************************************************************************
* Procedure to calculate the INVERSE of a given matrix M which is not *
* modified. The matrix is assumed to be 4 by 4 (transformation matrix). *
* Return TRUE if inverted matrix (InvM) do exists. *
*****************************************************************************/
int MatInverseMatrix(MatrixType M, MatrixType InvM)
{
MatrixType A;
int i, j, k;
RealType V;
MAT_COPY(A, M); /* Prepare temporary copy of M in A. */
MatGenUnitMat(InvM); /* Make it unit matrix. */
for (i = 0; i < 4; i++) {
V = A[i][i]; /* Find the new pivot. */
k = i;
for (j = i + 1; j < 4; j++)
if (ABS(A[j][i]) > ABS(V)) {
/* Find maximum on col i, row i+1..n */
V = A[j][i];
k = j;
}
j = k;
if (i != j)
for (k = 0; k < 4; k++) {
SWAP(RealType, A[i][k], A[j][k]);
SWAP(RealType, InvM[i][k], InvM[j][k]);
}
for (j = i + 1; j < 4; j++) { /* Eliminate col i from row i+1..n. */
V = A[j][i] / A[i][i];
for (k = 0; k < 4; k++) {
A[j][k] -= V * A[i][k];
InvM[j][k] -= V * InvM[i][k];
}
}
}
for (i = 3; i >= 0; i--) { /* Back Substitution. */
if (A[i][i] == 0) return FALSE; /* Error. */
for (j = 0; j < i; j++) { /* Eliminate col i from row 1..i-1. */
V = A[j][i] / A[i][i];
for (k = 0; k < 4; k++) {
/* A[j][k] -= V * A[i][k]; */
InvM[j][k] -= V * InvM[i][k];
}
}
}
for (i = 0; i < 4; i++) /* Normalize the inverse Matrix. */
for (j = 0; j < 4; j++)
InvM[i][j] /= A[i][i];
return TRUE;
}
/*****************************************************************************
* Routine to copy one vector to another: *
*****************************************************************************/
void VecCopy(VectorType Vdst, VectorType Vsrc)
{
int i;
for (i = 0; i < 3; i++)
Vdst[i] = Vsrc[i];
}
/*****************************************************************************
* Routine to normalize the vector length to a unit length: *
*****************************************************************************/
void VecNormalize(VectorType V)
{
int i;
RealType Len;
Len = VecLength(V);
if (Len > 0.0)
for (i = 0; i < 3; i++) {
V[i] /= Len;
if (ABS(V[i]) < EPSILON) V[i] = 0.0;
}
}
/*****************************************************************************
* Routine to calculate the magnitude (length) of a given 3D vector: *
*****************************************************************************/
RealType VecLength(VectorType V)
{
return sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
}
/*****************************************************************************
* Routine to calculate the cross product of two vectors: *
* Note Vres might be the same as V1 or V2 ! *
*****************************************************************************/
void VecCrossProd(VectorType Vres, VectorType V1, VectorType V2)
{
VectorType Vtemp;
Vtemp[0] = V1[1] * V2[2] - V2[1] * V1[2];
Vtemp[1] = V1[2] * V2[0] - V2[2] * V1[0];
Vtemp[2] = V1[0] * V2[1] - V2[0] * V1[1];
VecCopy(Vres, Vtemp);
}
/*****************************************************************************
* Routine to calculate the dot product of two vectors: *
*****************************************************************************/
RealType VecDotProd(VectorType V1, VectorType V2)
{
return V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2];
}
/*****************************************************************************
* Routine to generate rotation object around the X axis in Degree degrees: *
*****************************************************************************/
ObjectStruct *GenMatObjectRotX(RealType *Degree)
{
MatrixType Mat;
MatGenMatRotX1(DEG2RAD(*Degree), Mat); /* Generate the trans. matrix. */
return GenMatObject("", Mat, NULL);
}
/*****************************************************************************
* Routine to generate rotation object around the Y axis in Degree degrees: *
*****************************************************************************/
ObjectStruct *GenMatObjectRotY(RealType *Degree)
{
MatrixType Mat;
MatGenMatRotY1(DEG2RAD(*Degree), Mat); /* Generate the trans. matrix. */
return GenMatObject("", Mat, NULL);
}
/*****************************************************************************
* Routine to generate rotation object around the Z axis in Degree degrees: *
*****************************************************************************/
ObjectStruct *GenMatObjectRotZ(RealType *Degree)
{
MatrixType Mat;
MatGenMatRotZ1(DEG2RAD(*Degree), Mat); /* Generate the trans. matrix. */
return GenMatObject("", Mat, NULL);
}
/*****************************************************************************
* Routine to generate translation object according to XYZ vector Vec. *
*****************************************************************************/
ObjectStruct * GenMatObjectTrans(VectorType Vec)
{
MatrixType Mat;
/* Generate the transformation matrix */
MatGenMatTrans(Vec[0], Vec[1], Vec[2], Mat);
return GenMatObject("", Mat, NULL);
}
/*****************************************************************************
* Routine to scale an object according to XYZ scaling vector Vec. *
*****************************************************************************/
ObjectStruct * GenMatObjectScale(VectorType Vec)
{
MatrixType Mat;
/* Generate the transformation matrix */
MatGenMatScale(Vec[0], Vec[1], Vec[2], Mat);
return GenMatObject("", Mat, NULL);
}
/*****************************************************************************
* Routine to transform an object according to the transformation matrix. *
*****************************************************************************/
ObjectStruct *TransformObject(ObjectStruct *PObj, MatrixType Mat)
{
int i, j;
ObjectStruct *NewPObj;
CagdMatStruct CagdMat;
if (IS_POLY_OBJ(PObj)) {
int IsPolygon = !IS_POLYLINE_OBJ(PObj);
VectorType Pin, PTemp;
PolygonStruct *Pl;
VertexStruct *V, *VFirst;
NewPObj = CopyObject(NULL, PObj, FALSE);
Pl = NewPObj -> U.Pl.P;
while (Pl != NULL) {
V = VFirst = Pl -> V;
PT_ADD(Pin, V -> Pt, Pl -> Plane); /* Prepare point IN side. */
MatMultVecby4by4(Pin, Pin, Mat); /* Transformed relative to new. */
do {
if (IsPolygon) PT_ADD(PTemp, V -> Pt, V -> Normal);
MatMultVecby4by4(V -> Pt, V -> Pt, Mat); /* Update position. */
if (IsPolygon) {
MatMultVecby4by4(PTemp, PTemp, Mat); /* Update normal. */
PT_SUB(V -> Normal, PTemp, V -> Pt);
PT_NORMALIZE(V -> Normal);
}
V = V -> Pnext;
}
while (V != VFirst && V != NULL); /* VList is circular! */
if (IsPolygon)
UpdatePolyPlane(Pl, Pin);/* Update plane eqn. using IN point.*/
Pl = Pl -> Pnext;
}
}
else if (IS_SRF_OBJ(PObj)) {
NewPObj = CopyObject(NULL, PObj, FALSE);
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
CagdMat.Mat[i][j] = Mat[i][j];
CagdSrfMatTransform(NewPObj -> U.Srf.Srf, &CagdMat);
}
else if (IS_CRV_OBJ(PObj)) {
NewPObj = CopyObject(NULL, PObj, FALSE);
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
CagdMat.Mat[i][j] = Mat[i][j];
CagdCrvMatTransform(NewPObj -> U.Crv.Crv, &CagdMat);
}
else if (IS_OLST_OBJ(PObj)) {
NewPObj = AllocObject("", OBJ_LIST_OBJ, NULL);
for (i = 0; PObj -> U.PObjList[i] != NULL && i < MAX_OBJ_LIST; i++) {
NewPObj -> U.PObjList[i] = TransformObject(PObj -> U.PObjList[i],
Mat);
}
NewPObj -> U.PObjList[i] = NULL;
}
else {
WndwInputWindowPutStr("None transformable object ignored.");
NewPObj = CopyObject(NULL, PObj, FALSE);
}
return NewPObj;
}
/*****************************************************************************
* Routine to calc the distance between two 3d points *
*****************************************************************************/
RealType CGDistPointPoint(PointType P1, PointType P2)
{
RealType t;
#ifdef DEBUG
printf("CGDistPointPoint entered\n");
#endif /* DEBUG */
t = sqrt(SQR(P2[0]-P1[0]) + SQR(P2[1]-P1[1]) + SQR(P2[2]-P1[2]));
#ifdef DEBUG
printf("CGDistPointPoint exit\n");
#endif /* DEBUG */
return t;
}
/*****************************************************************************
* Routine to create the plane from given 3 points. if two of the points *
* are the same it returns FALSE, otherwise (succesfull) returns TRUE. *
*****************************************************************************/
int CGPlaneFrom3Points(PlaneType Plane, PointType Pt1, PointType Pt2,
PointType Pt3)
{
VectorType V1, V2;
#ifdef DEBUG
printf("CGPlaneFrom3Points entered\n");
#endif /* DEBUG */
if (PT_EQ(Pt1, Pt2) || PT_EQ(Pt2, Pt3) || PT_EQ(Pt1, Pt3)) return FALSE;
PT_SUB(V1, Pt2, Pt1);
PT_SUB(V2, Pt3, Pt2);
VecCrossProd(Plane, V1, V2);
PT_NORMALIZE(Plane);
Plane[3] = -DOT_PROD(Plane, Pt1);
#ifdef DEBUG
printf("CGPlaneFrom3Points exit\n");
#endif /* DEBUG */
return TRUE;
}
/*****************************************************************************
* Routine to calc the closest 3d point to a given 3d line. the line is *
* given as a point on it (Pl) and vector (Vl). *
*****************************************************************************/
void CGPointFromPointLine(PointType Point, PointType Pl, PointType Vl,
PointType ClosestPoint)
{
int i;
PointType V1, V2;
RealType CosAlfa, VecMag;
#ifdef DEBUG
printf("CGPointFromLinePlane entered\n");
#endif /* DEBUG */
for (i = 0; i < 3; i++) {
V1[i] = Point[i] - Pl[i];
V2[i] = Vl[i];
}
VecMag = VecLength(V1);
VecNormalize(V1); /* Normalized vector from Point to a point on line Pl. */
VecNormalize(V2); /* Normalized line direction vector. */
CosAlfa = VecDotProd(V1, V2); /* Find the angle between the two vectors. */
/* Find P1 - the closest point to Point on the line: */
for (i = 0; i < 3; i++)
ClosestPoint[i] = Pl[i] + V2[i] * CosAlfa * VecMag;
#ifdef DEBUG
printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG */
}
/*****************************************************************************
* Routine to calc the distance between 3d point and 3d line. the line is *
* given as a point on it (Pl) and vector (Vl). *
*****************************************************************************/
RealType CGDistPointLine(PointType Point, PointType Pl, PointType Vl)
{
RealType t;
PointType Ptemp;
#ifdef DEBUG
printf("CGDistPointLine entered\n");
#endif /* DEBUG */
CGPointFromPointLine(Point, Pl, Vl, Ptemp);/* Find closest point on line.*/
t = CGDistPointPoint(Point, Ptemp);
#ifdef DEBUG
printf("CGDistPointLine exit\n");
#endif /* DEBUG */
return t;
}
/*****************************************************************************
* Routine to calc the distance between a Point and a Plane. The Plane is *
* defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 elements vector. *
*****************************************************************************/
RealType CGDistPointPlane(PointType Point, PlaneType Plane)
{
RealType t;
#ifdef DEBUG
printf("CGDistPointPlane entered\n");
#endif /* DEBUG */
t = ((Plane[0] * Point [0] +
Plane[1] * Point [1] +
Plane[2] * Point [2] +
Plane[3]) /
sqrt(SQR(Plane[0]) + SQR(Plane[1]) + SQR(Plane[2])));
#ifdef DEBUG
printf("CGDistPointPlane exit\n");
#endif /* DEBUG */
return t;
}
/*****************************************************************************
* Routine to find the intersection point of a line and a plane (if any) *
* The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 *
* elements vector. The line is define via a point on it Pl and a direction *
* vector Vl. Return TRUE only if such point exists. *
*****************************************************************************/
int CGPointFromLinePlane(PointType Pl, PointType Vl, PlaneType Plane,
PointType InterPoint, RealType *t)
{
int i;
RealType DotProd;
#ifdef DEBUG
printf("CGPointFromLinePlane entered\n");
#endif /* DEBUG */
/* Check to see if they are vertical - no intersection at all! */
DotProd = VecDotProd(Vl, Plane);
if (ABS(DotProd) < EPSILON) return FALSE;
/* Else find t in line such that the plane equation plane is satisfied: */
*t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
/ DotProd;
/* And so find the intersection point which is at that t: */
for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
#ifdef DEBUG
printf("CGPointFromLinePlane exit\n");
#endif /* DEBUG */
return TRUE;
}
/*****************************************************************************
* Routine to find the intersection point of a line and a plane (if any) *
* The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 *
* elements vector. The line is define via a point on it Pl and a direction *
* vector Vl: Line == Pl + Vl * t, where t is the line parameter. *
* Return TRUE only if such point exists, in the t parameter range [0..1]. *
*****************************************************************************/
int CGPointFromLinePlane01(PointType Pl, PointType Vl, PlaneType Plane,
PointType InterPoint, RealType *t)
{
int i;
RealType DotProd;
#ifdef DEBUG
printf("CGPointFromLinePlane01 entered\n");
#endif /* DEBUG */
/* Check to see if they are vertical - no intersection at all! */
DotProd = VecDotProd(Vl, Plane);
if (ABS(DotProd) < EPSILON) return FALSE;
/* Else find t in line such that the plane equation plane is satisfied: */
*t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
/ DotProd;
if ((*t < 0.0 && !APX_EQ(*t, 0.0)) || /* Not in parameter range. */
(*t > 1.0 && !APX_EQ(*t, 1.0))) return FALSE;
/* And so find the intersection point which is at that t : */
for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
#ifdef DEBUG
printf("CGPointFromLinePlane01 exit\n");
#endif /* DEBUG */
return TRUE;
}
/*****************************************************************************
* Routine to find the two point Pti on the lines (Pli, Vli) , i = 1, 2 *
* with the minimal euclidian distance between them. In other words the *
* distance between Pt1 and Pt2 is defined as distance between the two lines. *
* The two points are calculated using the fact that if V = (Vl1 cross Vl2) *
* then these two points are the intersection point between the following: *
* point 1 - a plane (defined by V and line1) and the line line2. *
* point 2 - a plane (defined by V and line2) and the line line1. *
* This function returns TRUE iff the two lines are not parallel! *
*****************************************************************************/
int CG2PointsFromLineLine(PointType Pl1, PointType Vl1,
PointType Pl2, PointType Vl2,
PointType Pt1, RealType *t1,
PointType Pt2, RealType *t2)
{
int i;
PointType Vtemp;
PlaneType Plane1, Plane2;
#ifdef DEBUG
printf("CG2PointsFromLineLine entered\n");
#endif /* DEBUG */
VecCrossProd(Vtemp, Vl1, Vl2); /* Check to see if they are parallel. */
if (VecLength(Vtemp) < EPSILON) {
for (i = 0; i < 3; i++)
Pt1[i] = Pl1[i]; /* Pick point on line1 and find */
CGPointFromPointLine(Pl1, Pl2, Vl2, Pt2); /* closest point on line2. */
return FALSE;
}
/* Define the two planes: 1) Vl1, Pl1, Vtemp and 2) Vl2, Pl2, Vtemp */
/* Note this sets the first 3 elements A, B, C out of the 4... */
VecCrossProd(Plane1, Vl1, Vtemp); /* Find the A, B, C coef.'s. */
VecNormalize(Plane1);
VecCrossProd(Plane2, Vl2, Vtemp); /* Find the A, B, C coef.'s. */
VecNormalize(Plane2);
/* and now use a point on the plane to find the 4th coef. D: */
Plane1[3] = (-VecDotProd(Plane1, Pl1)); /* Note VecDotProd uses only the */
Plane2[3] = (-VecDotProd(Plane2, Pl2)); /* first three elements in vec. */
/* Thats it! now we should solve for the intersection point between a */
/* line and a plane but we already familiar with this problem... */
i = CGPointFromLinePlane(Pl1, Vl1, Plane2, Pt1, t1) &&
CGPointFromLinePlane(Pl2, Vl2, Plane1, Pt2, t2);
#ifdef DEBUG
printf("CG2PointsFromLineLine exit\n");
#endif /* DEBUG */
return i;
}
/*****************************************************************************
* Routine to find the distance between two lines (Pli, Vli) , i = 1, 2. *
*****************************************************************************/
RealType CGDistLineLine(PointType Pl1, PointType Vl1,
PointType Pl2, PointType Vl2)
{
RealType t1, t2;
PointType Ptemp1, Ptemp2;
#ifdef DEBUG
printf("CGDistLineLine entered\n");
#endif /* DEBUG */
CG2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, Ptemp1, &t1, Ptemp2, &t2);
t1 = CGDistPointPoint(Ptemp1, Ptemp2);
#ifdef DEBUG
printf("CGDistLineLine exit\n");
#endif /* DEBUG */
return t1;
}
/*****************************************************************************
* Routine implements Jordan Theorem: Fire a ray from a given point and find*
* number of intersections of ray with the polygon, excluding the given point *
* Pt (start of ray) itself, if on polygon boundary. The ray is fired in +X *
* (Axes == 0) or +Y (Axes == 1). And only the X/Y coordinates of the polygon *
* are taken into account, i.e. the orthogonal projection of the polygon on *
* a X/Y parallel plane (equal to polygon itself if on X/Y parallel plane...) *
* Note that if the point is on polygon boundary, the ray should not be in *
* its edge direction *
* *
* Algorithm: *
* 1. Set NumOfIntersection = 0; *
* Find vertex V not on Ray level and set AlgState to its level (below or *
* above the ray level). If none goto 3 *
* Mark VStart = V; *
* 2. 2.1. While State(V) == AlgState do *
* 2.1.1. V = V -> Pnext; *
* 2.1.2. If V == VStart goto 3 *
* end; *
* IntersectionMinX = INFINITY; *
* 2.2. While State(V) == ON_RAY do *
* IntersectionMin = MIN(IntersectionMin, V -> Pt[Axes]); *
* V = V -> Pnext; *
* end; *
* 2.3. If State(V) != AlgState do *
* 2.3.1. Find the intersection point between polygon edge *
* Vlast, V and the Ray and update IntersectionMin if *
* lower than it. *
* 2.3.2. If IntersectionMin is greater than Pt[Axes] increase *
* the NumOfIntersection counter by 1. *
* end; *
* 2.4. AlgState = State(V); *
* 2.5. goto 2.1. *
* 3. return NumOfIntersection; *
* *
*****************************************************************************/
int CGPolygonRayInter(PolygonStruct *Pl, PointType PtRay, int RayAxes)
{
int NewState, AlgState, RayOtherAxes,
NumOfInter = 0,
Quit = FALSE;
RealType InterMin, Inter, t;
VertexStruct *V, *VStart, *VLast;
RayOtherAxes = (RayAxes == 1 ? 0 : 1); /* Other dir: X -> Y, Y -> X. */
/* Stage 1 - find a vertex below the ray level: */
V = VStart = Pl -> V;
do {
if ((AlgState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
!= ON_RAY) break;
V = V -> Pnext;
}
while (V != VStart && V != NULL);
if (AlgState == ON_RAY) return 0;
VStart = V; /* Vertex Below Ray level */
/* Stage 2 - scan the vertices and count number of intersections. */
while (!Quit) {
/* Stage 2.1. : */
while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == AlgState) {
VLast = V;
V = V -> Pnext;
if (V == VStart) {
Quit = TRUE;
break;
}
}
InterMin = INFINITY;
/* Stage 2.2. : */
while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == ON_RAY) {
InterMin = MIN(InterMin, V -> Pt[RayAxes]);
VLast = V;
V = V -> Pnext;
if (V == VStart) Quit = TRUE;
}
/* Stage 2.3. : */
if ((NewState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
!= AlgState) {
/* Stage 2.3.1 Intersection with ray is in middle of edge: */
t = (PtRay[RayOtherAxes] - V -> Pt[RayOtherAxes]) /
(VLast -> Pt[RayOtherAxes] - V -> Pt[RayOtherAxes]);
Inter = VLast -> Pt[RayAxes] * t + V -> Pt[RayAxes] * (1.0 - t);
InterMin = MIN(InterMin, Inter);
/* Stage 2.3.2. comp. with ray base and inc. # of inter if above.*/
if (InterMin > PtRay[RayAxes] &&
!APX_EQ(InterMin, PtRay[RayAxes])) NumOfInter++;
}
AlgState = NewState;
}
return NumOfInter;
}
/*****************************************************************************
* Routine to return the relation between the ray level and a given point, *
* to be used in the CGPolygonRayInter routine above. *
*****************************************************************************/
static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes)
{
if (APX_EQ(PtRay[Axes], Pt[Axes]))
return ON_RAY;
else if (PtRay[Axes] < Pt[Axes])
return ABOVE_RAY;
else
return BELOW_RAY;
}
/*****************************************************************************
* Same as CGPolygonRayInter but for arbitrary oriented polygon. *
* The polygon (and the point) is first rotated to a XY parallel plane. *
*****************************************************************************/
int CGPolygonRayInter3D(PolygonStruct *Pl, PointType PtRay, int RayAxes)
{
int i;
MatrixType RotMat;
VertexStruct *V, *VHead;
PolygonStruct *RotPl;
PointType RotPt;
/* Make a copy of original to work on. */
RotPl = AllocPolygon(1, Pl ->Tags, CopyVList(Pl -> V), NULL);
/* Bring the polygon to a XY parallel plane by rotation. */
GenRotateMatrix(RotMat, Pl -> Plane);
V = VHead = RotPl -> V;
do { /* Transform the polygon itself. */
MatMultVecby4by4(V -> Pt, V -> Pt, RotMat);
V = V -> Pnext;
}
while (V != NULL && V != VHead);
MatMultVecby4by4(RotPt, PtRay, RotMat);
i = CGPolygonRayInter(RotPl, RotPt, RayAxes);
MyFree((char *) RotPl, ALLOC_POLYGON);
return i;
}
