//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Line3& rkLine, const Box3& rkBox)
{
Real fAWdU[3], fAWxDdU[3], fRhs;
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kWxD = rkLine.Direction().Cross(kDiff);
fAWdU[1] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(1)));
fAWdU[2] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(2)));
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;
fAWdU[0] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(0)));
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 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;
}
void Wml::BoxProjection (const Vector3<Real>& rkAxis,
const Box3<Real>& rkBox, Real& rfMin, Real& rfMax)
{
Real fOrigin = rkAxis.Dot(rkBox.Center());
Real fMaximumExtent =
Math<Real>::FAbs(rkBox.Extent(0)*rkAxis.Dot(rkBox.Axis(0))) +
Math<Real>::FAbs(rkBox.Extent(1)*rkAxis.Dot(rkBox.Axis(1))) +
Math<Real>::FAbs(rkBox.Extent(2)*rkAxis.Dot(rkBox.Axis(2)));
rfMin = fOrigin - fMaximumExtent;
rfMax = fOrigin + fMaximumExtent;
}
bool Wml::Culled (const Plane3<Real>& rkPlane, const Box3<Real>& rkBox)
{
Real fTmp[3] =
{
rkBox.Extent(0)*(rkPlane.GetNormal().Dot(rkBox.Axis(0))),
rkBox.Extent(1)*(rkPlane.GetNormal().Dot(rkBox.Axis(1))),
rkBox.Extent(2)*(rkPlane.GetNormal().Dot(rkBox.Axis(2)))
};
Real fRadius = Math<Real>::FAbs(fTmp[0]) + Math<Real>::FAbs(fTmp[1]) +
Math<Real>::FAbs(fTmp[2]);
Real fPseudoDistance = rkPlane.DistanceTo(rkBox.Center());
return fPseudoDistance <= -fRadius;
}
//----------------------------------------------------------------------------
static void MinimalBoxForAngles (int iQuantity, const Vector3* akPoint,
Real afAngle[3], Box3& rkBox)
{
Real fCos0 = Math::Cos(afAngle[0]);
Real fSin0 = Math::Sin(afAngle[0]);
Real fCos1 = Math::Cos(afAngle[1]);
Real fSin1 = Math::Sin(afAngle[1]);
Vector3 kAxis(fCos0*fSin1,fSin0*fSin1,fCos1);
Matrix3 kRot;
kRot.FromAxisAngle(kAxis,afAngle[2]);
Vector3 kMin = akPoint[0]*kRot, kMax = kMin;
for (int i = 1; i < iQuantity; i++)
{
Vector3 kTest = akPoint[i]*kRot;
if ( kTest.x < kMin.x )
kMin.x = kTest.x;
else if ( kTest.x > kMax.x )
kMax.x = kTest.x;
if ( kTest.y < kMin.y )
kMin.y = kTest.y;
else if ( kTest.y > kMax.y )
kMax.y = kTest.y;
if ( kTest.z < kMin.z )
kMin.z = kTest.z;
else if ( kTest.z > kMax.z )
kMax.z = kTest.z;
}
Vector3 kMid = 0.5f*(kMax + kMin);
Vector3 kRng = 0.5f*(kMax - kMin);
rkBox.Center() = kRot*kMid;
rkBox.Axis(0) = kRot.GetColumn(0);
rkBox.Axis(1) = kRot.GetColumn(1);
rkBox.Axis(2) = kRot.GetColumn(2);
rkBox.Extent(0) = kRng.x;
rkBox.Extent(1) = kRng.y;
rkBox.Extent(2) = kRng.z;
}
void MinBox3<Real>::MinimalBoxForAngles (int iQuantity,
const Vector3<Real>* akPoint, Real afAngle[3], Box3<Real>& rkBox)
{
Real fCos0 = Math<Real>::Cos(afAngle[0]);
Real fSin0 = Math<Real>::Sin(afAngle[0]);
Real fCos1 = Math<Real>::Cos(afAngle[1]);
Real fSin1 = Math<Real>::Sin(afAngle[1]);
Vector3<Real> kAxis(fCos0*fSin1,fSin0*fSin1,fCos1);
Matrix3<Real> kRot(kAxis,afAngle[2]);
Vector3<Real> kMin = akPoint[0]*kRot, kMax = kMin;
for (int i = 1; i < iQuantity; i++)
{
Vector3<Real> kTest = akPoint[i]*kRot;
if ( kTest.X() < kMin.X() )
kMin.X() = kTest.X();
else if ( kTest.X() > kMax.X() )
kMax.X() = kTest.X();
if ( kTest.Y() < kMin.Y() )
kMin.Y() = kTest.Y();
else if ( kTest.Y() > kMax.Y() )
kMax.Y() = kTest.Y();
if ( kTest.Z() < kMin.Z() )
kMin.Z() = kTest.Z();
else if ( kTest.Z() > kMax.Z() )
kMax.Z() = kTest.Z();
}
Vector3<Real> kMid = ((Real)0.5)*(kMax + kMin);
Vector3<Real> kRng = ((Real)0.5)*(kMax - kMin);
rkBox.Center() = kRot*kMid;
rkBox.Axis(0) = kRot.GetColumn(0);
rkBox.Axis(1) = kRot.GetColumn(1);
rkBox.Axis(2) = kRot.GetColumn(2);
rkBox.Extent(0) = kRng.X();
rkBox.Extent(1) = kRng.Y();
rkBox.Extent(2) = kRng.Z();
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Ray3& rkRay, const Box3& rkBox)
{
Real fWdU[3], fAWdU[3], fDdU[3], fADdU[3], fAWxDdU[3], fRhs;
Vector3 kDiff = rkRay.Origin() - rkBox.Center();
fWdU[0] = rkRay.Direction().Dot(rkBox.Axis(0));
fAWdU[0] = Math::FAbs(fWdU[0]);
fDdU[0] = kDiff.Dot(rkBox.Axis(0));
fADdU[0] = Math::FAbs(fDdU[0]);
if ( fADdU[0] > rkBox.Extent(0) && fDdU[0]*fWdU[0] >= 0.0f )
return false;
fWdU[1] = rkRay.Direction().Dot(rkBox.Axis(1));
fAWdU[1] = Math::FAbs(fWdU[1]);
fDdU[1] = kDiff.Dot(rkBox.Axis(1));
fADdU[1] = Math::FAbs(fDdU[1]);
if ( fADdU[1] > rkBox.Extent(1) && fDdU[1]*fWdU[1] >= 0.0f )
return false;
fWdU[2] = rkRay.Direction().Dot(rkBox.Axis(2));
fAWdU[2] = Math::FAbs(fWdU[2]);
fDdU[2] = kDiff.Dot(rkBox.Axis(2));
fADdU[2] = Math::FAbs(fDdU[2]);
if ( fADdU[2] > rkBox.Extent(2) && fDdU[2]*fWdU[2] >= 0.0f )
return false;
Vector3 kWxD = rkRay.Direction().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 Mgc::FindIntersection (const Line3& rkLine, const Box3& rkBox,
int& riQuantity, Vector3 akPoint[2])
{
// convert line to box coordinates
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kOrigin(
kDiff.Dot(rkBox.Axis(0)),
kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2))
);
Vector3 kDirection(
rkLine.Direction().Dot(rkBox.Axis(0)),
rkLine.Direction().Dot(rkBox.Axis(1)),
rkLine.Direction().Dot(rkBox.Axis(2))
);
Real fT0 = -Math::MAX_REAL, fT1 = Math::MAX_REAL;
bool bIntersects = FindIntersection(kOrigin,kDirection,rkBox.Extents(),
fT0,fT1);
if ( bIntersects )
{
if ( fT0 != fT1 )
{
riQuantity = 2;
akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
akPoint[1] = rkLine.Origin() + fT1*rkLine.Direction();
}
else
{
riQuantity = 1;
akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
}
}
else
{
riQuantity = 0;
}
return bIntersects;
}
bool Wml::InBox (const Vector3<Real>& rkPoint, const Box3<Real>& rkBox,
Real fEpsilon)
{
Vector3<Real> kDiff = rkPoint - rkBox.Center();
for (int i = 0; i < 3; i++)
{
Real fCoeff = kDiff.Dot(rkBox.Axis(i));
if ( Math<Real>::FAbs(fCoeff) > rkBox.Extent(i) + fEpsilon )
return false;
}
return true;
}
Box3<Real> Wml::ContOrientedBox (int iQuantity, const Vector3<Real>* akPoint)
{
Box3<Real> kBox;
GaussPointsFit(iQuantity,akPoint,kBox.Center(),kBox.Axes(),
kBox.Extents());
// Let C be the box center and let U0, U1, and U2 be the box axes. Each
// input point is of the form X = C + y0*U0 + y1*U1 + y2*U2. The
// following code computes min(y0), max(y0), min(y1), max(y1), min(y2),
// and max(y2). The box center is then adjusted to be
// C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 +
// 0.5*(min(y2)+max(y2))*U2
Vector3<Real> kDiff = akPoint[0] - kBox.Center();
Real fY0Min = kDiff.Dot(kBox.Axis(0)), fY0Max = fY0Min;
Real fY1Min = kDiff.Dot(kBox.Axis(1)), fY1Max = fY1Min;
Real fY2Min = kDiff.Dot(kBox.Axis(2)), fY2Max = fY2Min;
for (int i = 1; i < iQuantity; i++)
{
kDiff = akPoint[i] - kBox.Center();
Real fY0 = kDiff.Dot(kBox.Axis(0));
if ( fY0 < fY0Min )
fY0Min = fY0;
else if ( fY0 > fY0Max )
fY0Max = fY0;
Real fY1 = kDiff.Dot(kBox.Axis(1));
if ( fY1 < fY1Min )
fY1Min = fY1;
else if ( fY1 > fY1Max )
fY1Max = fY1;
Real fY2 = kDiff.Dot(kBox.Axis(2));
if ( fY2 < fY2Min )
fY2Min = fY2;
else if ( fY2 > fY2Max )
fY2Max = fY2;
}
kBox.Center() += (((Real)0.5)*(fY0Min+fY0Max))*kBox.Axis(0) +
(((Real)0.5)*(fY1Min+fY1Max))*kBox.Axis(1) +
(((Real)0.5)*(fY2Min+fY2Max))*kBox.Axis(2);
kBox.Extent(0) = ((Real)0.5)*(fY0Max - fY0Min);
kBox.Extent(1) = ((Real)0.5)*(fY1Max - fY1Min);
kBox.Extent(2) = ((Real)0.5)*(fY2Max - fY2Min);
return kBox;
}
//----------------------------------------------------------------------------
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;
}
Vector3<Real> Wml::GetPoint (int iIndex, const Box3<Real>& rkBox)
{
Vector3<Real> kPoint = rkBox.Center();
if ( iIndex & 4 )
kPoint += rkBox.Extent(2)*rkBox.Axis(2);
else
kPoint -= rkBox.Extent(2)*rkBox.Axis(2);
if ( iIndex & 2 )
kPoint += rkBox.Extent(1)*rkBox.Axis(1);
else
kPoint -= rkBox.Extent(1)*rkBox.Axis(1);
if ( iIndex & 1 )
kPoint += rkBox.Extent(0)*rkBox.Axis(0);
else
kPoint -= rkBox.Extent(0)*rkBox.Axis(0);
return kPoint;
}
bool Wml::ContOrientedBox (int iQuantity, const Vector3<Real>* akPoint,
const bool* abValid, Box3<Real>& rkBox)
{
if ( !GaussPointsFit(iQuantity,akPoint,abValid,rkBox.Center(),
rkBox.Axes(),rkBox.Extents()) )
{
return false;
}
// Let C be the box center and let U0, U1, and U2 be the box axes. Each
// input point is of the form X = C + y0*U0 + y1*U1 + y2*U2. The
// following code computes min(y0), max(y0), min(y1), max(y1), min(y2),
// and max(y2). The box center is then adjusted to be
// C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 +
// 0.5*(min(y2)+max(y2))*U2
// get first valid vertex
Vector3<Real> kDiff;
Real fY0Min = (Real)0.0, fY0Max = (Real)0.0;
Real fY1Min = (Real)0.0, fY1Max = (Real)0.0;
Real fY2Min = (Real)0.0, fY2Max = (Real)0.0;
int i;
for (i = 0; i < iQuantity; i++)
{
if ( abValid[i] )
{
kDiff = akPoint[i] - rkBox.Center();
fY0Min = kDiff.Dot(rkBox.Axis(0));
fY0Max = fY0Min;
fY1Min = kDiff.Dot(rkBox.Axis(1));
fY1Max = fY1Min;
fY2Min = kDiff.Dot(rkBox.Axis(2));
fY2Max = fY2Min;
break;
}
}
for (i++; i < iQuantity; i++)
{
if ( abValid[i] )
{
kDiff = akPoint[i] - rkBox.Center();
Real fY0 = kDiff.Dot(rkBox.Axis(0));
if ( fY0 < fY0Min )
fY0Min = fY0;
else if ( fY0 > fY0Max )
fY0Max = fY0;
Real fY1 = kDiff.Dot(rkBox.Axis(1));
if ( fY1 < fY1Min )
fY1Min = fY1;
else if ( fY1 > fY1Max )
fY1Max = fY1;
Real fY2 = kDiff.Dot(rkBox.Axis(2));
if ( fY2 < fY2Min )
fY2Min = fY2;
else if ( fY2 > fY2Max )
fY2Max = fY2;
}
}
rkBox.Center() += (0.5f*(fY0Min+fY0Max))*rkBox.Axis(0)
+ (0.5f*(fY1Min+fY1Max))*rkBox.Axis(1) +
(0.5f*(fY2Min+fY2Max))*rkBox.Axis(2);
rkBox.Extent(0) = 0.5f*(fY0Max - fY0Min);
rkBox.Extent(1) = 0.5f*(fY1Max - fY1Min);
rkBox.Extent(2) = 0.5f*(fY2Max - fY2Min);
return true;
}
Box3<Real> Wml::MergeBoxes (const Box3<Real>& rkBox0,
const Box3<Real>& rkBox1)
{
// construct a box that contains the input boxes
Box3<Real> kBox;
// The first guess at the box center. This value will be updated later
// after the input box vertices are projected onto axes determined by an
// average of box axes.
kBox.Center() = ((Real)0.5)*(rkBox0.Center() + rkBox1.Center());
// A box's axes, when viewed as the columns of a matrix, form a rotation
// matrix. The input box axes are converted to quaternions. The average
// quaternion is computed, then normalized to unit length. The result is
// the slerp of the two input quaternions with t-value of 1/2. The result
// is converted back to a rotation matrix and its columns are selected as
// the merged box axes.
Quaternion<Real> kQ0, kQ1;
kQ0.FromRotationMatrix(rkBox0.Axes());
kQ1.FromRotationMatrix(rkBox1.Axes());
if ( kQ0.Dot(kQ1) < 0.0f )
kQ1 = -kQ1;
Quaternion<Real> kQ = kQ0 + kQ1;
Real fInvLength = Math<Real>::InvSqrt(kQ.Dot(kQ));
kQ = fInvLength*kQ;
kQ.ToRotationMatrix(kBox.Axes());
// Project the input box vertices onto the merged-box axes. Each axis
// D[i] containing the current center C has a minimum projected value
// pmin[i] and a maximum projected value pmax[i]. The corresponding end
// points on the axes are C+pmin[i]*D[i] and C+pmax[i]*D[i]. The point C
// is not necessarily the midpoint for any of the intervals. The actual
// box center will be adjusted from C to a point C' that is the midpoint
// of each interval,
// C' = C + sum_{i=0}^2 0.5*(pmin[i]+pmax[i])*D[i]
// The box extents are
// e[i] = 0.5*(pmax[i]-pmin[i])
int i, j;
Real fDot;
Vector3<Real> akVertex[8], kDiff;
Vector3<Real> kMin = Vector3<Real>::ZERO;
Vector3<Real> kMax = Vector3<Real>::ZERO;
rkBox0.ComputeVertices(akVertex);
for (i = 0; i < 8; i++)
{
kDiff = akVertex[i] - kBox.Center();
for (j = 0; j < 3; j++)
{
fDot = kDiff.Dot(kBox.Axis(j));
if ( fDot > kMax[j] )
kMax[j] = fDot;
else if ( fDot < kMin[j] )
kMin[j] = fDot;
}
}
rkBox1.ComputeVertices(akVertex);
for (i = 0; i < 8; i++)
{
kDiff = akVertex[i] - kBox.Center();
for (j = 0; j < 3; j++)
{
fDot = kDiff.Dot(kBox.Axis(j));
if ( fDot > kMax[j] )
kMax[j] = fDot;
else if ( fDot < kMin[j] )
kMin[j] = fDot;
}
}
// [kMin,kMax] is the axis-aligned box in the coordinate system of the
// merged box axes. Update the current box center to be the center of
// the new box. Compute the extens based on the new center.
for (j = 0; j < 3; j++)
{
kBox.Center() += (((Real)0.5)*(kMax[j]+kMin[j]))*kBox.Axis(j);
kBox.Extent(j) = ((Real)0.5)*(kMax[j]-kMin[j]);
}
return kBox;
}
void Wml::GetBoxConfiguration (const Vector3<Real>& rkAxis,
const Box3<Real>& rkBox, ContactConfig<Real>& rkConfig)
{
// Description of coordinate ordering scheme for ContactConfig.m_aiIndex.
//
// Vertex number (up/down) vs. sign of extent (only matters in mapping
// back)
// 012
// 0 ---
// 1 +--
// 2 -+-
// 3 ++-
// 4 --+
// 5 +-+
// 6 -++
// 7 +++
//
// When it returns an ordering in the ContactConfig, it is also
// guarenteed to be in-order (if 4 vertices, then they are guarunteed in
// an order that will create a box, e.g. 0,1,3,2).
//
// assert: akAxis is an array containing unit length vectors
Real afAxis[3] =
{
rkAxis.Dot(rkBox.Axis(0)),
rkAxis.Dot(rkBox.Axis(1)),
rkAxis.Dot(rkBox.Axis(2))
};
Real afAAxis[3] =
{
Math<Real>::FAbs(afAxis[0]),
Math<Real>::FAbs(afAxis[1]),
Math<Real>::FAbs(afAxis[2])
};
Real fMaxProjectedExtent;
if ( afAAxis[0] < Math<Real>::EPSILON )
{
if ( afAAxis[1] < Math<Real>::EPSILON )
{
// face-face
rkConfig.m_kMap = m44;
fMaxProjectedExtent = afAAxis[2]*rkBox.Extent(2);
// faces have normals along axis[2]
if ( afAxis[2] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 0;
rkConfig.m_aiIndex[1] = 1;
rkConfig.m_aiIndex[2] = 3;
rkConfig.m_aiIndex[3] = 2;
rkConfig.m_aiIndex[4] = 6;
rkConfig.m_aiIndex[5] = 7;
rkConfig.m_aiIndex[6] = 5;
rkConfig.m_aiIndex[7] = 4;
}
else
{
rkConfig.m_aiIndex[0] = 6;
rkConfig.m_aiIndex[1] = 7;
rkConfig.m_aiIndex[2] = 5;
rkConfig.m_aiIndex[3] = 4;
rkConfig.m_aiIndex[4] = 0;
rkConfig.m_aiIndex[5] = 1;
rkConfig.m_aiIndex[6] = 3;
rkConfig.m_aiIndex[7] = 2;
}
}
else if ( afAAxis[2] < Math<Real>::EPSILON )
{
// face-face
rkConfig.m_kMap = m44;
fMaxProjectedExtent = afAAxis[1]*rkBox.Extent(1);
// faces have normals along axis[1]
if ( afAxis[1] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 4;
rkConfig.m_aiIndex[1] = 5;
rkConfig.m_aiIndex[2] = 1;
rkConfig.m_aiIndex[3] = 0;
rkConfig.m_aiIndex[4] = 2;
rkConfig.m_aiIndex[5] = 3;
rkConfig.m_aiIndex[6] = 7;
rkConfig.m_aiIndex[7] = 6;
}
else
{
rkConfig.m_aiIndex[0] = 2;
rkConfig.m_aiIndex[1] = 3;
rkConfig.m_aiIndex[2] = 7;
rkConfig.m_aiIndex[3] = 6;
rkConfig.m_aiIndex[4] = 4;
rkConfig.m_aiIndex[5] = 5;
rkConfig.m_aiIndex[6] = 1;
rkConfig.m_aiIndex[7] = 0;
}
}
else // only afAxis[0] is equal to 0
{
// seg-seg
rkConfig.m_kMap = m2_2;
fMaxProjectedExtent = afAAxis[1]*rkBox.Extent(1) +
afAAxis[2]*rkBox.Extent(2);
// axis 0 is perpendicular to rkAxis
if ( afAxis[1] > (Real)0.0 )
{
if ( afAxis[2] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 0;
rkConfig.m_aiIndex[1] = 1;
rkConfig.m_aiIndex[6] = 6;
rkConfig.m_aiIndex[7] = 7;
}
else
{
rkConfig.m_aiIndex[0] = 4;
rkConfig.m_aiIndex[1] = 5;
rkConfig.m_aiIndex[6] = 2;
rkConfig.m_aiIndex[7] = 3;
}
}
else // afAxis[1] < 0
{
if ( afAxis[2] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 2;
rkConfig.m_aiIndex[1] = 3;
rkConfig.m_aiIndex[6] = 4;
rkConfig.m_aiIndex[7] = 5;
}
else
{
rkConfig.m_aiIndex[0] = 6;
rkConfig.m_aiIndex[1] = 7;
rkConfig.m_aiIndex[6] = 0;
rkConfig.m_aiIndex[7] = 1;
}
}
}
}
else if ( afAAxis[1] < Math<Real>::EPSILON )
{
if ( afAAxis[2] < Math<Real>::EPSILON )
{
// face-face
rkConfig.m_kMap = m44;
fMaxProjectedExtent = afAAxis[0]*rkBox.Extent(0);
// faces have normals along axis[0]
if ( afAxis[0] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 0;
rkConfig.m_aiIndex[1] = 2;
rkConfig.m_aiIndex[2] = 6;
rkConfig.m_aiIndex[3] = 4;
rkConfig.m_aiIndex[4] = 5;
rkConfig.m_aiIndex[5] = 7;
rkConfig.m_aiIndex[6] = 3;
rkConfig.m_aiIndex[7] = 1;
}
else
{
rkConfig.m_aiIndex[4] = 0;
rkConfig.m_aiIndex[5] = 2;
rkConfig.m_aiIndex[6] = 6;
rkConfig.m_aiIndex[7] = 4;
rkConfig.m_aiIndex[0] = 5;
rkConfig.m_aiIndex[1] = 7;
rkConfig.m_aiIndex[2] = 3;
rkConfig.m_aiIndex[3] = 1;
}
}
else // only afAxis[1] is equal to 0
{
// seg-seg
rkConfig.m_kMap = m2_2;
fMaxProjectedExtent = afAAxis[0]*rkBox.Extent(0) +
afAAxis[2]*rkBox.Extent(2);
// axis 1 is perpendicular to rkAxis
if ( afAxis[0] > (Real)0.0 )
{
if ( afAxis[2] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 0;
rkConfig.m_aiIndex[1] = 2;
rkConfig.m_aiIndex[6] = 5;
rkConfig.m_aiIndex[7] = 7;
}
else
{
rkConfig.m_aiIndex[0] = 4;
rkConfig.m_aiIndex[1] = 6;
rkConfig.m_aiIndex[6] = 1;
rkConfig.m_aiIndex[7] = 3;
}
}
else // afAxis[0] < 0
{
if ( afAxis[2] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 1;
rkConfig.m_aiIndex[1] = 3;
rkConfig.m_aiIndex[6] = 4;
rkConfig.m_aiIndex[7] = 6;
}
else
{
rkConfig.m_aiIndex[0] = 5;
rkConfig.m_aiIndex[1] = 7;
rkConfig.m_aiIndex[6] = 0;
rkConfig.m_aiIndex[7] = 2;
}
}
}
}
else if ( afAAxis[2] < Math<Real>::EPSILON ) // only axis2 less than zero
{
// seg-seg
rkConfig.m_kMap = m2_2;
fMaxProjectedExtent = afAAxis[0]*rkBox.Extent(0) +
afAAxis[1]*rkBox.Extent(1);
// axis 2 is perpendicular to rkAxis
if ( afAxis[0] > (Real)0.0 )
{
if ( afAxis[1] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 0;
rkConfig.m_aiIndex[1] = 4;
rkConfig.m_aiIndex[6] = 3;
rkConfig.m_aiIndex[7] = 7;
}
else
{
rkConfig.m_aiIndex[0] = 2;
rkConfig.m_aiIndex[1] = 6;
rkConfig.m_aiIndex[6] = 1;
rkConfig.m_aiIndex[7] = 5;
}
}
else // afAxis[0] < 0
{
if ( afAxis[1] > (Real)0.0 )
{
rkConfig.m_aiIndex[0] = 1;
rkConfig.m_aiIndex[1] = 5;
rkConfig.m_aiIndex[6] = 2;
rkConfig.m_aiIndex[7] = 6;
}
else
{
rkConfig.m_aiIndex[0] = 3;
rkConfig.m_aiIndex[1] = 7;
rkConfig.m_aiIndex[6] = 0;
rkConfig.m_aiIndex[7] = 4;
}
}
}
else // no axis is equal to zero
{
// point-point (unique maximal and minimal vertex)
rkConfig.m_kMap = m1_1;
fMaxProjectedExtent = afAAxis[0]*rkBox.Extent(0) +
afAAxis[1]*rkBox.Extent(1) + afAAxis[2]*rkBox.Extent(2);
// only these two vertices matter, the rest are irrelevant
rkConfig.m_aiIndex[0] =
( afAxis[0] > (Real)0.0 ? 0 : 1 ) +
( afAxis[1] > (Real)0.0 ? 0 : 2 ) +
( afAxis[2] > (Real)0.0 ? 0 : 4 );
// by ordering the vertices this way, opposite corners add up to 7
rkConfig.m_aiIndex[7] = 7 - rkConfig.m_aiIndex[0];
}
// Find projections onto line
Real fOrigin = rkAxis.Dot(rkBox.Center());
rkConfig.m_fMin = fOrigin - fMaxProjectedExtent;
rkConfig.m_fMax = fOrigin + fMaxProjectedExtent;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Line3& rkLine, 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
// compute coordinates of line in box coordinate system
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kPnt(kDiff.Dot(rkBox.Axis(0)),kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2)));
Vector3 kDir(rkLine.Direction().Dot(rkBox.Axis(0)),
rkLine.Direction().Dot(rkBox.Axis(1)),
rkLine.Direction().Dot(rkBox.Axis(2)));
// Apply reflections so that direction vector has nonnegative components.
bool bReflect[3];
int i;
for (i = 0; i < 3; i++)
{
if ( kDir[i] < 0.0f )
{
kPnt[i] = -kPnt[i];
kDir[i] = -kDir[i];
bReflect[i] = true;
}
else
{
bReflect[i] = false;
}
}
Real fSqrDistance = 0.0f;
if ( kDir.x > 0.0f )
{
if ( kDir.y > 0.0f )
{
if ( kDir.z > 0.0f )
{
// (+,+,+)
CaseNoZeros(kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (+,+,0)
Case0(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
else
{
if ( kDir.z > 0.0f )
{
// (+,0,+)
Case0(0,2,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (+,0,0)
Case00(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
}
else
{
if ( kDir.y > 0.0f )
{
if ( kDir.z > 0.0f )
{
// (0,+,+)
Case0(1,2,0,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (0,+,0)
Case00(1,0,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
else
{
if ( kDir.z > 0.0f )
{
// (0,0,+)
Case00(2,0,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (0,0,0)
Case000(kPnt,rkBox,fSqrDistance);
if ( pfLParam )
*pfLParam = 0.0f;
}
}
}
if ( pfLParam )
{
// undo reflections
for (i = 0; i < 3; i++)
{
if ( bReflect[i] )
kPnt[i] = -kPnt[i];
}
*pfBParam0 = kPnt.x;
*pfBParam1 = kPnt.y;
*pfBParam2 = kPnt.z;
}
return fSqrDistance;
}
bool Wml::FindIntersection (const Box3<Real>& rkBox,
const Vector3<Real>& rkBoxVelocity, const Sphere3<Real>& rkSphere,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real>& rkP)
{
// Find intersections relative to the coordinate system of the box.
// The sphere is transformed to the box coordinates and the velocity of
// the sphere is relative to the box.
Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();
Vector3<Real> kVel = rkSphVelocity - rkBoxVelocity;
Real fAx = kCDiff.Dot(rkBox.Axis(0));
Real fAy = kCDiff.Dot(rkBox.Axis(1));
Real fAz = kCDiff.Dot(rkBox.Axis(2));
Real fVx = kVel.Dot(rkBox.Axis(0));
Real fVy = kVel.Dot(rkBox.Axis(1));
Real fVz = kVel.Dot(rkBox.Axis(2));
// flip coordinate frame into the first octant
int iSignX = 1;
if ( fAx < (Real)0.0 )
{
fAx = -fAx;
fVx = -fVx;
iSignX = -1;
}
int iSignY = 1;
if ( fAy < (Real)0.0 )
{
fAy = -fAy;
fVy = -fVy;
iSignY = -1;
}
int iSignZ = 1;
if ( fAz < (Real)0.0 )
{
fAz = -fAz;
fVz = -fVz;
iSignZ = -1;
}
// intersection coordinates
Real fIx, fIy, fIz;
int iRetVal;
if ( fAx <= rkBox.Extent(0) )
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere center inside box
rfTFirst = (Real)0.0;
riQuantity = 0;
return false;
}
else
{
// sphere above face on axis Z
iRetVal = FindFaceRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere above face on axis Y
iRetVal = FindFaceRegionIntersection(rkBox.Extent(0),
rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);
}
else
{
// sphere is above the edge formed by faces y and z
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(1),
rkBox.Extent(0),rkBox.Extent(2),fAy,fAx,fAz,fVy,fVx,fVz,
rfTFirst,rkSphere.Radius(),fIy,fIx,fIz,true);
}
}
}
else
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere above face on axis X
iRetVal = FindFaceRegionIntersection(rkBox.Extent(1),
rkBox.Extent(2),rkBox.Extent(0),fAy,fAz,fAx,fVy,fVz,fVx,
rfTFirst,rkSphere.Radius(),fIy,fIz,fIx,true);
}
else
{
// sphere is above the edge formed by faces x and z
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere is above the edge formed by faces x and y
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0),
rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);
}
else
{
// sphere is above the corner formed by faces x,y,z
iRetVal = FindVertexRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz);
}
}
}
if ( iRetVal != 1 || rfTFirst > fTMax )
{
riQuantity = 0;
return false;
}
// calculate actual intersection (move point back into world coordinates)
riQuantity = 1;
rkP = rkBox.Center() + (iSignX*fIx)*rkBox.Axis(0) +
(iSignY*fIy)*rkBox.Axis(1) + (iSignZ*fIz)*rkBox.Axis(2);
return true;
}
bool Wml::TestIntersection (const Box3<Real>& rkBox,
const Sphere3<Real>& rkSphere)
{
// Test for intersection in the coordinate system of the box by
// transforming the sphere into that coordinate system.
Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();
Real fAx = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(0)));
Real fAy = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(1)));
Real fAz = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(2)));
Real fDx = fAx - rkBox.Extent(0);
Real fDy = fAy - rkBox.Extent(1);
Real fDz = fAz - rkBox.Extent(2);
if ( fAx <= rkBox.Extent(0) )
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere center inside box
return true;
}
else
{
// potential sphere-face intersection with face z
return fDz <= rkSphere.Radius();
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-face intersection with face y
return fDy <= rkSphere.Radius();
}
else
{
// potential sphere-edge intersection with edge formed
// by faces y and z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDy*fDy + fDz*fDz <= fRSqr;
}
}
}
else
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-face intersection with face x
return fDx <= rkSphere.Radius();
}
else
{
// potential sphere-edge intersection with edge formed
// by faces x and z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDz*fDz <= fRSqr;
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-edge intersection with edge formed
// by faces x and y
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDy*fDy <= fRSqr;
}
else
{
// potential sphere-vertex intersection at corner formed
// by faces x,y,z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDy*fDy + fDz*fDz <= fRSqr;
}
}
}
}
