size_t collide(CircleShape &circle, BoxShape &box, float dt, Contact *pxContacts, size_t numMaxContacts)
{
dt, pxContacts, numMaxContacts;
const Point2f &p1 = circle.body.getPosition();
const Point2f &p2 = box.body.getPosition();
// work in the box's coordinate system
const Point2f kDiff = p1 - p2;
// compute squared distance and closest point on box
float fSqrDistance = 0.0f, fDelta;
Point2f kClosest(kDiff * box.GetDir(0), kDiff * box.GetDir(1));
const Point2f &extents = box.GetExtents();
if (kClosest.x < -extents.x)
{
fDelta = kClosest.x + extents.x;
fSqrDistance += fDelta*fDelta;
kClosest.x = -extents.x;
}
else if (kClosest.x > extents.x)
{
fDelta = kClosest.x - extents.x;
fSqrDistance += fDelta*fDelta;
kClosest.x = extents.x;
}
if (kClosest.y < -extents.y)
{
fDelta = kClosest.y + extents.y;
fSqrDistance += fDelta*fDelta;
kClosest.y = -extents.y;
}
else if (kClosest.y > extents.y)
{
fDelta = kClosest.y - extents.y;
fSqrDistance += fDelta*fDelta;
kClosest.y = extents.y;
}
if (fSqrDistance > circle.GetRadius() * circle.GetRadius())
{
return 0;
}
Point2f d = p2 + kClosest - p1;
d.Normalize();
pxContacts[0] = Contact(p1 + d * circle.GetRadius(), p2 + kClosest, &circle.body, &box.body);
return 1;
}
Real DistVector3Ellipsoid3<Real>::GetSquared ()
{
// compute coordinates of point in ellipsoid coordinate system
Vector3<Real> kDiff = m_rkVector - m_rkEllipsoid.Center;
Vector3<Real> kEPoint(kDiff.Dot(m_rkEllipsoid.Axis[0]),
kDiff.Dot(m_rkEllipsoid.Axis[1]),kDiff.Dot(m_rkEllipsoid.Axis[2]));
const Real* afExtent = m_rkEllipsoid.Extent;
Real fA2 = afExtent[0]*afExtent[0];
Real fB2 = afExtent[1]*afExtent[1];
Real fC2 = afExtent[2]*afExtent[2];
Real fU2 = kEPoint.X()*kEPoint.X();
Real fV2 = kEPoint.Y()*kEPoint.Y();
Real fW2 = kEPoint.Z()*kEPoint.Z();
Real fA2U2 = fA2*fU2, fB2V2 = fB2*fV2, fC2W2 = fC2*fW2;
// initial guess
Real fURatio = kEPoint.X()/afExtent[0];
Real fVRatio = kEPoint.Y()/afExtent[1];
Real fWRatio = kEPoint.Z()/afExtent[2];
Real fT;
if (fURatio*fURatio+fVRatio*fVRatio+fWRatio*fWRatio < (Real)1.0)
{
fT = (Real)0.0;
}
else
{
Real fMax = afExtent[0];
if (afExtent[1] > fMax)
{
fMax = afExtent[1];
}
if (afExtent[2] > fMax)
{
fMax = afExtent[2];
}
fT = fMax*kEPoint.Length();
}
// Newton's method
const int iMaxIteration = 64;
Real fP, fQ, fR;
for (int i = 0; i < iMaxIteration; i++)
{
fP = fT+fA2;
fQ = fT+fB2;
fR = fT+fC2;
Real fP2 = fP*fP;
Real fQ2 = fQ*fQ;
Real fR2 = fR*fR;
Real fS = fP2*fQ2*fR2-fA2U2*fQ2*fR2-fB2V2*fP2*fR2-fC2W2*fP2*fQ2;
if (Math<Real>::FAbs(fS) < Math<Real>::ZERO_TOLERANCE)
{
break;
}
Real fPQ = fP*fQ, fPR = fP*fR, fQR = fQ*fR, fPQR = fP*fQ*fR;
Real fDS = ((Real)2.0)*(fPQR*(fQR+fPR+fPQ)-fA2U2*fQR*(fQ+fR)-
fB2V2*fPR*(fP+fR)-fC2W2*fPQ*(fP+fQ));
fT -= fS/fDS;
}
Vector3<Real> kClosest(fA2*kEPoint.X()/fP,fB2*kEPoint.Y()/fQ,
fC2*kEPoint.Z()/fR);
kDiff = kClosest - kEPoint;
Real fSqrDistance = kDiff.SquaredLength();
m_kClosestPoint0 = m_rkVector;
m_kClosestPoint1 = m_rkEllipsoid.Center +
kClosest.X()*m_rkEllipsoid.Axis[0] +
kClosest.Y()*m_rkEllipsoid.Axis[1] +
kClosest.Z()*m_rkEllipsoid.Axis[2];
return fSqrDistance;
}
