Пример #1
0
//----------------------------------------------------------------------------
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;
}
Пример #3
0
//----------------------------------------------------------------------------
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;
    }
}
Пример #4
0
FixedPoint Distance(  fp2d& _a,  fp2d& _b )
{
	FixedPoint sqd = SqrDistance( _a, _b);
	sqd.SetRawValue( fisqrt( sqd.GetDebugRawValue() << 8 ));
	return sqd;
}