int IntrLine2Segment2<Real>::Classify (Real* afS, Vector2<Real>* pkDiff,
Vector2<Real>* pkDiffN)
{
// The intersection of two lines is a solution to P0+s0*D0 = P1+s1*D1.
// Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q. If D0.Dot(Perp(D1)) = 0,
// the lines are parallel. Additionally, if Q.Dot(Perp(D1)) = 0, the
// lines are the same. If D0.Dot(Perp(D1)) is not zero, then
// s0 = Q.Dot(Perp(D1))/D0.Dot(Perp(D1))
// produces the point of intersection. Also,
// s1 = Q.Dot(Perp(D0))/D0.Dot(Perp(D1))
Vector2<Real> kDiff = m_pkSegment->Origin - m_pkLine->Origin;
if (pkDiff)
{
*pkDiff = kDiff;
}
Real fD0DotPerpD1 = m_pkLine->Direction.DotPerp(m_pkSegment->Direction);
if (Math<Real>::FAbs(fD0DotPerpD1) > Math<Real>::ZERO_TOLERANCE)
{
// lines intersect in a single point
if (afS)
{
Real fInvD0DotPerpD1 = ((Real)1.0)/fD0DotPerpD1;
Real fDiffDotPerpD0 = kDiff.DotPerp(m_pkLine->Direction);
Real fDiffDotPerpD1 = kDiff.DotPerp(m_pkSegment->Direction);
afS[0] = fDiffDotPerpD1*fInvD0DotPerpD1;
afS[1] = fDiffDotPerpD0*fInvD0DotPerpD1;
}
return IT_POINT;
}
// lines are parallel
kDiff.Normalize();
if (pkDiffN)
{
*pkDiffN = kDiff;
}
Real fDiffNDotPerpD1 = kDiff.DotPerp(m_pkSegment->Direction);
if (Math<Real>::FAbs(fDiffNDotPerpD1) <= Math<Real>::ZERO_TOLERANCE)
{
// lines are colinear
return IT_SEGMENT;
}
// lines are parallel, but distinct
return IT_EMPTY;
}
int IntrSegment2Segment2<Real>::Classify (Real* s, Vector2<Real>* diff,
Vector2<Real>* diffN)
{
// The intersection of two lines is a solution to P0+s0*D0 = P1+s1*D1.
// Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q. If D0.Dot(Perp(D1)) = 0,
// the lines are parallel. Additionally, if Q.Dot(Perp(D1)) = 0, the
// lines are the same. If D0.Dot(Perp(D1)) is not zero, then
// s0 = Q.Dot(Perp(D1))/D0.Dot(Perp(D1))
// produces the point of intersection. Also,
// s1 = Q.Dot(Perp(D0))/D0.Dot(Perp(D1))
Vector2<Real> originDiff = mSegment1->Center - mSegment0->Center;
if (diff)
{
*diff = originDiff;
}
Real D0DotPerpD1 = mSegment0->Direction.DotPerp(mSegment1->Direction);
if (Math<Real>::FAbs(D0DotPerpD1) > Math<Real>::ZERO_TOLERANCE)
{
// Lines intersect in a single point.
if (s)
{
Real invD0DotPerpD1 = ((Real)1)/D0DotPerpD1;
Real diffDotPerpD0 = originDiff.DotPerp(mSegment0->Direction);
Real diffDotPerpD1 = originDiff.DotPerp(mSegment1->Direction);
s[0] = diffDotPerpD1*invD0DotPerpD1;
s[1] = diffDotPerpD0*invD0DotPerpD1;
}
return IT_POINT;
}
// Lines are parallel.
originDiff.Normalize();
if (diffN)
{
*diffN = originDiff;
}
Real diffNDotPerpD1 = originDiff.DotPerp(mSegment1->Direction);
if (Math<Real>::FAbs(diffNDotPerpD1) <= Math<Real>::ZERO_TOLERANCE)
{
// Lines are colinear.
return IT_SEGMENT;
}
// Lines are parallel, but distinct.
return IT_EMPTY;
}
void IntrLine2Triangle2<Real>::TriangleLineRelations (
const Vector2<Real>& origin, const Vector2<Real>& direction,
const Triangle2<Real>& triangle, Real dist[3], int sign[3],
int& positive, int& negative, int& zero)
{
positive = 0;
negative = 0;
zero = 0;
for (int i = 0; i < 3; ++i)
{
Vector2<Real> diff = triangle.V[i] - origin;
dist[i] = diff.DotPerp(direction);
if (dist[i] > Math<Real>::ZERO_TOLERANCE)
{
sign[i] = 1;
++positive;
}
else if (dist[i] < -Math<Real>::ZERO_TOLERANCE)
{
sign[i] = -1;
++negative;
}
else
{
dist[i] = (Real)0;
sign[i] = 0;
++zero;
}
}
}
void IntrLine2Triangle2<Real>::TriangleLineRelations (
const Vector2<Real>& rkOrigin, const Vector2<Real>& rkDirection,
const Triangle2<Real>& rkTriangle, Real afDist[3], int aiSign[3],
int& riPositive, int& riNegative, int& riZero)
{
riPositive = 0;
riNegative = 0;
riZero = 0;
for (int i = 0; i < 3; i++)
{
Vector2<Real> kDiff = rkTriangle.V[i] - rkOrigin;
afDist[i] = kDiff.DotPerp(rkDirection);
if (afDist[i] > Math<Real>::ZERO_TOLERANCE)
{
aiSign[i] = 1;
riPositive++;
}
else if (afDist[i] < -Math<Real>::ZERO_TOLERANCE)
{
aiSign[i] = -1;
riNegative++;
}
else
{
afDist[i] = (Real)0.0;
aiSign[i] = 0;
riZero++;
}
}
}
bool IntrTriangle3Cylinder3<Real>::DiskOverlapsSegment (
const Vector2<Real>& Q0, const Vector2<Real>& Q1 ) const
{
Real rSqr = mCylinder->Radius * mCylinder->Radius;
Vector2<Real> D = Q0 - Q1;
Real dot = Q0.Dot( D );
if ( dot <= ( Real )0 )
{
return Q0.Dot( Q0 ) <= rSqr;
}
Real lenSqr = D.Dot( D );
if ( dot >= lenSqr )
{
return Q1.Dot( Q1 ) <= rSqr;
}
dot = D.DotPerp( Q0 );
return dot * dot <= lenSqr * rSqr;
}
Vector2<Real> BiQuadToSqr<Real>::Transform (const Vector2<Real>& P)
{
Vector2<Real> A = mP00 - P;
Real AB = A.DotPerp(mB);
Real AC = A.DotPerp(mC);
// 0 = ac*bc+(bc^2+ac*bd-ab*cd)*s+bc*bd*s^2 = k0 + k1*s + k2*s^2
Real k0 = AC*mBC;
Real k1 = mBC*mBC + AC*mBD - AB*mCD;
Real k2 = mBC*mBD;
if (Math<Real>::FAbs(k2) >= Math<Real>::ZERO_TOLERANCE)
{
// The s-equation is quadratic.
Real inv = ((Real)0.5)/k2;
Real discr = k1*k1 - ((Real)4)*k0*k2;
Real root = Math<Real>::Sqrt(Math<Real>::FAbs(discr));
Vector2<Real> result0;
result0.X() = (-k1 - root)*inv;
result0.Y() = AB/(mBC + mBD*result0.X());
Real deviation0 = Deviation(result0);
if (deviation0 == (Real)0)
{
return result0;
}
Vector2<Real> result1;
result1.X() = (-k1 + root)*inv;
result1.Y() = AB/(mBC + mBD*result1.X());
Real deviation1 = Deviation(result1);
if (deviation1 == (Real)0)
{
return result1;
}
if (deviation0 <= deviation1)
{
if (deviation0 <= Math<Real>::ZERO_TOLERANCE)
{
return result0;
}
}
else
{
if (deviation1 <= Math<Real>::ZERO_TOLERANCE)
{
return result1;
}
}
}
else
{
// The s-equation is linear.
Vector2<Real> result;
result.X() = -k0/k1;
result.Y() = AB/(mBC + mBD*result.X());
Real deviation = Deviation(result);
if (deviation <= Math<Real>::ZERO_TOLERANCE)
{
return result;
}
}
// Point is outside the quadrilateral, return invalid.
return Vector2<Real>(Math<Real>::MAX_REAL, Math<Real>::MAX_REAL);
}
Vector2<Real> BiQuadToSqr<Real>::Transform (const Vector2<Real>& rkP)
{
Vector2<Real> kA = m_kP00 - rkP;
Real fAB = kA.DotPerp(m_kB);
Real fAC = kA.DotPerp(m_kC);
// 0 = ac*bc+(bc^2+ac*bd-ab*cd)*s+bc*bd*s^2 = k0 + k1*s + k2*s^2
Real fK0 = fAC*m_fBC;
Real fK1 = m_fBC*m_fBC + fAC*m_fBD - fAB*m_fCD;
Real fK2 = m_fBC*m_fBD;
if (Math<Real>::FAbs(fK2) >= Math<Real>::ZERO_TOLERANCE)
{
// s-equation is quadratic
Real fInv = ((Real)0.5)/fK2;
Real fDiscr = fK1*fK1 - ((Real)4.0)*fK0*fK2;
Real fRoot = Math<Real>::Sqrt(Math<Real>::FAbs(fDiscr));
Vector2<Real> kResult0;
kResult0.X() = (-fK1 - fRoot)*fInv;
kResult0.Y() = fAB/(m_fBC + m_fBD*kResult0.X());
Real fDeviation0 = Deviation(kResult0);
if (fDeviation0 == (Real)0.0)
{
return kResult0;
}
Vector2<Real> kResult1;
kResult1.X() = (-fK1 + fRoot)*fInv;
kResult1.Y() = fAB/(m_fBC + m_fBD*kResult1.X());
Real fDeviation1 = Deviation(kResult1);
if (fDeviation1 == (Real)0.0)
{
return kResult1;
}
if (fDeviation0 <= fDeviation1)
{
if (fDeviation0 <= Math<Real>::ZERO_TOLERANCE)
{
return kResult0;
}
}
else
{
if (fDeviation1 <= Math<Real>::ZERO_TOLERANCE)
{
return kResult1;
}
}
}
else
{
// s-equation is linear
Vector2<Real> kResult;
kResult.X() = -fK0/fK1;
kResult.Y() = fAB/(m_fBC + m_fBD*kResult.X());
Real fDeviation = Deviation(kResult);
if (fDeviation <= Math<Real>::ZERO_TOLERANCE)
{
return kResult;
}
}
// point is outside the quadrilateral, return invalid
return Vector2<Real>(Math<Real>::MAX_REAL,Math<Real>::MAX_REAL);
}