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); }