//----------------------------------------------------------------------------
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 );
}
Real DistPoint2Ellipse2<Real>::GetSquared ()
{
// Compute coordinates of point in ellipse coordinate system.
Vector2<Real> diff = *mPoint - mEllipse->Center;
Vector2<Real> y(diff.Dot(mEllipse->Axis[0]), diff.Dot(mEllipse->Axis[1]));
Vector2<Real> x;
Real sqrDistance = SqrDistance(mEllipse->Extent, y, x);
mClosestPoint0 = *mPoint;
mClosestPoint1 = mEllipse->Center +
x[0]*mEllipse->Axis[0] +
x[1]*mEllipse->Axis[1];
return sqrDistance;
}
//----------------------------------------------------------------------------
ContactType Collide (const SphereStruct& sph, const Vector3f& sphVelocity,
const TriangleStruct& tri, const Vector3f& triVelocity, float tMax,
float& contactTime, Vector3f& contactPoint)
{
// Test the sphere-triangle relationship at time zero.
float sqrDistance = SqrDistance(sph.C, tri, contactPoint);
if (sqrDistance < sph.RSqr)
{
contactTime = 0.0f;
return OVERLAPPING;
}
else if (sqrDistance == sph.RSqr)
{
contactTime = 0.0f;
return CONTACT;
}
// The sphere and triangle are initially separated. Compute the velocity
// of the sphere relative to triangle, so the triangle is then stationary.
Vector3f V = sphVelocity - triVelocity;
if (V == Vector3f::ZERO)
{
// The objects are stationary relative to each other.
return SEPARATED;
}
float t0Intr = FLT_MAX, t1Intr = -FLT_MAX;
float t0, t1;
if (IntersectLinePolyhedron(sph, V, tri, tMax, t0, t1))
{
if (t0 < t0Intr)
{
t0Intr = t0;
}
if (t1 > t1Intr)
{
t1Intr = t1;
}
}
int i;
for (i = 0; i < 3; ++i)
{
if (IntersectLineCylinder(sph, V, tri.M[i], tri.N, tri.EN[i],
tri.E[i], tri.H[i], tMax, t0, t1))
{
if (t0 < t0Intr)
{
t0Intr = t0;
}
if (t1 > t1Intr)
{
t1Intr = t1;
}
}
}
for (i = 0; i < 3; ++i)
{
if (IntersectLineSphere(sph, V, tri.P[i], tMax, t0, t1))
{
if (t0 < t0Intr)
{
t0Intr = t0;
}
if (t1 > t1Intr)
{
t1Intr = t1;
}
}
}
if (t0Intr <= t1Intr)
{
contactTime = t0Intr;
sqrDistance = SqrDistance(sph.C + contactTime*V, tri, contactPoint);
contactPoint += contactTime*triVelocity;
return CONTACT;
}
else
{
return SEPARATED;
}
}
FixedPoint Distance( fp2d& _a, fp2d& _b )
{
FixedPoint sqd = SqrDistance( _a, _b);
sqd.SetRawValue( fisqrt( sqd.GetDebugRawValue() << 8 ));
return sqd;
}