Real Wml::SqrDistance (const Vector3<Real>& rkPoint,
const Segment3<Real>& rkSegment, Real* pfParam)
{
Vector3<Real> kDiff = rkPoint - rkSegment.Origin();
Real fT = kDiff.Dot(rkSegment.Direction());
if ( fT <= (Real)0.0 )
{
fT = (Real)0.0;
}
else
{
Real fSqrLen= rkSegment.Direction().SquaredLength();
if ( fT >= fSqrLen )
{
fT = (Real)1.0;
kDiff -= rkSegment.Direction();
}
else
{
fT /= fSqrLen;
kDiff -= fT*rkSegment.Direction();
}
}
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
Segment3<Real> Cylinder3<Real>::GetSegment () const
{
Segment3<Real> kSegment;
kSegment.Direction() = m_fHeight*m_kDirection;
kSegment.Origin() = m_kCenter - ((Real)0.5)*kSegment.Direction();
return kSegment;
}
Real Mgc::SqrDistance (const Vector3& rkPoint, const Segment3& rkSegment,
Real* pfParam)
{
Vector3 kDiff = rkPoint - rkSegment.Origin();
Real fT = kDiff.Dot(rkSegment.Direction());
if ( fT <= 0.0f )
{
fT = 0.0f;
}
else
{
Real fSqrLen= rkSegment.Direction().SquaredLength();
if ( fT >= fSqrLen )
{
fT = 1.0f;
kDiff -= rkSegment.Direction();
}
else
{
fT /= fSqrLen;
kDiff -= fT*rkSegment.Direction();
}
}
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
Real DistTriangle3Rectangle3<Real>::GetSquared ()
{
// Compare edges of triangle to the interior of rectangle.
Real sqrDist = Math<Real>::MAX_REAL, sqrDistTmp;
Segment3<Real> edge;
int i0, i1;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
edge.Center = ((Real)0.5)*(mTriangle->V[i0] +
mTriangle->V[i1]);
edge.Direction = mTriangle->V[i1] - mTriangle->V[i0];
edge.Extent = ((Real)0.5)*edge.Direction.Normalize();
edge.ComputeEndPoints();
DistSegment3Rectangle3<Real> querySR(edge, *mRectangle);
sqrDistTmp = querySR.GetSquared();
if (sqrDistTmp < sqrDist)
{
// The triangle point is reported in mClosestPoint0 and the
// rectangle point is reported in mClosestPoint1. The querySR
// calculator is for triangleEdge-rectangle, so GetClosestPoint0()
// and GetClosestPoint1() must be called as listed next.
mClosestPoint0 = querySR.GetClosestPoint0();
mClosestPoint1 = querySR.GetClosestPoint1();
sqrDist = sqrDistTmp;
}
}
// Compare edges of rectangle to the interior of triangle.
for (i1 = 0; i1 < 2; ++i1)
{
for (i0 = -1; i0 <= 1; i0 += 2)
{
edge.Center = mRectangle->Center +
(i0*mRectangle->Extent[1-i1]) *
mRectangle->Axis[1-i1];
edge.Direction = mRectangle->Axis[i1];
edge.Extent = mRectangle->Extent[i1];
edge.ComputeEndPoints();
DistSegment3Triangle3<Real> queryST(edge, *mTriangle);
sqrDistTmp = queryST.GetSquared();
if (sqrDistTmp < sqrDist)
{
// The triangle point is reported in mClosestPoint0 and the
// rectangle point is reported in mClosestPoint1. The queryST
// calculator is for rectangleEdge-triangle, so
// GetClosestPoint1() and GetClosestPoint0() must be called as
// listed next.
mClosestPoint0 = queryST.GetClosestPoint1();
mClosestPoint1 = queryST.GetClosestPoint0();
sqrDist = sqrDistTmp;
}
}
}
return sqrDist;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Segment3& rkSeg, const Box3& rkBox,
Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
#ifdef _DEBUG
// The four parameters pointers are either all non-null or all null.
if ( pfLParam )
{
assert( pfBParam0 && pfBParam1 && pfBParam2 );
}
else
{
assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
}
#endif
Line3 kLine;
kLine.Origin() = rkSeg.Origin();
kLine.Direction() = rkSeg.Direction();
Real fLP, fBP0, fBP1, fBP2;
Real fSqrDistance = SqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
if ( fLP >= 0.0f )
{
if ( fLP <= 1.0f )
{
if ( pfLParam )
{
*pfLParam = fLP;
*pfBParam0 = fBP0;
*pfBParam1 = fBP1;
*pfBParam2 = fBP2;
}
return fSqrDistance;
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin()+rkSeg.Direction(),
rkBox,pfBParam0,pfBParam1,pfBParam2);
if ( pfLParam )
*pfLParam = 1.0f;
return fSqrDistance;
}
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin(),rkBox,pfBParam0,
pfBParam1,pfBParam2);
if ( pfLParam )
*pfLParam = 0.0f;
return fSqrDistance;
}
}
//----------------------------------------------------------------------------
float VertexCollapse::GetWeight (int iM, int iZ, int iP, Vector3* akVertex)
{
Segment3 kSegment;
kSegment.Origin() = akVertex[iM];
kSegment.Direction() = akVertex[iP] - akVertex[iM];
float fSqrDist = SqrDistance(akVertex[iZ],kSegment);
float fSqrLen = kSegment.Direction().SquaredLength();
return ( fSqrLen > 0.0f ? fSqrDist/fSqrLen : FLT_MAX );
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Segment3& rkSegment,
const Triangle3& rkTriangle, Vector3& rkPoint)
{
Real fSegP;
if ( SqrDistance(rkSegment,rkTriangle,&fSegP) <= gs_fEpsilon )
{
rkPoint = rkSegment.Origin() + fSegP*rkSegment.Direction();
return true;
}
return false;
}
Real DistRectangle3Rectangle3<Real>::GetSquared ()
{
// Compare edges of rectangle0 to the interior of rectangle1.
Real sqrDist = Math<Real>::MAX_REAL, sqrDistTmp;
Segment3<Real> edge;
int i0, i1;
for (i1 = 0; i1 < 2; ++i1)
{
for (i0 = -1; i0 <= 1; i0 += 2)
{
edge.Center = mRectangle0->Center +
(i0*mRectangle0->Extent[1-i1]) *
mRectangle0->Axis[1-i1];
edge.Direction = mRectangle0->Axis[i1];
edge.Extent = mRectangle0->Extent[i1];
edge.ComputeEndPoints();
DistSegment3Rectangle3<Real> querySR(edge, *mRectangle1);
sqrDistTmp = querySR.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = querySR.GetClosestPoint0();
mClosestPoint1 = querySR.GetClosestPoint1();
sqrDist = sqrDistTmp;
}
}
}
// Compare edges of rectangle1 to the interior of rectangle0.
for (i1 = 0; i1 < 2; ++i1)
{
for (i0 = -1; i0 <= 1; i0 += 2)
{
edge.Center = mRectangle1->Center +
(i0*mRectangle1->Extent[1-i1]) *
mRectangle1->Axis[1-i1];
edge.Direction = mRectangle1->Axis[i1];
edge.Extent = mRectangle1->Extent[i1];
edge.ComputeEndPoints();
DistSegment3Rectangle3<Real> querySR(edge, *mRectangle0);
sqrDistTmp = querySR.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = querySR.GetClosestPoint0();
mClosestPoint1 = querySR.GetClosestPoint1();
sqrDist = sqrDistTmp;
}
}
}
return sqrDist;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Segment3& rkSegment, const Box3& rkBox,
int& riQuantity, Vector3 akPoint[2])
{
// convert segment to box coordinates
Vector3 kDiff = rkSegment.Origin() - rkBox.Center();
Vector3 kOrigin(
kDiff.Dot(rkBox.Axis(0)),
kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2))
);
Vector3 kDirection(
rkSegment.Direction().Dot(rkBox.Axis(0)),
rkSegment.Direction().Dot(rkBox.Axis(1)),
rkSegment.Direction().Dot(rkBox.Axis(2))
);
Real fT0 = 0.0f, fT1 = 1.0f;
bool bIntersects = FindIntersection(kOrigin,kDirection,rkBox.Extents(),
fT0,fT1);
if ( bIntersects )
{
if ( fT0 > 0.0f )
{
if ( fT1 < 1.0f )
{
riQuantity = 2;
akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
akPoint[1] = rkSegment.Origin() + fT1*rkSegment.Direction();
}
else
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
}
}
else // fT0 == 0
{
if ( fT1 < 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin() + fT1*rkSegment.Direction();
}
else // fT1 == 1
{
// segment entirely in box
riQuantity = 0;
}
}
}
else
{
riQuantity = 0;
}
return bIntersects;
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Segment3& rkSegment, const Box3& rkBox)
{
Real fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
Vector3 kSDir = 0.5f*rkSegment.Direction();
Vector3 kSCen = rkSegment.Origin() + kSDir;
Vector3 kDiff = kSCen - rkBox.Center();
fAWdU[0] = Math::FAbs(kSDir.Dot(rkBox.Axis(0)));
fADdU[0] = Math::FAbs(kDiff.Dot(rkBox.Axis(0)));
fRhs = rkBox.Extent(0) + fAWdU[0];
if ( fADdU[0] > fRhs )
return false;
fAWdU[1] = Math::FAbs(kSDir.Dot(rkBox.Axis(1)));
fADdU[1] = Math::FAbs(kDiff.Dot(rkBox.Axis(1)));
fRhs = rkBox.Extent(1) + fAWdU[1];
if ( fADdU[1] > fRhs )
return false;
fAWdU[2] = Math::FAbs(kSDir.Dot(rkBox.Axis(2)));
fADdU[2] = Math::FAbs(kDiff.Dot(rkBox.Axis(2)));
fRhs = rkBox.Extent(2) + fAWdU[2];
if ( fADdU[2] > fRhs )
return false;
Vector3 kWxD = kSDir.Cross(kDiff);
fAWxDdU[0] = Math::FAbs(kWxD.Dot(rkBox.Axis(0)));
fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
if ( fAWxDdU[0] > fRhs )
return false;
fAWxDdU[1] = Math::FAbs(kWxD.Dot(rkBox.Axis(1)));
fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
if ( fAWxDdU[1] > fRhs )
return false;
fAWxDdU[2] = Math::FAbs(kWxD.Dot(rkBox.Axis(2)));
fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
if ( fAWxDdU[2] > fRhs )
return false;
return true;
}
bool Wml::FindIntersection (const Segment3<Real>& rkSegment,
const Cylinder3<Real>& rkCylinder, int& riQuantity,
Vector3<Real> akPoint[2])
{
Real afT[2];
if ( rkCylinder.Capped() )
{
riQuantity = Find(rkSegment.Origin(),rkSegment.Direction(),
rkCylinder,afT);
}
else
{
riQuantity = FindHollow(rkSegment.Origin(),rkSegment.Direction(),
rkCylinder,afT);
}
int iClipQuantity = 0;
for (int i = 0; i < riQuantity; i++)
{
if ( (Real)0.0 <= afT[i] && afT[i] <= (Real)1.0 )
{
akPoint[iClipQuantity++] = rkSegment.Origin() +
afT[i]*rkSegment.Direction();
}
}
riQuantity = iClipQuantity;
return riQuantity > 0;
}
Real DistTriangle3Triangle3<Real>::GetSquared ()
{
// Compare edges of triangle0 to the interior of triangle1.
Real sqrDist = Math<Real>::MAX_REAL, sqrDistTmp;
Segment3<Real> edge;
Real ratio;
int i0, i1;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
edge.Center = ((Real)0.5)*(mTriangle0->V[i0] +
mTriangle0->V[i1]);
edge.Direction = mTriangle0->V[i1] - mTriangle0->V[i0];
edge.Extent = ((Real)0.5)*edge.Direction.Normalize();
edge.ComputeEndPoints();
DistSegment3Triangle3<Real> queryST(edge, *mTriangle1);
sqrDistTmp = queryST.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = queryST.GetClosestPoint0();
mClosestPoint1 = queryST.GetClosestPoint1();
sqrDist = sqrDistTmp;
ratio = queryST.GetSegmentParameter()/edge.Extent;
mTriangleBary0[i0] = ((Real)0.5)*((Real)1 - ratio);
mTriangleBary0[i1] = (Real)1 - mTriangleBary0[i0];
mTriangleBary0[3-i0-i1] = (Real)0;
mTriangleBary1[0] = queryST.GetTriangleBary(0);
mTriangleBary1[1] = queryST.GetTriangleBary(1);
mTriangleBary1[2] = queryST.GetTriangleBary(2);
if (sqrDist <= Math<Real>::ZERO_TOLERANCE)
{
return (Real)0;
}
}
}
// Compare edges of triangle1 to the interior of triangle0.
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
edge.Center = ((Real)0.5)*(mTriangle1->V[i0] +
mTriangle1->V[i1]);
edge.Direction = mTriangle1->V[i1] - mTriangle1->V[i0];
edge.Extent = ((Real)0.5)*edge.Direction.Normalize();
edge.ComputeEndPoints();
DistSegment3Triangle3<Real> queryST(edge, *mTriangle0);
sqrDistTmp = queryST.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = queryST.GetClosestPoint0();
mClosestPoint1 = queryST.GetClosestPoint1();
sqrDist = sqrDistTmp;
ratio = queryST.GetSegmentParameter()/edge.Extent;
mTriangleBary1[i0] = ((Real)0.5)*((Real)1 - ratio);
mTriangleBary1[i1] = (Real)1 - mTriangleBary1[i0];
mTriangleBary1[3-i0-i1] = (Real)0;
mTriangleBary0[0] = queryST.GetTriangleBary(0);
mTriangleBary0[1] = queryST.GetTriangleBary(1);
mTriangleBary0[2] = queryST.GetTriangleBary(2);
if (sqrDist <= Math<Real>::ZERO_TOLERANCE)
{
return (Real)0;
}
}
}
return sqrDist;
}
bool Wml::FindIntersection (const Triangle3<Real>& rkTri,
const Vector3<Real>& rkTriVelocity, const Sphere3<Real>& rkSphere,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// triangle vertices
Vector3<Real> akV[3] =
{
rkTri.Origin(),
rkTri.Origin() + rkTri.Edge0(),
rkTri.Origin() + rkTri.Edge1()
};
// triangle edges
Vector3<Real> akE[3] =
{
akV[1] - akV[0],
akV[2] - akV[1],
akV[0] - akV[2]
};
// triangle normal
Vector3<Real> kN = akE[1].Cross(akE[0]);
// sphere center projection on triangle normal
Real fNdC = kN.Dot(rkSphere.Center());
// Radius projected length in normal direction. This defers the square
// root to normalize kN until absolutely needed.
Real fNormRadiusSqr =
kN.SquaredLength()*rkSphere.Radius()*rkSphere.Radius();
// triangle projection on triangle normal
Real fNdT = kN.Dot(akV[0]);
// Distance from sphere to triangle along the normal
Real fDist = fNdC - fNdT;
// normals for the plane formed by edge i and the triangle normal
Vector3<Real> akExN[3] =
{
akE[0].Cross(kN),
akE[1].Cross(kN),
akE[2].Cross(kN)
};
Segment3<Real> kSeg;
if ( fDist*fDist <= fNormRadiusSqr )
{
// sphere currently intersects the plane of the triangle
// see which edges the sphere center is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
{
bInside[i] = ( akExN[i].Dot(rkSphere.Center()) >=
akExN[i].Dot(akV[i]) );
}
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// triangle inside sphere
return false;
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 1,2
return FindTriSphrCoplanarIntersection(2,akV,kN,akExN[2],
akE[2],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 2,0
return FindTriSphrCoplanarIntersection(0,akV,kN,akExN[0],
akE[0],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edges 0,1
return FindTriSphrCoplanarIntersection(1,akV,kN,akExN[1],
akE[1],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
else
{
// sphere does not currently intersect the plane of the triangle
// sphere moving, triangle stationary
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
// Find point of intersection of the sphere and the triangle
// plane. Where this point occurs on the plane relative to the
// triangle determines the potential kind of intersection.
kN.Normalize();
// Point on sphere we care about intersecting the triangle plane
Vector3<Real> kSpherePoint;
// Which side of the triangle is the sphere on?
if ( fNdC > fNdT )
{
// positive side
if ( kVel.Dot(kN) >= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() - rkSphere.Radius()*kN;
}
else
{
// negative side
if ( kVel.Dot(kN) <= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() + rkSphere.Radius()*kN;
}
// find intersection of velocity ray and triangle plane
// project ray and plane onto the plane normal
Real fPlane = kN.Dot(akV[0]);
Real fPoint = kN.Dot(kSpherePoint);
Real fVel = kN.Dot(kVel);
Real fTime = (fPlane - fPoint)/fVel;
// where this intersects
Vector3<Real> kIntrPoint = kSpherePoint + fTime*kVel;
// see which edges this intersection point is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
bInside[i] = (akExN[i].Dot(kIntrPoint) >= akExN[i].Dot(akV[i]));
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// intersects face at time fTime
if ( fTime > fTMax )
{
// intersection after tMax
return false;
}
else
{
rfTFirst = fTime;
riQuantity = 1;
// kIntrPoint is the point in space, assuming that
// TriVel is 0. Re-adjust the point to where it
// should be, were it not.
akP[0] = kIntrPoint + fTime*rkTriVelocity;
return true;
}
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 2
return FindSphereVertexIntersection(akV[2],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 0
return FindSphereVertexIntersection(akV[0],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with vertex 1
return FindSphereVertexIntersection(akV[1],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
}
bool Mgc::FindIntersection (const Segment3& rkSegment, const Sphere& rkSphere,
int& riQuantity, Vector3 akPoint[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
Vector3 kDiff = rkSegment.Origin() - rkSphere.Center();
Real fA = rkSegment.Direction().SquaredLength();
Real fB = kDiff.Dot(rkSegment.Direction());
Real fC = kDiff.SquaredLength() -
rkSphere.Radius()*rkSphere.Radius();
// no intersection if Q(t) has no real roots
Real afT[2];
Real fDiscr = fB*fB - fA*fC;
if ( fDiscr < 0.0f )
{
riQuantity = 0;
return false;
}
else if ( fDiscr > 0.0f )
{
Real fRoot = Math::Sqrt(fDiscr);
Real fInvA = 1.0f/fA;
afT[0] = (-fB - fRoot)*fInvA;
afT[1] = (-fB + fRoot)*fInvA;
// assert: t0 < t1 since A > 0
if ( afT[0] > 1.0f || afT[1] < 0.0f )
{
riQuantity = 0;
return false;
}
else if ( afT[0] >= 0.0f )
{
if ( afT[1] > 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
return true;
}
else
{
riQuantity = 2;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
akPoint[1] = rkSegment.Origin()+afT[1]*rkSegment.Direction();
return true;
}
}
else // afT[1] >= 0
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[1]*rkSegment.Direction();
return true;
}
}
else
{
afT[0] = -fB/fA;
if ( 0.0f <= afT[0] && afT[0] <= 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
return true;
}
else
{
riQuantity = 0;
return false;
}
}
}
Real DistLine3Rectangle3<Real>::GetSquared ()
{
// Test if line intersects rectangle. If so, the squared distance is
// zero.
Vector3<Real> N = mRectangle->Axis[0].Cross( mRectangle->Axis[1] );
Real NdD = N.Dot( mLine->Direction );
if ( Math<Real>::FAbs( NdD ) > Math<Real>::ZERO_TOLERANCE )
{
// The line and rectangle are not parallel, so the line intersects
// the plane of the rectangle.
Vector3<Real> diff = mLine->Origin - mRectangle->Center;
Vector3<Real> U, V;
Vector3<Real>::GenerateComplementBasis( U, V, mLine->Direction );
Real UdD0 = U.Dot( mRectangle->Axis[0] );
Real UdD1 = U.Dot( mRectangle->Axis[1] );
Real UdPmC = U.Dot( diff );
Real VdD0 = V.Dot( mRectangle->Axis[0] );
Real VdD1 = V.Dot( mRectangle->Axis[1] );
Real VdPmC = V.Dot( diff );
Real invDet = ( ( Real )1 ) / ( UdD0 * VdD1 - UdD1 * VdD0 );
// Rectangle coordinates for the point of intersection.
Real s0 = ( VdD1 * UdPmC - UdD1 * VdPmC ) * invDet;
Real s1 = ( UdD0 * VdPmC - VdD0 * UdPmC ) * invDet;
if ( Math<Real>::FAbs( s0 ) <= mRectangle->Extent[0]
&& Math<Real>::FAbs( s1 ) <= mRectangle->Extent[1] )
{
// Line parameter for the point of intersection.
Real DdD0 = mLine->Direction.Dot( mRectangle->Axis[0] );
Real DdD1 = mLine->Direction.Dot( mRectangle->Axis[1] );
Real DdDiff = mLine->Direction.Dot( diff );
mLineParameter = s0 * DdD0 + s1 * DdD1 - DdDiff;
// Rectangle coordinates for the point of intersection.
mRectCoord[0] = s0;
mRectCoord[1] = s1;
// The intersection point is inside or on the rectangle.
mClosestPoint0 = mLine->Origin +
mLineParameter * mLine->Direction;
mClosestPoint1 = mRectangle->Center +
s0 * mRectangle->Axis[0] + s1 * mRectangle->Axis[1];
return ( Real )0;
}
}
// Either (1) the line is not parallel to the rectangle and the point of
// intersection of the line and the plane of the rectangle is outside the
// rectangle or (2) the line and rectangle are parallel. Regardless, the
// closest point on the rectangle is on an edge of the rectangle. Compare
// the line to all four edges of the rectangle.
Real sqrDist = Math<Real>::MAX_REAL;
Vector3<Real> scaledDir[2] =
{
mRectangle->Extent[0]* mRectangle->Axis[0],
mRectangle->Extent[1]* mRectangle->Axis[1]
};
for ( int i1 = 0; i1 < 2; ++i1 )
{
for ( int i0 = 0; i0 < 2; ++i0 )
{
Segment3<Real> segment;
segment.Center = mRectangle->Center +
( ( Real )( 2 * i0 - 1 ) ) * scaledDir[i1];
segment.Direction = mRectangle->Axis[1 - i1];
segment.Extent = mRectangle->Extent[1 - i1];
segment.ComputeEndPoints();
DistLine3Segment3<Real> queryLS( *mLine, segment );
Real sqrDistTmp = queryLS.GetSquared();
if ( sqrDistTmp < sqrDist )
{
mClosestPoint0 = queryLS.GetClosestPoint0();
mClosestPoint1 = queryLS.GetClosestPoint1();
sqrDist = sqrDistTmp;
mLineParameter = queryLS.GetLineParameter();
Real ratio = queryLS.GetSegmentParameter() / segment.Extent;
mRectCoord[0] = mRectangle->Extent[0] * ( ( 1 - i1 ) * ( 2 * i0 - 1 ) +
i1 * ratio );
mRectCoord[1] = mRectangle->Extent[1] * ( ( 1 - i0 ) * ( 2 * i1 - 1 ) +
i0 * ratio );
}
}
}
return sqrDist;
}
Real Wml::SqrDistance (const Rectangle3<Real>& rkRct0,
const Rectangle3<Real>& rkRct1, Real* pfRct0P0, Real* pfRct0P1,
Real* pfRct1P0, Real* pfRct1P1)
{
Real fS, fT, fS0, fT0; // rct0 parameters
Real fU, fV, fU0, fV0; // rct1 parameters
Real fSqrDist, fSqrDist0;
Segment3<Real> kSeg;
// compare edges of rct0 against all of rct1
kSeg.Origin() = rkRct0.Origin();
kSeg.Direction() = rkRct0.Edge0();
fSqrDist = SqrDistance(kSeg,rkRct1,&fS,&fU,&fV);
fT = (Real)0.0;
kSeg.Direction() = rkRct0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
fS0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
fS0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge1();
kSeg.Direction() = rkRct0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fS0,&fU0,&fV0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of pgm1 against all of pgm0
kSeg.Origin() = rkRct1.Origin();
kSeg.Direction() = rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
fV0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkRct1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
fU0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
fU0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge1();
kSeg.Direction() = rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
fV0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfRct0P0 )
*pfRct0P0 = fS;
if ( pfRct0P1 )
*pfRct0P1 = fT;
if ( pfRct1P0 )
*pfRct1P0 = fU;
if ( pfRct1P1 )
*pfRct1P1 = fV;
return fSqrDist;
}
static bool FindTriSphrCoplanarIntersection (int iVertex,
const Vector3<Real> akV[3], const Vector3<Real>& /* rkNormal */,
const Vector3<Real>& rkSideNorm, const Vector3<Real>& rkSide,
const Sphere3<Real>& rkSphere, const Vector3<Real>& rkTriVelocity,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// TO DO. The parameter rkNormal is not used here. Is this an error?
// Or does the caller make some adjustments to the other inputs to
// account for the normal?
// iVertex is the "hinge" vertex that the two potential edges that can
// be intersected by the sphere connect to, and it indexes into akV.
// rkSideNorm is the normal of the plane formed by (iVertex,iVertex+1)
// and the tri norm, passed so as not to recalculate
// check for intersections at time 0
Vector3<Real> kDist = akV[iVertex] - rkSphere.Center();
if ( kDist.SquaredLength() < rkSphere.Radius()*rkSphere.Radius() )
{
// already intersecting that vertex
rfTFirst = (Real)0.0;
return false;
}
// Tri stationary, sphere moving
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
// check for easy out
if ( kVel.Dot(kDist) <= (Real)0.0 )
{
// moving away
return false;
}
// find intersection of velocity ray and side normal
// project ray and plane onto the plane normal
Real fPlane = rkSideNorm.Dot(akV[iVertex]);
Real fCenter = rkSideNorm.Dot(rkSphere.Center());
Real fVel = rkSideNorm.Dot(kVel);
Real fFactor = (fPlane - fCenter)/fVel;
Vector3<Real> kPoint = rkSphere.Center() + fFactor*kVel;
// now, find which side of the hinge vertex this lies by projecting
// both the vertex and this new point onto the triangle edge (the same
// edge whose "normal" was used to find this point)
Real fVertex = rkSide.Dot(akV[iVertex]);
Real fPoint = rkSide.Dot(kPoint);
Segment3<Real> kSeg;
if ( fPoint >= fVertex )
{
// intersection with edge (iVertex,iVertex+1)
kSeg.Origin() = akV[iVertex];
kSeg.Direction() = akV[(iVertex+1)%3] - akV[iVertex];
}
else
{
// intersection with edge (iVertex-1,iVertex)
if ( iVertex != 0 )
kSeg.Origin() = akV[iVertex-1];
else
kSeg.Origin() = akV[2];
kSeg.Direction() = akV[iVertex] - kSeg.Origin();
}
// This could be either an sphere-edge or a sphere-vertex intersection
// (this test isn't enough to differentiate), so use the full-on
// line-sphere test.
return FindIntersection(kSeg,rkTriVelocity,rkSphere,rkSphVelocity,
rfTFirst,fTMax,riQuantity,akP);
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Parallelogram3& rkPgm0,
const Parallelogram3& rkPgm1, Real* pfPgm0P0, Real* pfPgm0P1,
Real* pfPgm1P0, Real* pfPgm1P1)
{
Real fS, fT, fS0, fT0; // pgm0 parameters
Real fU, fV, fU0, fV0; // pgm1 parameters
Real fSqrDist, fSqrDist0;
Segment3 kSeg;
// compare edges of pgm0 against all of pgm1
kSeg.Origin() = rkPgm0.Origin();
kSeg.Direction() = rkPgm0.Edge0();
fSqrDist = SqrDistance(kSeg,rkPgm1,&fS,&fU,&fV);
fT = 0.0f;
kSeg.Direction() = rkPgm0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
fS0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
fS0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge1();
kSeg.Direction() = rkPgm0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fS0,&fU0,&fV0);
fT0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of pgm1 against all of pgm0
kSeg.Origin() = rkPgm1.Origin();
kSeg.Direction() = rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
fV0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkPgm1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
fU0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
fU0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge1();
kSeg.Direction() = rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
fV0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfPgm0P0 )
*pfPgm0P0 = fS;
if ( pfPgm0P1 )
*pfPgm0P1 = fT;
if ( pfPgm1P0 )
*pfPgm1P0 = fU;
if ( pfPgm1P1 )
*pfPgm1P1 = fV;
return Math::FAbs(fSqrDist);
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Triangle3& rkTri0, const Triangle3& rkTri1,
Real* pfTri0P0, Real* pfTri0P1, Real* pfTri1P0, Real* pfTri1P1)
{
Real fS, fT, fU, fV, fS0, fT0, fU0, fV0, fSqrDist, fSqrDist0;
Segment3 kSeg;
// compare edges of tri0 against all of tri1
kSeg.Origin() = rkTri0.Origin();
kSeg.Direction() = rkTri0.Edge0();
fSqrDist = SqrDistance(kSeg,rkTri1,&fS,&fU,&fV);
fT = 0.0f;
kSeg.Direction() = rkTri0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri0.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 1.0f-fT0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of tri1 against all of tri0
kSeg.Origin() = rkTri1.Origin();
kSeg.Direction() = rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fU0,&fS0,&fT0);
fV0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkTri1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri1.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 1.0f-fV0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfTri0P0 )
*pfTri0P0 = fS;
if ( pfTri0P1 )
*pfTri0P1 = fT;
if ( pfTri1P0 )
*pfTri1P0 = fU;
if ( pfTri1P1 )
*pfTri1P1 = fV;
return Math::FAbs(fSqrDist);
}
Real DistLine3Triangle3<Real>::GetSquared ()
{
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3<Real> edge0 = mTriangle->V[1] - mTriangle->V[0];
Vector3<Real> edge1 = mTriangle->V[2] - mTriangle->V[0];
Vector3<Real> normal = edge0.UnitCross(edge1);
Real NdD = normal.Dot(mLine->Direction);
if (Math<Real>::FAbs(NdD) > Math<Real>::ZERO_TOLERANCE)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3<Real> diff = mLine->Origin - mTriangle->V[0];
Vector3<Real> U, V;
Vector3<Real>::GenerateComplementBasis(U, V, mLine->Direction);
Real UdE0 = U.Dot(edge0);
Real UdE1 = U.Dot(edge1);
Real UdDiff = U.Dot(diff);
Real VdE0 = V.Dot(edge0);
Real VdE1 = V.Dot(edge1);
Real VdDiff = V.Dot(diff);
Real invDet = ((Real)1)/(UdE0*VdE1 - UdE1*VdE0);
// Barycentric coordinates for the point of intersection.
Real b1 = (VdE1*UdDiff - UdE1*VdDiff)*invDet;
Real b2 = (UdE0*VdDiff - VdE0*UdDiff)*invDet;
Real b0 = (Real)1 - b1 - b2;
if (b0 >= (Real)0 && b1 >= (Real)0 && b2 >= (Real)0)
{
// Line parameter for the point of intersection.
Real DdE0 = mLine->Direction.Dot(edge0);
Real DdE1 = mLine->Direction.Dot(edge1);
Real DdDiff = mLine->Direction.Dot(diff);
mLineParameter = b1*DdE0 + b2*DdE1 - DdDiff;
// Barycentric coordinates for the point of intersection.
mTriangleBary[0] = b0;
mTriangleBary[1] = b1;
mTriangleBary[2] = b2;
// The intersection point is inside or on the triangle.
mClosestPoint0 = mLine->Origin +
mLineParameter*mLine->Direction;
mClosestPoint1 = mTriangle->V[0] + b1*edge0 + b2*edge1;
return (Real)0;
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
Real sqrDist = Math<Real>::MAX_REAL;
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
Segment3<Real> segment;
segment.Center = ((Real)0.5)*(mTriangle->V[i0] +
mTriangle->V[i1]);
segment.Direction = mTriangle->V[i1] - mTriangle->V[i0];
segment.Extent = ((Real)0.5)*segment.Direction.Normalize();
segment.ComputeEndPoints();
DistLine3Segment3<Real> queryLS(*mLine, segment);
Real sqrDistTmp = queryLS.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = queryLS.GetClosestPoint0();
mClosestPoint1 = queryLS.GetClosestPoint1();
sqrDist = sqrDistTmp;
mLineParameter = queryLS.GetLineParameter();
Real ratio = queryLS.GetSegmentParameter()/segment.Extent;
mTriangleBary[i0] = ((Real)0.5)*((Real)1 - ratio);
mTriangleBary[i1] = (Real)1 - mTriangleBary[i0];
mTriangleBary[3-i0-i1] = (Real)0;
}
}
return sqrDist;
}