/****************************************************************************** * Geom_Bsc.c - basic geometry. * ******************************************************************************* * (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 "geom_loc.h" #ifdef DEBUG /* Print information on entry and exit of routines. */ IRIT_SET_DEBUG_PARAMETER(_DebugGMEntryExit, FALSE); #endif /* DEBUG */ #define QUART(x) (SQR(SQR((RealType) (x)))) #define SQRT3(x) (pow(FABS((RealType) (x)), 1.0 / 3.0) * SIGN(x)) #define GM_COLLINEAR_EPS 1e-10 #define GM_QUARTIC_SOLVE_EPS 1e-10 STATIC_DATA RealType GMBasicEps = IRIT_UEPS, GMBasicColinEps = GM_COLLINEAR_EPS; static int GMPointRayRelation(PointType Pt, PointType PtRay, int Axes); static RealType GMSolveApplyNewton(RealType Root, RealType A, RealType B, RealType C, RealType D); /***************************************************************************** * DESCRIPTION: M * Sets the epsilon to use in basic geometry processing. M * * * PARAMETERS: M * Eps: New epsilon to use. M * * * RETURN VALUE: M * RealType: Old epsilon value. M * * * KEYWORDS: M * GMBasicSetEps M *****************************************************************************/ RealType GMBasicSetEps(RealType Eps) { RealType OldVal = GMBasicEps; GMBasicColinEps = GM_COLLINEAR_EPS * Eps / IRIT_UEPS; GMBasicEps = Eps; return OldVal; } /***************************************************************************** * 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) { RealType Len; Len = VEC_LENGTH(V); if (Len > 0.0) { Len = 1.0 / Len; VEC_SCALE(V, Len); } } /***************************************************************************** * 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 * Compute the angle between two planar vectors V1 and V2. Angle is zero M * if V2 is in the exact same direction as V1, negative if V2 turns right and M * positive if V2 turns left. Angle is returned in degrees in the domain of M * (-180, +180]. Only the XY coefficients of V1 and V2 are considered. M * * * PARAMETERS: M * V1, V2: Planar vectors to compute their relative angle, in degrees. M * Normalize: TRUE if vectors need normalization first, FALSE if unit size. M * * * RETURN VALUE: M * RealType: Angle between V1 and V2, in degree. M * * * KEYWORDS: M * GMPlanarVecVecAngle M *****************************************************************************/ RealType GMPlanarVecVecAngle(VectorType V1, VectorType V2, int Normalize) { RealType *VP1, *VP2, DProd, CProd; VectorType V1Tmp, V2Tmp; if (Normalize) { VEC2D_COPY(V1Tmp, V1); VEC2D_NORMALIZE(V1Tmp); VP1 = V1Tmp; VEC2D_COPY(V2Tmp, V2); VEC2D_NORMALIZE(V2Tmp); VP2 = V2Tmp; } else { VP1 = V1; VP2 = V2; } DProd = DOT_PROD_2D(VP1, VP2); CProd = CROSS_PROD_2D(VP1, VP2); if (CProd == 0) return DProd > 0 ? 0.0 : 180.0; else if (CProd > 0.0) return RAD2DEG(acos(DProd)); else return -RAD2DEG(acos(DProd)); } /***************************************************************************** * DESCRIPTION: M * Verifies the collinearity of three points. M * * * PARAMETERS: M * Pt1, Pt2, Pt3: Three points to verify for collinearity. M * * * RETURN VALUE: M * int: TRUE if collinear, FALSE otherwise. M * * * SEE ALSO: M * GMCollinear3PtsInside M * * * KEYWORDS: M * GMCollinear3Pts, collinearity M *****************************************************************************/ int GMCollinear3Pts(PointType Pt1, PointType Pt2, PointType Pt3) { VectorType V1, V2, V3; RealType L1Sqr, L2Sqr; PT_SUB(V1, Pt1, Pt2); PT_SUB(V2, Pt2, Pt3); L1Sqr = PT_SQR_LENGTH(V1); L2Sqr = PT_SQR_LENGTH(V2); if (L1Sqr < SQR(GMBasicColinEps) || L2Sqr < SQR(GMBasicColinEps)) return TRUE; CROSS_PROD(V3, V1, V2); return PT_SQR_LENGTH(V3) < (L1Sqr * L2Sqr) * SQR(GMBasicColinEps); } /***************************************************************************** * DESCRIPTION: M * Computes an orthogonal vector in R^3 to the given vector. M * * * PARAMETERS: M * V: Input vector to find an orthogonal vector for. M * OV: Output newly computed orthogonal (possibly unit) vector to V, in R^3.M * UnitLen: If TRUE, normalize the outpt vector. M * * * RETURN VALUE: M * int: M * * * KEYWORDS: M * GMOrthogonalVector M *****************************************************************************/ int GMOrthogonalVector(VectorType V, VectorType OV, int UnitLen) { VectorType U; VEC_RESET(U); /* Zeros the maximal coefficient in V. */ if (fabs(V[0]) <= fabs(V[1]) && fabs(V[0]) <= fabs(V[2])) { U[0] = 1.0; } else if (fabs(V[1]) <= fabs(V[0]) && fabs(V[1]) <= fabs(V[2])) { U[1] = 1.0; } else { U[2] = 1.0; } CROSS_PROD(OV, V, U); if (PT_EQ_ZERO(OV)) return FALSE; if (UnitLen) VEC_NORMALIZE(OV); return TRUE; } /***************************************************************************** * DESCRIPTION: M * Verifies the collinearity of three points and that Pt2 is in the line M * segment [Pt1, Pt3]. M * * * PARAMETERS: M * Pt1, Pt2, Pt3: Three points to verify for collinearity. M * * * RETURN VALUE: M * int: TRUE if collinear and in segment, FALSE otherwise. M * * * SEE ALSO: M * GMCollinear3Pts M * * * KEYWORDS: M * GMCollinear3PtsInside, collinearity M *****************************************************************************/ int GMCollinear3PtsInside(PointType Pt1, PointType Pt2, PointType Pt3) { VectorType V1, V2, V3; RealType L1Sqr, L2Sqr; PT_SUB(V1, Pt1, Pt2); PT_SUB(V2, Pt2, Pt3); L1Sqr = PT_SQR_LENGTH(V1); L2Sqr = PT_SQR_LENGTH(V2); if (L1Sqr < SQR(GMBasicColinEps) || L2Sqr < SQR(GMBasicColinEps)) return TRUE; if (DOT_PROD(V1, V2) < 0.0) return FALSE; CROSS_PROD(V3, V1, V2); return PT_SQR_LENGTH(V3) < (L1Sqr * L2Sqr) * SQR(GMBasicColinEps); } /***************************************************************************** * DESCRIPTION: M * Verifies the coplanarity of four points. M * * * PARAMETERS: M * Pt1, Pt2, Pt3, Pt4: Four points to verify for coplanarity. M * * * RETURN VALUE: M * int: TRUE if coplanar, FALSE otherwise. M * * * SEE ALSO: M * GMCollinear3Pts M * * * KEYWORDS: M * GMCoplanar4Pts, coplanarity M *****************************************************************************/ int GMCoplanar4Pts(PointType Pt1, PointType Pt2, PointType Pt3, PointType Pt4) { VectorType V1, V2, V3, V4; RealType L1Sqr, L2Sqr, L3Sqr, t; PT_SUB(V1, Pt1, Pt2); PT_SUB(V2, Pt1, Pt3); PT_SUB(V3, Pt1, Pt4); L1Sqr = PT_SQR_LENGTH(V1); L2Sqr = PT_SQR_LENGTH(V2); L3Sqr = PT_SQR_LENGTH(V3); if (L1Sqr < SQR(GMBasicColinEps) || L2Sqr < SQR(GMBasicColinEps) || L3Sqr < SQR(GMBasicColinEps)) return TRUE; CROSS_PROD(V4, V1, V2); t = FABS(DOT_PROD(V3, V4)); return SQR(t) < (L1Sqr * L2Sqr * L3Sqr) * SQR(GMBasicColinEps); } /***************************************************************************** * 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 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 * GMDistPointPoint, point point distance M *****************************************************************************/ RealType GMDistPointPoint(PointType P1, PointType P2) { RealType t; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointPoint entered\n")); #endif /* DEBUG */ t = sqrt(SQR(P2[0] - P1[0]) + SQR(P2[1] - P1[1]) + SQR(P2[2] - P1[2])); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointPoint exit\n")); #endif /* DEBUG */ return t; } /***************************************************************************** * DESCRIPTION: M * Computes the linear combination of V1 and V2 that yields V. It is M * assumed that the three vectors are coplanar. M * * * PARAMETERS: M * V1, V2: The two vectors that span the plane containing V. M * V: To compute lin. comb. "w[0] * V1 + w[1] * V2" for. M * w: The two scalar coefficients to be compuled. M * * * RETURN VALUE: M * int: TRUE if successful, FALSE otherwise. M * * * KEYWORDS: M * GMFindLinComb2Vecs M *****************************************************************************/ int GMFindLinComb2Vecs(VectorType V1, VectorType V2, VectorType V, RealType w[2]) { int Axis1, Axis2; RealType Desc; VectorType Vec; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMFindLinComb2Vecs entered\n")); #endif /* DEBUG */ /* Find out the two (out of the three) axes that affects the most. */ CROSS_PROD(Vec, V1, V2); if (Vec[0] < Vec[1] && Vec[0] < Vec[2]) { Axis1 = 1; Axis2 = 2; } else if (Vec[1] < Vec[0] && Vec[1] < Vec[2]) { Axis1 = 0; Axis2 = 2; } else { Axis1 = 0; Axis2 = 1; } /* Solve the 2x2 linear system. */ Desc = V1[Axis1] * V2[Axis2] - V1[Axis2] * V2[Axis1]; if (FABS(Desc) < GMBasicEps) return FALSE; w[0] = (V[Axis1] * V2[Axis2] - V[Axis2] * V2[Axis1]) / Desc; w[1] = (V1[Axis1] * V[Axis2] - V1[Axis2] * V[Axis1]) / Desc; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMFindLinComb2Vecs exit\n")); #endif /* DEBUG */ return TRUE; } /***************************************************************************** * DESCRIPTION: M * Routine to construct a line through 2 points, in the plane. If the M * points are the same it returns FALSE, otherwise (successful), TRUE. M * * * PARAMETERS: M * Line: To compute as Ax + By + C, such that A^2 + B^2 = 1. M * Pt1, Pt2: Two points to fit a line through. Only XY coordinates are M * considered. M * * * RETURN VALUE: M * int: TRUE if successful, FALSE otherwise. M * * * KEYWORDS: M * GMLineFrom2Points, line M *****************************************************************************/ int GMLineFrom2Points(LineType Line, PointType Pt1, PointType Pt2) { RealType Size; Line[0] = Pt2[1] - Pt1[1]; Line[1] = Pt1[0] - Pt2[0]; Line[2] = -Line[0] * Pt1[0] - Line[1] * Pt1[1]; if ((Size = sqrt(SQR(Line[0]) + SQR(Line[1]))) < GMBasicEps) return FALSE; else { Size = 1.0 / Size; Line[0] *= Size; Line[1] *= Size; Line[2] *= Size; return TRUE; } } /***************************************************************************** * DESCRIPTION: M * Computes a point on and the direction of the given line M * * * PARAMETERS: M * Line: To extract and point and a direction for. M * Pt: A point on line Line. M * Dir: The direction of line Line. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * GMPointVecFromLine M *****************************************************************************/ void GMPointVecFromLine(LineType Line, PointType Pt, VectorType Dir) { Pt[2] = Dir[2] = 0.0; if (FABS(Line[0]) > FABS(Line[1])) { Pt[0] = -Line[2] / Line[0]; Pt[1] = 0; } else { Pt[0] = 0; Pt[1] = -Line[2] / Line[1]; } Dir[0] = Line[1]; Dir[1] = -Line[0]; } /***************************************************************************** * 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 collinear 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 * GMPlaneFrom3Points, plane M *****************************************************************************/ int GMPlaneFrom3Points(PlaneType Plane, PointType Pt1, PointType Pt2, PointType Pt3) { VectorType V1, V2; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPlaneFrom3Points entered\n")); #endif /* DEBUG */ if (GMCollinear3Pts(Pt1, Pt2, Pt3)) return FALSE; VEC_SUB(V1, Pt2, Pt1); VEC_SUB(V2, Pt3, Pt2); CROSS_PROD(Plane, V1, V2); VEC_NORMALIZE(Plane); Plane[3] = -DOT_PROD(Plane, Pt1); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPlaneFrom3Points exit\n")); #endif /* DEBUG */ 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 * GMPointFromPointLine, point line distance M *****************************************************************************/ void GMPointFromPointLine(PointType Point, PointType Pl, PointType Vl, PointType ClosestPoint) { PointType V1; RealType CosAlpha, l; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromPointLine entered\n")); #endif /* DEBUG */ VEC_SUB(V1, Point, Pl); /* Compute the magnitude square of Vl. */ if ((l = VEC_SQR_LENGTH(Vl)) < SQR(IRIT_EPS)) l = SQR(IRIT_EPS); /* Find angle between the two vectors, including magnitudes of V1 & Vl. */ CosAlpha = DOT_PROD(V1, Vl) / l; /* Find P1 - the closest point to Point on the line: */ PT_SCALE_AND_ADD(ClosestPoint, Pl, Vl, CosAlpha); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromPointLine exit\n")); #endif /* DEBUG */ } /***************************************************************************** * 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 * GMDistPointLine, point line distance M *****************************************************************************/ RealType GMDistPointLine(PointType Point, PointType Pl, PointType Vl) { RealType t; PointType Ptemp; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointLine entered\n")); #endif /* DEBUG */ GMPointFromPointLine(Point, Pl, Vl, Ptemp);/* Find closest point on line.*/ t = PT_PT_DIST(Point, Ptemp); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointLine exit\n")); #endif /* DEBUG */ 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 * GMDistPointPlane, point plane distance M *****************************************************************************/ RealType GMDistPointPlane(PointType Point, PlaneType Plane) { RealType t; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointPlane entered\n")); #endif /* DEBUG */ t = (DOT_PROD(Plane, Point) + Plane[3]) / VEC_SQR_LENGTH(Plane); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistPointPlane exit\n")); #endif /* DEBUG */ 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 * GMPointFromLinePlane, line plane intersection M *****************************************************************************/ int GMPointFromLinePlane(PointType Pl, PointType Vl, PlaneType Plane, PointType InterPoint, RealType *t) { RealType DotProd; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromLinePlane entered\n")); #endif /* DEBUG */ /* Check to see if they are vertical - no intersection at all! */ DotProd = DOT_PROD(Vl, Plane); if (FABS(DotProd) < GMBasicEps) return FALSE; /* Vl is coplanar to Plane. */ /* Else find t in line such that the plane equation plane is satisfied: */ *t = (-Plane[3] - DOT_PROD(Plane, Pl)) / DotProd; /* And so find the intersection point which is at that t: */ PT_SCALE_AND_ADD(InterPoint, Pl, Vl, *t); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromLinePlane exit\n")); #endif /* DEBUG */ 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 * GMPointFromLinePlane01, line plane intersection M *****************************************************************************/ int GMPointFromLinePlane01(PointType Pl, PointType Vl, PlaneType Plane, PointType InterPoint, RealType *t) { RealType DotProd; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromLinePlane01 entered\n")); #endif /* DEBUG */ /* Check to see if they are vertical - no intersection at all! */ DotProd = DOT_PROD(Vl, Plane); if (FABS(DotProd) < GMBasicEps) return FALSE; /* Else find t in line such that the plane equation plane is satisfied: */ *t = (-Plane[3] - DOT_PROD(Plane, Pl)) / 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: */ PT_SCALE_AND_ADD(InterPoint, Pl, Vl, *t); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMPointFromLinePlane01 exit\n")); #endif /* DEBUG */ 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 * * * SEE ALSO: M * GM2PointsFromCircCirc M * * * KEYWORDS: M * GM2PointsFromLineLine, line line distance M *****************************************************************************/ int GM2PointsFromLineLine(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 IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GM2PointsFromLineLine entered\n")); #endif /* DEBUG */ CROSS_PROD(Vtemp, Vl1, Vl2); /* Check to see if they are parallel. */ if (VEC_SQR_LENGTH(Vtemp) < SQR(GMBasicEps)) { *t1 = *t2 = IRIT_INFNTY; PT_COPY(Pt1, Pl1); /* Pick point on line1 and find */ GMPointFromPointLine(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... */ CROSS_PROD(Plane1, Vl1, Vtemp); /* Find the A, B, C coef.'s. */ VEC_SAFE_NORMALIZE(Plane1); CROSS_PROD(Plane2, Vl2, Vtemp); /* Find the A, B, C coef.'s. */ VEC_SAFE_NORMALIZE(Plane2); /* and now use a point on the plane to find the 4th coef. D: */ Plane1[3] = -DOT_PROD(Plane1, Pl1); /* DOT_PROD uses only first */ Plane2[3] = -DOT_PROD(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 = GMPointFromLinePlane(Pl1, Vl1, Plane2, Pt1, t1) && GMPointFromLinePlane(Pl2, Vl2, Plane1, Pt2, t2); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GM2PointsFromLineLine exit\n")); #endif /* DEBUG */ return i; } /***************************************************************************** * DESCRIPTION: M * Find the intersection point (if exists) of three planes. M * * * PARAMETERS: M * Pl1, Pl2, Pl3: Three planes to consider. M * Pt: Intersection point found (if any). M * * * RETURN VALUE: M * int: TRUE if exists an intersection point, FALSE otherwise. M * * * KEYWORDS: M * GMPointFrom3Planes M *****************************************************************************/ int GMPointFrom3Planes(PlaneType Pl1, PlaneType Pl2, PlaneType Pl3, PointType Pt) { MatrixType Mat, InvMat; VectorType V; MatGenUnitMat(Mat); Mat[0][0] = Pl1[0]; Mat[1][0] = Pl1[1]; Mat[2][0] = Pl1[2]; Mat[0][1] = Pl2[0]; Mat[1][1] = Pl2[1]; Mat[2][1] = Pl2[2]; Mat[0][2] = Pl3[0]; Mat[1][2] = Pl3[1]; Mat[2][2] = Pl3[2]; if (!MatInverseMatrix(Mat, InvMat)) return FALSE; V[0] = -Pl1[3]; V[1] = -Pl2[3]; V[2] = -Pl3[3]; MatMultVecby4by4(Pt, V, InvMat); return TRUE; } /***************************************************************************** * 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 * GMDistLineLine, line line distance M *****************************************************************************/ RealType GMDistLineLine(PointType Pl1, PointType Vl1, PointType Pl2, PointType Vl2) { RealType t1, t2; PointType Ptemp1, Ptemp2; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistLineLine entered\n")); #endif /* DEBUG */ GM2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, Ptemp1, &t1, Ptemp2, &t2); t1 = PT_PT_DIST(Ptemp1, Ptemp2); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMDistLineLine exit\n")); #endif /* DEBUG */ return t1; } /***************************************************************************** * DESCRIPTION: M * Test if the given polygon intersects the given plane. M * * * PARAMETERS: M * Pl: Polygon to test if interestes plane Pln. M * Pln: Plane to examine if polygon Pl intersects. Assumed normalized M * normal vector in Pln. M * MinDist: Returns closest vertex in Pl to Pln. M * * * RETURN VALUE: M * int: TRUE if intersects, FALSE otherwise. M * * * SEE ALSO: M * GMPolygonRayInter, GMSplitPolygonAtPlane M * * * KEYWORDS: M * GMPolygonPlaneInter M *****************************************************************************/ int GMPolygonPlaneInter(IPPolygonStruct *Pl, PlaneType Pln, RealType *MinDist) { IPVertexStruct *VHead = Pl -> PVertex, *V = VHead; RealType MaxNegDist = 0.0, MaxPosDist = 0.0; *MinDist = IRIT_INFNTY; do { RealType Dist = DOT_PROD(V -> Coord, Pln) + Pln[3]; if (Dist > 0.0) { if (*MinDist > Dist) *MinDist = Dist; MaxPosDist = MAX(Dist, MaxPosDist); } else { Dist = -Dist; if (*MinDist > Dist) *MinDist = Dist; MaxNegDist = MAX(Dist, MaxNegDist); } V = V -> Pnext; } while (V != NULL && V != VHead); return MaxNegDist > 0.0 && MaxPosDist > 0.0; } /***************************************************************************** * DESCRIPTION: M * Split the given convex polygon where it intersects the given plane. M * Pl is updated to in inside potion (where the normal is point into) and M * Pl -> Pnext will hold the second half. M * * * PARAMETERS: M * Pl: Polygon to split if interestes plane Pln. M * Pln: Plane to split polygon Pl at. Assumed normalized M * normal vector in Pln. M * * * RETURN VALUE: M * int: TRUE if intersects, FALSE otherwise. M * * * SEE ALSO: M * GMPolygonRayInter, GMPolygonPlaneInter M * * * KEYWORDS: M * GMSplitPolygonAtPlane M *****************************************************************************/ int GMSplitPolygonAtPlane(IPPolygonStruct *Pl, PlaneType Pln) { int i, NumInters = 0; RealType D1, D2; IPVertexStruct *InterV[2], *PrevV[2], *VHead = Pl -> PVertex, *V = VHead; do { RealType t; PointType InterPoint; VectorType Vl; IPVertexStruct *VNext = V -> Pnext == NULL ? VHead : V -> Pnext; VEC_SUB(Vl, VNext -> Coord, V -> Coord); if (GMPointFromLinePlane(V -> Coord, Vl, Pln, InterPoint, &t) && t > 0.0 && t <= 1.0) { PrevV[NumInters] = V; InterV[NumInters] = IPAllocVertex2(V -> Pnext); PT_COPY(InterV[NumInters] -> Coord, InterPoint); GMUpdateVertexByInterp(InterV[NumInters], V, VNext, TRUE, TRUE, TRUE); if (++NumInters >= 2) break; } V = VNext; } while (V != VHead); if (NumInters < 2) { for (i = 0; i < NumInters; i++) IPFreeVertex(InterV[i]); return FALSE; } /* We have two intersection locations - split the polygon. */ PrevV[0] -> Pnext = InterV[0]; PrevV[1] -> Pnext = InterV[1]; if (GMSplitPolyInPlaceAt2Vertices(Pl, InterV[0], InterV[1])) { D1 = GMPolyPlaneClassify(Pl, Pln); D2 = GMPolyPlaneClassify(Pl -> Pnext, Pln); if (D1 > 0 && D2 < 0) return TRUE; else if (D1 < 0 && D2 > 0) { SWAP(IPVertexStruct *, Pl -> PVertex, Pl -> Pnext -> PVertex); return TRUE; } else { GEOM_FATAL_ERROR(GEOM_ERR_INVALID_POLYGON); return FALSE; } } else return FALSE; } /***************************************************************************** * DESCRIPTION: M * Classify a plane as (mostly) on the positive side of the plane, M * returning a positive value, or (mostly) on the negative side of the plane, M * returning a negative value. M * * * PARAMETERS: M * Pl: Polygon to consider and classify. M * Pln: Plane to claassify against. M * * * RETURN VALUE: M * RealType: Positive or negative, classifying the polygon's side. M * * * KEYWORDS: M * GMPolyPlaneClassify M *****************************************************************************/ RealType GMPolyPlaneClassify(IPPolygonStruct *Pl, PlaneType Pln) { RealType D = 0.0; IPVertexStruct *VHead = Pl -> PVertex, *V = VHead; do { D += DOT_PROD(Pln, V -> Coord) + Pln[3]; V = V -> Pnext; } while (V != NULL && V != VHead); return D; } /***************************************************************************** * 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 * * * SEE ALSO: M * GMPolygonPlaneInter, GMSplitPolygonAtPlane M * * * KEYWORDS: M * GMPolygonRayInter, ray polygon intersection, Jordan theorem M *****************************************************************************/ int GMPolygonRayInter(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 = GMPointRayRelation(V -> Coord, PtRay, RayOtherAxes)) != GM_ON_RAY) break; V = V -> Pnext; } while (V != VStart && V != NULL); if (AlgState == GM_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 (GMPointRayRelation(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 (GMPointRayRelation(V -> Coord, PtRay, RayOtherAxes) == GM_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 = GMPointRayRelation(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 GMPolygonRayInter 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 GMPointRayRelation(PointType Pt, PointType PtRay, int Axes) { if (IRIT_APX_EQ(PtRay[Axes], Pt[Axes])) return GM_ON_RAY; else if (PtRay[Axes] < Pt[Axes]) return GM_ABOVE_RAY; else return GM_BELOW_RAY; } /***************************************************************************** * DESCRIPTION: M * Same as GMPolygonRayInter but for arbitrary oriented polygon. M * The polygon is transformed into the XY plane and then GMPolygonRayInter 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 * GMPolygonRayInter3D, ray polygon intersection, Jordan theorem M *****************************************************************************/ int GMPolygonRayInter3D(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, IPCopyVertexList(Pl -> PVertex), NULL); /* Make sure list is circular. */ V = IPGetLastVrtx(RotPl -> PVertex); if (V -> Pnext == NULL) V -> Pnext = RotPl -> PVertex; /* Bring the polygon to a XY parallel plane by rotation. */ GMGenRotateMatrix(RotMat, Pl -> Plane); V = VHead = RotPl -> PVertex; do { /* Transform the polygon itself. */ MatMultPtby4by4(V -> Coord, V -> Coord, RotMat); V = V -> Pnext; } while (V != VHead); MatMultPtby4by4(RotPt, PtRay, RotMat); i = GMPolygonRayInter(RotPl, RotPt, RayAxes); IPFreePolygonList(RotPl); return i; } /***************************************************************************** * 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 in the XY plane. 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 pointer to a static space holding the three Barycentric M * coefficients, or NULL if point Pt is outside the triangle M * Pt1 Pt2 Pt3. M * * * SEE ALSO: M * GMBaryCentric3Pts M * * * KEYWORDS: M * GMBaryCentric3Pts2D M *****************************************************************************/ RealType *GMBaryCentric3Pts2D(PointType Pt1, PointType Pt2, PointType Pt3, PointType Pt) { STATIC_DATA VectorType RetVal; VectorType V1, V2, V3; RealType R, X12, X23, X31; PT2D_SUB(V1, Pt, Pt1); PT2D_SUB(V2, Pt, Pt2); PT2D_SUB(V3, Pt, Pt3); X12 = CROSS_PROD_2D(V1, V2); X23 = CROSS_PROD_2D(V2, V3); X31 = CROSS_PROD_2D(V3, V1); if (X12 * X23 < -GMBasicEps || X23 * X31 < -GMBasicEps || X31 * X12 < -GMBasicEps) return NULL; /* Pt is out of the triangle Pt1 Pt2 Pt3. */ RetVal[0] = FABS(X23); RetVal[1] = FABS(X31); RetVal[2] = FABS(X12); if ((R = RetVal[0] + RetVal[1] + RetVal[2]) > 0.0) { R = 1.0 / R; PT_SCALE(RetVal, R); } return RetVal; } /***************************************************************************** * 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 pointer to a static space holding the three Barycentric M * coefficients, or NULL if point Pt is outside the triangle M * Pt1 Pt2 Pt3. M * * * SEE ALSO: M * GMBaryCentric3Pts2D M * * * KEYWORDS: M * GMBaryCentric3Pts M *****************************************************************************/ RealType *GMBaryCentric3Pts(PointType Pt1, PointType Pt2, PointType Pt3, PointType Pt) { STATIC_DATA VectorType RetVal; 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) < -GMBasicEps || DOT_PROD(X23, X31) < -GMBasicEps || DOT_PROD(X31, X12) < -GMBasicEps) 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); } return RetVal; } /***************************************************************************** * DESCRIPTION: M * Finds the two intersection points of the given two planar circles. M * * * PARAMETERS: M * Center1, Radius1: Geometry of first circle. M * Center2, Radius2: Geometry of second circle. M * Inter1, Inter2: Where the two intersection locations will be placed. M * * * RETURN VALUE: M * int: TRUE for successful computation, FALSE for failure. M * * * SEE ALSO: M * GM2PointsFromLineLine, GM2PointsFromCircCirc3D, GMCircleFrom3Points M * GMCircleFrom2Pts2Tans, GM2BiTansFromCircCirc, GM2TanLinesFromCircCirc M * * * KEYWORDS: M * GM2PointsFromCircCirc, circle circle intersection M *****************************************************************************/ int GM2PointsFromCircCirc(PointType Center1, RealType Radius1, PointType Center2, RealType Radius2, PointType Inter1, PointType Inter2) { int RetVal = TRUE; RealType A, B, C, Delta, a = Center2[0] - Center1[0], b = Center2[1] - Center1[1], c = (SQR(Radius1) - SQR(Radius2) + SQR(Center2[0]) - SQR(Center1[0]) + SQR(Center2[1]) - SQR(Center1[1])) * 0.5; if (PT_APX_EQ(Center1, Center2)) { Inter1[0] = Inter2[0] = Radius1; Inter1[1] = Inter2[1] = 0.0; } else if (FABS(a) > FABS(b)) { /* Solve for Y first. */ A = SQR(b) / SQR(a) + 1; B = 2 * (b * Center1[0] / a - b * c / SQR(a) - Center1[1]); C = SQR(c / a) + SQR(Center1[0]) + SQR(Center1[1]) -2 * c * Center1[0] / a - SQR(Radius1); Delta = SQR(B) - 4 * A * C; if (Delta < 0) { /* If no solution, do something almost reasonable. */ RetVal = FALSE; Delta = 0; } Inter1[1] = (-B + sqrt(Delta)) / (2 * A); Inter2[1] = (-B - sqrt(Delta)) / (2 * A); Inter1[0] = (c - b * Inter1[1]) / a; Inter2[0] = (c - b * Inter2[1]) / a; } else { /* Solve for X first. */ A = SQR(a) / SQR(b) + 1; B = 2 * (a * Center1[1] / b - a * c / SQR(b) - Center1[0]); C = SQR(c / b) + SQR(Center1[0]) + SQR(Center1[1]) -2 * c * Center1[1] / b - SQR(Radius1); Delta = SQR(B) - 4 * A * C; if (Delta < 0) { /* If no solution, do something almost reasonable. */ RetVal = FALSE; Delta = 0; } Inter1[0] = (-B + sqrt(Delta)) / (2 * A); Inter2[0] = (-B - sqrt(Delta)) / (2 * A); Inter1[1] = (c - a * Inter1[0]) / b; Inter2[1] = (c - a * Inter2[0]) / b; } Inter1[2] = Inter2[2] = 0.0; return RetVal; } /***************************************************************************** * DESCRIPTION: M * Compute the intersection of two circles in general position in R^3. M * The circles are centered at Cntr1/2 in a plane normal to Nrml1/2 and have M * a radius of Rad1/2. The upto two intersections are returned in Inter1/2. M * * * PARAMETERS: M * Cntr1, Nrml1, Rad1: Center, normal and radius of first circle. M * Cntr2, Nrml2, Rad2: Center, normal and radius of second circle. M * Inter1: First intersection location in E3. M * Inter2: Second intersection location in E3. M * * * RETURN VALUE: M * int: Number of intersections found: 0, 1, or 2. M * * * SEE ALSO: M * GM2PointsFromLineLine, GM2PointsFromCircCirc, GMCircleFrom3Points M * GMCircleFrom2Pts2Tans, GM2BiTansFromCircCirc, GM2TanLinesFromCircCirc M * * * KEYWORDS: M * GM2PointsFromCircCirc3D M *****************************************************************************/ int GM2PointsFromCircCirc3D(PointType Cntr1, VectorType Nrml1, RealType Rad1, PointType Cntr2, VectorType Nrml2, RealType Rad2, PointType Inter1, PointType Inter2) { MatrixType Mat, InvMat; PointType Cntr1t, Cntr2t; VectorType Nrml1t, Nrml2t; /* Bring Nrml1 to be the Z axis. */ GMGenMatrixZ2Dir(Mat, Nrml1); MatInverseMatrix(Mat, InvMat); MatMultPtby4by4(Cntr1t, Cntr1, InvMat); MatMultPtby4by4(Cntr2t, Cntr2, InvMat); MatMultVecby4by4(Nrml1t, Nrml1, InvMat); MatMultVecby4by4(Nrml2t, Nrml2, InvMat); if (PT_APX_EQ(Nrml1, Nrml2)) { /* It is a 2D problem. Use GM2PointsFromCircCirc. */ if (GM2PointsFromCircCirc(Cntr1, Rad1, Cntr2, Rad2, Inter1, Inter2)) return 2; else return 0; } else { RealType a, h; PointType P1, ClosestPoint; VectorType V1, V2; PlaneType Plane2; /* It is a 3D problem. Circle one is XY parallel at level Cntr1t[2]. */ /* Find the intersection of the Plane 'Z = Cntr1t[2]' with second */ /* circle. */ VEC_COPY(Plane2, Nrml2t); Plane2[3] = -DOT_PROD(Cntr2t, Nrml2t); /* Substituting 'Z = Cntr1t[2]' into the plane euqation, we can */ /* derive the vector of the line via the perpendicular to the vector */ /* and then find a point on that line. */ V1[0] = Plane2[0]; /* The line's normal vector. */ V1[1] = Plane2[1]; V1[2] = 0.0; V2[0] = V1[1]; /* The line's vector. */ V2[1] = -V1[0]; V2[2] = 0.0; if (FABS(Plane2[0]) > FABS(Plane2[1])) { /* Assume Y is zero and find X as point on line: */ P1[0] = -(Plane2[2] * Cntr1t[2] + Plane2[3]) / Plane2[0]; P1[1] = 0.0; P1[2] = Cntr1t[2]; } else { /* Assume X is zero and find Y as point on line: */ P1[0] = 0.0; P1[1] = -(Plane2[2] * Cntr1t[2] + Plane2[3]) / Plane2[1]; P1[2] = Cntr1t[2]; } /* Find the closest point to Cntr1t on Line "P1 + V2 t" and apply */ /* some pythagorian relations to derive the final solution. */ GMPointFromPointLine(Cntr1t, P1, V2, ClosestPoint); a = PT_PT_DIST(Cntr1t, ClosestPoint); if (FABS(Rad1) < FABS(a)) return 0; h = sqrt(SQR(Rad1) - SQR(a)); /* Compute the solutions as "ClosestPoint +/- h V2": */ VEC_NORMALIZE(V2); VEC_SCALE(V2, h); PT_ADD(Inter1, ClosestPoint, V2); PT_SUB(Inter2, ClosestPoint, V2); } /* Map the solutions back to their original position. */ MatMultPtby4by4(Inter1, Inter1, Mat); MatMultPtby4by4(Inter2, Inter2, Mat); return PT_APX_EQ(Inter1, Inter2) ? 1 : 2; } /***************************************************************************** * DESCRIPTION: M * Routine to construct a circle through given 3 points. If two of the M * points are the same or the three points are collinear it returns FALSE, M * otherwise (successful), it returns TRUE. M * * * PARAMETERS: M * Center: Of computed circle. M * Radius: Of computed circle. M * Pt1, Pt2, Pt3: Three points to fit a circle through. M * * * RETURN VALUE: M * int: TRUE if successful, FALSE otherwise. M * * * SEE ALSO: M * GM2PointsFromCircCirc3D, GM2PointsFromLineLine, GM2PointsFromCircCirc M * GMCircleFrom2Pts2Tans, GM2BiTansFromCircCirc, GM2TanLinesFromCircCirc M * * * KEYWORDS: M * GMCircleFrom3Points, circle M *****************************************************************************/ int GMCircleFrom3Points(PointType Center, RealType *Radius, PointType Pt1, PointType Pt2, PointType Pt3) { int RetVal; RealType t1, t2; PointType Pl1, Pl2, PInter1, PInter2; VectorType Vl1, Vl2; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMCircleFrom3Points entered\n")); #endif /* DEBUG */ if (GMCollinear3Pts(Pt1, Pt2, Pt3)) return FALSE; PT_SUB(Vl1, Pt2, Pt1); SWAP(RealType, Vl1[0], Vl1[1]); Vl1[1] = -Vl1[1]; PT_SUB(Vl2, Pt3, Pt2); SWAP(RealType, Vl2[0], Vl2[1]); Vl2[1] = -Vl2[1]; PT_BLEND(Pl1, Pt1, Pt2, 0.5); PT_BLEND(Pl2, Pt2, Pt3, 0.5); RetVal = GM2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, PInter1, &t1, PInter2, &t2); PT_BLEND(Center, PInter1, PInter2, 0.5); *Radius = sqrt(SQR(Center[0] - Pt1[0]) + SQR(Center[1] - Pt1[1])); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMCircleFrom3Points exit\n")); #endif /* DEBUG */ return RetVal; } /***************************************************************************** * DESCRIPTION: M * Routine to construct a circle through given 3 points. If two of the M * points are the same or the three points are collinear it returns FALSE, M * otherwise (successful), it returns TRUE. M * * * PARAMETERS: M * Center: Of computed circle. M * Radius: Of computed circle. M * Pt1, Pt2: Two points to fit a circle through. M * Tan1, Tan2: Two tangents to the circle at Pt1, Pt2. M * * * RETURN VALUE: M * int: TRUE if successful, FALSE otherwise. M * * * SEE ALSO: M * GM2PointsFromCircCirc3D, GM2PointsFromLineLine, GM2PointsFromCircCirc M * GMCircleFrom3Points, GM2BiTansFromCircCirc, GM2TanLinesFromCircCirc M * * * KEYWORDS: M * GMCircleFrom2Pts2Tans, circle M *****************************************************************************/ int GMCircleFrom2Pts2Tans(PointType Center, RealType *Radius, PointType Pt1, PointType Pt2, VectorType Tan1, VectorType Tan2) { int RetVal; RealType t1, t2; PointType PInter1, PInter2; VectorType Nrml1, Nrml2; #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMCircleFrom2Pts2Tans entered\n")); #endif /* DEBUG */ if (PT_APX_EQ(Pt1, Pt2)) return FALSE; Nrml1[0] = Tan1[1]; Nrml1[1] = -Tan1[0]; Nrml1[2] = 0.0; Nrml2[0] = Tan2[1]; Nrml2[1] = -Tan2[0]; Nrml2[2] = 0.0; RetVal = GM2PointsFromLineLine(Pt1, Nrml1, Pt2, Nrml2, PInter1, &t1, PInter2, &t2); PT_BLEND(Center, PInter1, PInter2, 0.5); *Radius = sqrt(SQR(Center[0] - Pt1[0]) + SQR(Center[1] - Pt1[1])); #ifdef DEBUG IRIT_IF_DEBUG_ON_PARAMETER(_DebugGMEntryExit) fprintf(stderr, IRIT_EXP_STR("GMCircleFrom2Pts2Tans exit\n")); #endif /* DEBUG */ return RetVal; } /***************************************************************************** * DESCRIPTION: M * Finds the two intersection points of the given two planar circles. M * * * PARAMETERS: M * Center1, Radius1: Geometry of first circle. M * Center2, Radius2: Geometry of second circle. M * OuterTans: TRUE for outer two tangents, FALSE for inner two. M * TanPts: The two tangents designated by the end points of the M * Segments. M * * * RETURN VALUE: M * int: TRUE for successful computation, FALSE for failure or no such M * bitangents exist for the current configuration. M * * * SEE ALSO: M * GM2PointsFromLineLine, GM2PointsFromCircCirc3D, GMCircleFrom3Points M * GMCircleFrom2Pts2Tans, GM2PointsFromCircCirc, GM2TanLinesFromCircCirc M * * * KEYWORDS: M * GM2BiTansFromCircCirc, circle circle tangenties M *****************************************************************************/ int GM2BiTansFromCircCirc(PointType Center1, RealType Radius1, PointType Center2, RealType Radius2, int OuterTans, PointType TanPts[2][2]) { int i; LineType TanLines[2]; PointType LnPt; VectorType LnDir; if (!GM2TanLinesFromCircCirc(Center1, Radius1, Center2, Radius2, OuterTans, TanLines)) return FALSE; /* Find tangency locations of lines and the two circles. */ for (i = 0; i < 2; i++) { GMPointVecFromLine(TanLines[i], LnPt, LnDir); GMPointFromPointLine(Center1, LnPt, LnDir, TanPts[i][0]); GMPointFromPointLine(Center2, LnPt, LnDir, TanPts[i][1]); } return TRUE; } /***************************************************************************** * DESCRIPTION: M * Finds the two tangent lines to the given two planar circles. M * * * PARAMETERS: M * Center1, Radius1: Geometry of first circle. M * Center2, Radius2: Geometry of second circle. M * OuterTans: TRUE for outer two tangents, FALSE for inner two. M * Tans: The two tangent lines designated by line equatios. M * * * RETURN VALUE: M * int: TRUE for successful computation, FALSE for failure or no such M * bitangents exist for the current configuration. M * * * SEE ALSO: M * GM2PointsFromLineLine, GM2PointsFromCircCirc3D, GMCircleFrom3Points M * GMCircleFrom2Pts2Tans, GM2PointsFromCircCirc, GM2BiTansFromCircCirc M * * * KEYWORDS: M * GM2TanLinesFromCircCirc, circle circle tangenties M *****************************************************************************/ int GM2TanLinesFromCircCirc(PointType Center1, RealType Radius1, PointType Center2, RealType Radius2, int OuterTans, LineType Tans[2]) { int i, n, SwapXY; RealType A, B, C, Sols[2]; PointType Cntr1, Cntr2; PT_COPY(Cntr1, Center1); PT_COPY(Cntr2, Center2); if (PT_PT_DIST(Cntr1, Cntr2) < FABS(Radius1 - Radius2)) return FALSE; /* No bitangentices. */ if (OuterTans) { Radius1 = FABS(Radius1); /* Both should be positive. */ Radius2 = FABS(Radius2); } else { Radius1 = -FABS(Radius1); /* Different signs! */ Radius2 = FABS(Radius2); } if (FABS(Cntr1[0] - Cntr2[0]) < FABS(Cntr1[1] - Cntr2[1])) { /* Exchange the role of X and Y as we divide by Dx. */ SWAP(RealType, Cntr1[0], Cntr1[1]); SWAP(RealType, Cntr2[0], Cntr2[1]); SwapXY = TRUE; } else SwapXY = FALSE; /* Build the coefficients of the quadratic equation and solve. */ A = SQR(Cntr1[0] - Cntr2[0]) + SQR(Cntr1[1] - Cntr2[1]); B = 2 * (Radius1 * (Cntr2[1] - Cntr1[1]) - Radius2 * (Cntr2[1] - Cntr1[1])); C = SQR(Radius1 - Radius2) - SQR(Cntr1[0] - Cntr2[0]); if (A != 0) n = GMSolveQuadraticEqn(B / A, C / A, Sols); else n = 0; if (n < 2) return FALSE; for (i = 0; i < 2; i++) { Tans[i][0] = (Sols[i] * (Cntr2[1] - Cntr1[1]) + (Radius1 - Radius2)) / (Cntr1[0] - Cntr2[0]); Tans[i][1] = Sols[i]; Tans[i][2] = (Cntr2[0] * (Sols[i] * Cntr1[1] - Radius1) - Cntr1[0] * (Sols[i] * Cntr2[1] - Radius2)) / (Cntr1[0] - Cntr2[0]); } if (SwapXY) { /* Exchange the role of X and Y back. */ SWAP(RealType, Tans[0][0], Tans[0][1]); SWAP(RealType, Tans[1][0], Tans[1][1]); } return TRUE; } /***************************************************************************** * DESCRIPTION: M * Computes the solutions, if any, of the given quadratic equation. Only M * real solutions are considered. M * * * PARAMETERS: M * A, B: The equation's coefficients as x^2 + A x + B = 0. M * Sols: Where to place the solutions. At most two. M * * * RETURN VALUE: M * int: Number of real solutions. M * * * SEE ALSO: M * GMSolveCubicEqn, GMSolveCubicEqn2, GMSolveQuadraticEqn2 M * GMSolveQuarticEqn M * * * KEYWORDS: M * GMSolveQuadraticEqn M *****************************************************************************/ int GMSolveQuadraticEqn(RealType A, RealType B, RealType *Sols) { RealType d = SQR(A) - 4.0 * B; if (d < 0.0) return 0; else if (d == 0.0) { Sols[0] = -0.5 * A; return 1; } else { d = sqrt(d); Sols[0] = (-A - d) * 0.5; Sols[1] = (-A + d) * 0.5; return 2; } } /***************************************************************************** * DESCRIPTION: M * Calculates the two square roots of the quadratic equation: M * x^2 + Bx + C = 0 M * * * PARAMETERS: M * B, C: The coefficients of the quadratic polynomial. M * RSols, ISols: Solutions such that each pair RSols[i], ISols[i] is the M * complex root(i). M * * * RETURN VALUE: M * int: The number of REAL solutions of the polynomial. M * * * SEE ALSO: M * GMSolveCubicEqn, GMSolveCubicEqn2, GMSolveQuadraticEqn M * GMSolveQuarticEqn M * * * KEYWORDS: M * GMSolveQuadraticEqn2 M *****************************************************************************/ int GMSolveQuadraticEqn2(RealType B, RealType C, RealType *RSols, RealType *ISols) { int n; RealType D = SQR(B) - 4 * C; if (D >= 0) { /* 2 real solutions. */ D = sqrt(D); RSols[0] = (-B + D) * 0.5; RSols[1] = (-B - D) * 0.5; ISols[0] = ISols[1] = 0.0; n = 2; } else { /* Two conjugate complex solutions. */ D = sqrt(-D); RSols[0] = -B * 0.5; ISols[0] = -D * 0.5; RSols[1] = -B * 0.5; ISols[1] = +D * 0.5; n = 0; } return n; } /***************************************************************************** * DESCRIPTION: M * Computes the solutions, if any, of the given cubic equation. Only real M * solutions are considered. M * * * PARAMETERS: M * A, B, C: The equation's coefficients as x^3 + A x^2 + B x + C = 0. M * Sols: Where to place the solutions. At most three. M * * * RETURN VALUE: M * int: Number of real solutions. M * * * SEE ALSO: M * GMSolveQuadraticEqn, GMSolveQuadraticEqn2, GMSolveCubicEqn2, M * GMSolveQuarticEqn M * * * KEYWORDS: M * GMSolveCubicEqn M *****************************************************************************/ int GMSolveCubicEqn(RealType A, RealType B, RealType C, RealType *Sols) { int n; RealType Q = (3.0 * B - SQR(A)) / 9.0, R = (9.0 * A * B - 27.0 * C - 2.0 * CUBE(A)) / 54.0, D = CUBE(Q) + SQR(R); if (D < 0.0) { /* We have 3 real solutions. */ RealType R1 = 2.0 * sqrt(-Q), R2 = A / 3.0, Theta = acos(R / sqrt(CUBE(-Q))); Sols[0] = R1 * cos(Theta / 3.0) - R2; Sols[1] = R1 * cos((Theta + M_PI_MUL_2) / 3.0) - R2; Sols[2] = R1 * cos((Theta + 2.0 * M_PI_MUL_2) / 3.0) - R2; n = 3; } else { /* We have 1 real solutions. */ RealType RD1, RD2; D = sqrt(D); RD1 = R - D; RD2 = R + D; Sols[0] = pow(FABS(RD1), 1.0 / 3.0) * SIGN(RD1) + pow(FABS(RD2), 1.0 / 3.0) * SIGN(RD2) - A / 3.0; n = 1; } # ifdef DEBUG { IRIT_SET_IF_DEBUG_ON_PARAMETER(_DebugGMCubicSol, FALSE) { int i; for (i = 0; i < n; i++) { RealType x = Sols[i]; fprintf(stderr, "Sol = %f, (%d)\n", x, (((x + A) * x) + B) * x + C); } } } # endif /* DEBUG */ return n; } /***************************************************************************** * DESCRIPTION: M * Calculates the three roots (complex & real) of the cubic equation M * x^3 + Ax^2 + Bx + C = 0 M * Note: Cubic equations have at least one real root; this function always M * calculates the real root first, and the other two (possibly complex) later.M * This order of filling RSols & ISols is CRUCIAL for GMSolveQuarticEqn() M * * * PARAMETERS: M * A, B, C: The coefficients of the cubic polynomial. M * RSols, ISols: Each pair (RSols[i], ISols[i]) is the complex root(i) M * * * RETURN VALUE: M * int: The number of REAL solutions of the cubic polynomial. M * * * SEE ALSO: M * GMSolveCubicEqn, GMSolveQuadraticEqn, GMSolveQuadraticEqn2, M * GMSolveQuarticEqn M * * * KEYWORDS: M * GMSolveCubicEqn2 M *****************************************************************************/ int GMSolveCubicEqn2(RealType A, RealType B, RealType C, RealType *RSols, RealType *ISols) { int n; RealType Q = (3.0 * B - SQR(A)) / 9.0, R = (9.0 * A * B - 27.0 * C - 2.0 * CUBE(A)) / 54.0, D = CUBE(Q) + SQR(R); if (D == 0.0 && Q == 0.0 && R == 0.0) { /* 3 equal real solution. */ RSols[0] = RSols[1] = RSols[2] = SQRT3(-C); ISols[0] = ISols[1] = ISols[2] = 0.0; n=3; } else if (D <= 0.0) { /* We have 3 distinct real solutions. */ RealType R1, R2, Theta; R1 = 2.0 * sqrt(-Q), R2 = A / 3.0, Theta = acos(R / sqrt(CUBE(-Q))); RSols[0] = R1 * cos(Theta / 3.0) - R2; RSols[1] = R1 * cos((Theta + M_PI_MUL_2) / 3.0) - R2; RSols[2] = R1 * cos((Theta + 2.0 * M_PI_MUL_2) / 3.0) - R2; ISols[0] = ISols[1] = ISols[2] = 0.0; n = 3; } else { /* We have 1 real solution. */ RealType RD1, RD2; D = sqrt(D); RD1 = R - D; RD2 = R + D; RSols[0] = SQRT3(RD1) + SQRT3(RD2) - A/3.0; ISols[0] = 0.0; /* Continue solving via dividing the cubic polynomial */ n = GMSolveQuadraticEqn2(A + RSols[0], B + RSols[0] * (A + RSols[0]), RSols + 1, ISols + 1); n += 1 ; /* counting RSols[0] */ } return n; } /***************************************************************************** * DESCRIPTION: * * Auxiliary function for the function GMSolveQuarticEqn. * * Given a real solution found analytically but not precise: F(x) != 0, * * this function enhances the error of F(x) using Newton's method. * * * * PARAMETERS: * * Root: The real (analytic) solution of the quartic polynomial. * * A, B, C, D: The coefficients of the quartic polynomial. * * * * RETURN VALUE: * * RealType: The most accurate rounded solution function could derive. * *****************************************************************************/ static RealType GMSolveApplyNewton(RealType Root, RealType A, RealType B, RealType C, RealType D) { int i; RealType EvalRoot, /* f(Root) - Original value to be improved. */ EvalX, /* f(x) = x^4 + Ax^3 + Bx^2 + Cx + D */ Evaldx, /* f'(x) */ EvalF, /* F = f^2(x) */ EvalFdx, /* F'(x) */ EvalFd2x, /* F''(x) */ x, NewX; /* Init loop, and don't loop too much because cucles are expected. */ NewX = Root; i = 0; EvalX = EvalRoot = D + Root * (C + Root * (B + Root * (A + Root))); if (EvalX < GM_QUARTIC_SOLVE_EPS) /* Avoid FP precision problems. */ return Root; do { x = NewX; Evaldx = C + x * (2 * B + x * (3 * A + 4 * x)); EvalF = SQR(EvalX); EvalFdx = 2 * EvalX * Evaldx; /* F'(x) = 2f(x)f'(x) */ EvalFd2x = 2 * (Evaldx * Evaldx + EvalX * (2 * B + x * (6 * A + 12 * x))); /* Newton: NewX = x - EvalF/EvalFdx + second order term; */ NewX = x - (EvalF / EvalFdx) * (1 + (EvalF * EvalFd2x) / (2 * SQR(EvalFdx))); EvalX = D + NewX * (C + NewX * (B + NewX * (A + NewX))); if (EvalX < GM_QUARTIC_SOLVE_EPS) /* Avoid FP precision problems. */ break; i++; } while ((NewX != x) && (i <= 10) && (EvalX < EvalRoot)); return EvalX >= EvalRoot ? Root : NewX; } /***************************************************************************** * DESCRIPTION: M * Computes the (upto) four real roots of the quartic equation M * x^4 + Ax^3 + Bx^2 + Cx + D = 0 V * M * Note 1 M * ------ M * In order to avoid building a library for complex numbers arithmetics, two M * arrays are used ISols[] and RSols[], where each RSols[i] and ISols[i], M * represent a complex number, and so calculation where made on the fly; M * Anyway, some of these calculations where performed in a specific way to M * reduce errors of double-precision nature. (especially calculating square M * roots of complex numbers). M * M * Note 2. M * ---------- M * In the case of Cubic and Quadratic equations, the number of real solutions M * is determined via the valus of D (the descrimenant); As such, the number M * of real solutions is easier to depict. M * However, Euler's solution for quartic equations manipulates all the M * three solutions of the cubic (real and complex), hoping to find some real M * roots by eliminating the imaginary part of the complex solutions. M * This, however, is a weakness of dependency upon the accuracy of the M * numbers' representation as a double-precision floating point. M * * * PARAMETERS: M * a, b, c, d: The coefficients of the quartic polynomial. M * Sols: The real roots of the polynomial. M * * * RETURN VALUE: M * int: The number of REAL solutions of the polynomial. M * * * SEE ALSO: M * GMSolveCubicEqn, GMSolveCubicEqn2, M * GMSolveQuadraticEqn, GMSolveQuadraticEqn2 M * * * KEYWORDS: M * GMSolveQuarticEqn M *****************************************************************************/ int GMSolveQuarticEqn(RealType a, RealType b, RealType c, RealType d, RealType *Sols) { int n = 0; RealType e, f, g, RSols[4] = { 0 }, ISols[4] = { 0 }; ZAP_MEM(Sols, sizeof(RealType) * 4); /* Get a depressed quartic equation - no coefficient for x^3 */ e = (b - 3.0 * SQR(a) / 8.0); f = (c + CUBE(a) / 8.0 - (a * b / 2.0)); g = (d - (3.0 * QUART(a) / 256.0) + (SQR(a) * b / 16.0) - (a * c / 4.0)); /* Zero is a solution. Solving a Cubic form (and subtracting a/4 from */ /* solutions. */ if (g == 0) { /* Includes d==0. */ if (d == 0 || f == 0.0 && e == 0.0) { Sols[0] = Sols[1] = Sols[2] = Sols[3] = -a / 4.0; n = 4; } else { int i = 0; /* No coef for x^3. */ n = GMSolveCubicEqn2(0.0, e, f, RSols, ISols); for (i = 0; i < n; i++) /* Real roots are returned first... */ Sols[i] = RSols[i] - a / 4.0; Sols[n] = -a / 4.0; n = n + 1; } } /* End if (g==0). */ /* It's a quadratic equation Y^2 -> Y, may skip negative roots already. */ else if (f == 0) { int i = GMSolveQuadraticEqn2(e, g, RSols, ISols); n = 0; if (i > 0) { if (RSols[0] >= 0) { Sols[0] = (+sqrt(RSols[0]) - a / 4.0); Sols[1] = (-sqrt(RSols[0]) - a / 4.0); n = 2; } if (RSols[1] >= 0) { Sols[n] = (+sqrt(RSols[1]) - a / 4.0); Sols[n+1] = (-sqrt(RSols[1]) - a / 4.0); n += 2; } } } /* End if (f==0). */ else { /* d,f,g != 0. */ int i; RealType p = 0.0, q = 0.0, r = 0.0, ip = 0.0, iq = 0.0; n = GMSolveCubicEqn2(e / 2.0, (SQR(e) - (4.0 * g)) / 16.0, -SQR(f) / 64.0, RSols, ISols); /* Square roots of three real numbers (may be negative). */ if (n == 3) { /* p = sqrt(r[1]). */ if (RSols[1] >= 0) { p = sqrt(RSols[1]); ip = 0.0; } else { ip = sqrt(RSols[1]); p = 0.0; } /* q=sqrt(r[2]). */ if (RSols[2] >= 0) { q = sqrt(RSols[2]); iq = 0.0; } else { iq = sqrt(RSols[2]); q = 0.0; } /* r = sqrt(r[0]); r != 0 and is a real number. */ if (ip == 0.0 && iq == 0.0) { r = -f / (8.0 * p * q); } else { r = f / (8.0 * ip * iq); /* i*i = -1 */ } } /* End if n==3. */ /* n = 1; performing complex square-root calculations. */ else { /* These are the same for q and p. */ RealType sqrR = SQR(RSols[1]), sqrI = SQR(ISols[1]), T = sqrt(sqrR + sqrI); /* p = sqrt(r[1]). */ ip = sqrt((T - RSols[1]) / 2.0); p = sqrt((RSols[1] + T) / 2.0) * SIGN(ISols[1]) * SIGN(ip); /* q = sqrt(r[2]). */ iq = sqrt((T - RSols[2]) / 2.0); q = sqrt((T + RSols[2]) / 2.0) * SIGN(ISols[2]) * SIGN(iq); /* r = sqrt(r[0]), which is always a real number. */ r = -f / (8.0 * (p * q - ip * iq)); } /* Euler's solution for quartic equations. */ RSols[0] = p + q + r - (a/4.0); ISols[0] = ip + iq; RSols[1] = p - q - r - (a/4.0); ISols[1] = ip - iq; RSols[2] = -p + q - r - (a/4.0); ISols[2] = -ip + iq; RSols[3] = -p - q + r - (a/4.0); ISols[3] = -ip - iq; /* Consider real solution only. */ n = 0; for (i = 0; i < 4; i++) { if (ISols[i] == 0.0) { Sols[n] = GMSolveApplyNewton(RSols[i], a, b, c, d); n++; } } } /* End else (d != 0; f != 0, g != 0). */ return n; }