void IntersectLineSegments(const float A1x, const float A1y, const float A1z,
const float A2x, const float A2y, const float A2z,
const float B1x, const float B1y, const float B1z,
const float B2x, const float B2y, const float B2z,
bool infinite_lines, float epsilon, float &PointOnSegAx,
float &PointOnSegAy, float &PointOnSegAz, float &PointOnSegBx,
float &PointOnSegBy, float &PointOnSegBz)
{
float temp = 0.f;
float epsilon_squared = epsilon * epsilon;
// Compute parameters from Equations (1) and (2) in the text
float Lax = A2x - A1x;
float Lay = A2y - A1y;
float Laz = A2z - A1z;
float Lbx = B2x - B1x;
float Lby = B2y - B1y;
float Lbz = B2z - B1z;
// From Equation (15)
float L11 = (Lax * Lax) + (Lay * Lay) + (Laz * Laz);
float L22 = (Lbx * Lbx) + (Lby * Lby) + (Lbz * Lbz);
// Line/Segment A is degenerate ---- Special Case #1
if (L11 < epsilon_squared)
{
PointOnSegAx = A1x;
PointOnSegAy = A1y;
PointOnSegAz = A1z;
FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, A1x, A1y, A1z,
infinite_lines, epsilon, PointOnSegBx, PointOnSegBy,
PointOnSegBz, temp);
}
// Line/Segment B is degenerate ---- Special Case #1
else if (L22 < epsilon_squared)
{
PointOnSegBx = B1x;
PointOnSegBy = B1y;
PointOnSegBz = B1z;
FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
infinite_lines, epsilon, PointOnSegAx, PointOnSegAy,
PointOnSegAz, temp);
}
// Neither line/segment is degenerate
else
{
// Compute more parameters from Equation (3) in the text.
float ABx = B1x - A1x;
float ABy = B1y - A1y;
float ABz = B1z - A1z;
// and from Equation (15).
float L12 = -(Lax * Lbx) - (Lay * Lby) - (Laz * Lbz);
float DetL = L11 * L22 - L12 * L12;
// Lines/Segments A and B are parallel ---- special case #2.
if (FABS(DetL) < epsilon)
{
FindNearestPointOfParallelLineSegments(A1x, A1y, A1z, A2x, A2y, A2z,
Lax, Lay, Laz,
B1x, B1y, B1z, B2x, B2y, B2z,
Lbx, Lby, Lbz,
infinite_lines, epsilon,
PointOnSegAx, PointOnSegAy, PointOnSegAz,
PointOnSegBx, PointOnSegBy, PointOnSegBz);
}
// The general case
else
{
// from Equation (15)
float ra = Lax * ABx + Lay * ABy + Laz * ABz;
float rb = -Lbx * ABx - Lby * ABy - Lbz * ABz;
float t = (L11 * rb - ra * L12)/DetL; // Equation (12)
#ifdef USE_CRAMERS_RULE
float s = (L22 * ra - rb * L12)/DetL;
#else
float s = (ra-L12*t)/L11; // Equation (13)
#endif // USE_CRAMERS_RULE
#ifdef CHECK_ANSWERS
float check_ra = s*L11 + t*L12;
float check_rb = s*L12 + t*L22;
assert(FABS(check_ra-ra) < epsilon);
assert(FABS(check_rb-rb) < epsilon);
#endif // CHECK_ANSWERS
// if we are dealing with infinite lines or if parameters s and t both
// lie in the range [0,1] then just compute the points using Equations
// (1) and (2) from the text.
PointOnSegAx = (A1x + s * Lax);
PointOnSegAy = (A1y + s * Lay);
PointOnSegAz = (A1z + s * Laz);
PointOnSegBx = (B1x + t * Lbx);
PointOnSegBy = (B1y + t * Lby);
PointOnSegBz = (B1z + t * Lbz);
// otherwise, at least one of s and t is outside of [0,1] and we have to
// handle this case.
if (false == infinite_lines && (OUT_OF_RANGE(s) || OUT_OF_RANGE(t)))
{
AdjustNearestPoints(A1x, A1y, A1z, Lax, Lay, Laz,
B1x, B1y, B1z, Lbx, Lby, Lbz,
epsilon, s, t,
PointOnSegAx, PointOnSegAy, PointOnSegAz,
PointOnSegBx, PointOnSegBy, PointOnSegBz);
}
}
}
}
/**************************************************************************
|
| Method: IntersectLineSegments
|
| Purpose: Find the nearest point between two finite length line segments
| or two infinite lines in 3-dimensional space. The function calculates
| the point on each line/line segment that is closest to the other
| line/line segment, the midpoint between the nearest points, and
| the vector between these two points. If the two nearest points
| are close within a tolerance, a flag is set indicating the lines
| have a "true" intersection.
|
| Parameters: Input:
| ------
| A1x, A1y, A1z - Coordinates of first defining point of line/segment A
| A2x, A2y, A2z - Coordinates of second defining point of line/segment A
| B1x, B1y, B1z - Coordinates of first defining point of line/segment B
| B2x, B2y, B2z - Coordinates of second defining point of line/segment B
| infinite_lines - set to true if lines are to be treated as infinite
| epsilon - tolerance value to be used to check for degenerate
| and parallel lines, and to check for true intersection.
|
| Output:
| -------
| PointOnSegAx, - Coordinates of the point on segment A that are nearest
| PointOnSegAy, to segment B. This corresponds to point C in the text.
| PointOnSegAz
| PointOnSegBx, - Coordinates of the point on segment B that are nearest
| PointOnSegBy, to segment A. This corresponds to point D in the text.
| PointOnSegBz
| NearestPointX, - Midpoint between the two nearest points. This can be
| NearestPointY, treated as *the* intersection point if nearest points
| NearestPointZ are sufficiently close. This corresponds to point P
| in the text.
| NearestVectorX, - Vector between the nearest point on A to the nearest
| point on segment B. This vector is normal to both
| lines if the lines are infinite, but is not guaranteed
| to be normal to both lines if both lines are finite
| length.
| true_intersection - true if the nearest points are close within a small
| tolerance.
**************************************************************************/
void IntersectLineSegments(const number A1x, const number A1y, const number A1z,
const number A2x, const number A2y, const number A2z,
const number B1x, const number B1y, const number B1z,
const number B2x, const number B2y, const number B2z,
bool infinite_lines, number epsilon, number &PointOnSegAx,
number &PointOnSegAy, number &PointOnSegAz, number &PointOnSegBx,
number &PointOnSegBy, number &PointOnSegBz, number &NearestPointX,
number &NearestPointY, number &NearestPointZ, number &NearestVectorX,
number &NearestVectorY, number &NearestVectorZ, bool &true_intersection)
{
number temp = 0.f;
number epsilon_squared = epsilon * epsilon;
// Compute parameters from Equations (1) and (2) in the text
number Lax = A2x - A1x;
number Lay = A2y - A1y;
number Laz = A2z - A1z;
number Lbx = B2x - B1x;
number Lby = B2y - B1y;
number Lbz = B2z - B1z;
// From Equation (15)
number L11 = (Lax * Lax) + (Lay * Lay) + (Laz * Laz);
number L22 = (Lbx * Lbx) + (Lby * Lby) + (Lbz * Lbz);
// Line/Segment A is degenerate ---- Special Case #1
if (L11 < epsilon_squared)
{
PointOnSegAx = A1x;
PointOnSegAy = A1y;
PointOnSegAz = A1z;
FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, A1x, A1y, A1z,
infinite_lines, epsilon, PointOnSegBx, PointOnSegBy,
PointOnSegBz, temp);
}
// Line/Segment B is degenerate ---- Special Case #1
else if (L22 < epsilon_squared)
{
PointOnSegBx = B1x;
PointOnSegBy = B1y;
PointOnSegBz = B1z;
FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
infinite_lines, epsilon, PointOnSegAx, PointOnSegAy,
PointOnSegAz, temp);
}
// Neither line/segment is degenerate
else
{
// Compute more parameters from Equation (3) in the text.
number ABx = B1x - A1x;
number ABy = B1y - A1y;
number ABz = B1z - A1z;
// and from Equation (15).
number L12 = -(Lax * Lbx) - (Lay * Lby) - (Laz * Lbz);
number DetL = L11 * L22 - L12 * L12;
// Lines/Segments A and B are parallel ---- special case #2.
if (FABS(DetL) < epsilon)
{
FindNearestPointOfParallelLineSegments(A1x, A1y, A1z, A2x, A2y, A2z,
Lax, Lay, Laz,
B1x, B1y, B1z, B2x, B2y, B2z,
Lbx, Lby, Lbz,
infinite_lines, epsilon,
PointOnSegAx, PointOnSegAy, PointOnSegAz,
PointOnSegBx, PointOnSegBy, PointOnSegBz);
}
// The general case
else
{
// from Equation (15)
number ra = Lax * ABx + Lay * ABy + Laz * ABz;
number rb = -Lbx * ABx - Lby * ABy - Lbz * ABz;
number t = (L11 * rb - ra * L12)/DetL; // Equation (12)
#ifdef USE_CRAMERS_RULE
number s = (L22 * ra - rb * L12)/DetL;
#else
number s = (ra-L12*t)/L11; // Equation (13)
#endif // USE_CRAMERS_RULE
#ifdef CHECK_ANSWERS
number check_ra = s*L11 + t*L12;
number check_rb = s*L12 + t*L22;
assert(FABS(check_ra-ra) < epsilon);
assert(FABS(check_rb-rb) < epsilon);
#endif // CHECK_ANSWERS
// if we are dealing with infinite lines or if parameters s and t both
// lie in the range [0,1] then just compute the points using Equations
// (1) and (2) from the text.
PointOnSegAx = (A1x + s * Lax);
PointOnSegAy = (A1y + s * Lay);
PointOnSegAz = (A1z + s * Laz);
PointOnSegBx = (B1x + t * Lbx);
PointOnSegBy = (B1y + t * Lby);
PointOnSegBz = (B1z + t * Lbz);
// otherwise, at least one of s and t is outside of [0,1] and we have to
// handle this case.
if (false == infinite_lines && (OUT_OF_RANGE(s) || OUT_OF_RANGE(t)))
{
AdjustNearestPoints(A1x, A1y, A1z, Lax, Lay, Laz,
B1x, B1y, B1z, Lbx, Lby, Lbz,
epsilon, s, t,
PointOnSegAx, PointOnSegAy, PointOnSegAz,
PointOnSegBx, PointOnSegBy, PointOnSegBz);
}
}
}
NearestPointX = 0.5f * (PointOnSegAx + PointOnSegBx);
NearestPointY = 0.5f * (PointOnSegAy + PointOnSegBy);
NearestPointZ = 0.5f * (PointOnSegAz + PointOnSegBz);
NearestVectorX = PointOnSegBx - PointOnSegAx;
NearestVectorY = PointOnSegBy - PointOnSegAy;
NearestVectorZ = PointOnSegBz - PointOnSegAz;
// optional check to indicate if the lines truly intersect
true_intersection = (FABS(NearestVectorX) +
FABS(NearestVectorY) +
FABS(NearestVectorZ)) < epsilon ? true : false;
}
void IntersectLineSegments(const D3DXVECTOR3 & A1,
const D3DXVECTOR3 & A2,
const D3DXVECTOR3 & B1,
const D3DXVECTOR3 & B2,
//bool infinite_lines, /*float epsilon,*/
D3DXVECTOR3 & OutA,
D3DXVECTOR3 & OutB)
{
float temp = 0.f;
const float epsilon = MY_EPSILON;
const float epsilon_squared = MY_EPSILON*MY_EPSILON;
// Compute parameters from Equations (1) and (2) in the text
D3DXVECTOR3 La = A2-A1;
D3DXVECTOR3 Lb = B2-B1;
// From Equation (15)
float L11 = D3DXVec3LengthSq(&La);
float L22 = D3DXVec3LengthSq(&Lb);
// Line/Segment A is degenerate ---- Special Case #1
if (L11 < epsilon_squared)
{
OutA = A1;
FindNearestPointOnLineSegment(B1, Lb, A1,
OutB, temp);
}
// Line/Segment B is degenerate ---- Special Case #1
else if (L22 < epsilon_squared)
{
OutB = B1;
FindNearestPointOnLineSegment(A1, La, B1,
OutA, temp);
}
// Neither line/segment is degenerate
else
{
// Compute more parameters from Equation (3) in the text.
D3DXVECTOR3 AB = B1 - A1;
// and from Equation (15).
float L12 = -D3DXVec3Dot(&La, &Lb);
float DetL = L11 * L22 - L12 * L12;
// Lines/Segments A and B are parallel ---- special case #2.
if (FABS(DetL) < epsilon)
{
FindNearestPointOfParallelLineSegments(A1, A2,
La,
B1, B2,
Lb,
OutA, OutB);
}
// The general case
else
{
// from Equation (15)
float ra = D3DXVec3Dot(&La, &AB);//Lax * ABx + Lay * ABy + Laz * ABz;
float rb = D3DXVec3Dot(&Lb, &AB);//-Lbx * ABx - Lby * ABy - Lbz * ABz;
float t = (L11 * rb - ra * L12)/DetL; // Equation (12)
#ifdef USE_CRAMERS_RULE
float s = (L22 * ra - rb * L12)/DetL;
#else
float s = (ra-L12*t)/L11; // Equation (13)
#endif // USE_CRAMERS_RULE
#ifdef CHECK_ANSWERS
float check_ra = s*L11 + t*L12;
float check_rb = s*L12 + t*L22;
assert(FABS(check_ra-ra) < epsilon);
assert(FABS(check_rb-rb) < epsilon);
#endif // CHECK_ANSWERS
// if we are dealing with infinite lines or if parameters s and t both
// lie in the range [0,1] then just compute the points using Equations
// (1) and (2) from the text.
OutA = (A1 + s * La);
OutB = (B1 + t * Lb);
// otherwise, at least one of s and t is outside of [0,1] and we have to
// handle this case.
if ((OUT_OF_RANGE(s) || OUT_OF_RANGE(t)))
{
AdjustNearestPoints(A1,La,B1,Lb,
s, t,
OutA,
OutB);
}
}
}
}
