C++ GetSquared函数代码示例

Real Distance<Real,TVector>::GetSquared (Real tmin, Real tmax,
    const TVector& velocity0, const TVector& velocity1)
{
    // The assumption is that distance f(t) is a convex function.  If
    // f'(tmin) >= 0, then the minimum occurs at tmin.  If f'(tmax) <= 0,
    // then the minimum occurs at tmax.  Otherwise, f'(0) < 0 and
    // f'(tmax) > 0 and the minimum occurs at some t in (tmin,tmax).

    Real t0 = tmin;
    Real f0 = GetSquared(t0, velocity0, velocity1);
    if (f0 <= ZeroThreshold)
    {
        // The distance is effectively zero.  The objects are initially in
        // contact.
        mContactTime = t0;
        return (Real)0;
    }
    Real df0 = GetDerivativeSquared(t0, velocity0, velocity1);
    if (df0 >= (Real)0)
    {
        // The distance is increasing on [0,tmax].
        mContactTime = t0;
        return f0;
    }

    Real t1 = tmax;
    Real f1 = GetSquared(t1, velocity0, velocity1);
    if (f1 <= ZeroThreshold)
    {
        // The distance is effectively zero.
        mContactTime = t1;
        return (Real)0;
    }
    Real df1 = GetDerivativeSquared(t1, velocity0, velocity1);
    if (df1 <= (Real)0)
    {
        // The distance is decreasing on [0,tmax].
        mContactTime = t1;
        return f1;
    }

    // Start the process with Newton's method for computing a time when the
    // distance is zero.  During this process we will switch to a numerical
    // minimizer if we decide that the distance cannot be zero.
    int i;
    for (i = 0; i < MaximumIterations; ++i)
    {
        // Compute the next Newton's iterate.
        Real t = t0 - f0/df0;
        if (t >= tmax)
        {
            // The convexity of the graph guarantees that when this condition
            // happens, the distance is always positive.  Switch to a
            // numerical minimizer.
            break;
        }

        Real f = GetSquared(t, velocity0, velocity1);
        if (f <= ZeroThreshold)
        {
            // The distance is effectively zero.
            mContactTime = t;
            return (Real)0;
        }

        Real df = GetDerivativeSquared(t, velocity0, velocity1);
        if (df >= (Real)0)
        {
            // The convexity of the graph guarantees that when this condition
            // happens, the distance is always positive.  Switch to a
            // numerical minimizer.
            break;
        }

        t0 = t;
        f0 = f;
        df0 = df;
    }

    if (i == MaximumIterations)
    {
        // Failed to converge within desired number of iterations.  To
        // reach here, the derivative values were always negative, so report
        // the distance at the last time.
        mContactTime = t0;
        return f0;
    }

    // The distance is always positive.  Use bisection to find the root of
    // the derivative function.
    Real tm = t0;
    for (i = 0; i < MaximumIterations; ++i)
    {
        tm = ((Real)0.5)*(t0 + t1);
        Real dfm = GetDerivativeSquared(tm, velocity0, velocity1);
        Real product = dfm*df0;
        if (product < -ZeroThreshold)
        {
            t1 = tm;
            df1 = dfm;
//.........这里部分代码省略.........

Real DistVector3Segment3<Real>::Get ()
{
    Real fSqrDist = GetSquared();
    return Math<Real>::Sqrt(fSqrDist);
}

Real DistLine2Segment2<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistSegment3Rectangle3<Real>::Get ()
{
    Real fSqrDist = GetSquared();
    return Math<Real>::Sqrt(fSqrDist);
}

Real DistPoint3Segment3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistLine2Ray2<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistVector3Box3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistRay3Triangle3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistLine2Line2<Real>::Get ()
{
	Real sqrDist = GetSquared();
	return Math<Real>::Sqrt(sqrDist);
}

Real DistLine3Circle3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistVector3Tetrahedron3<Real>::Get ()
{
    Real fSqrDist = GetSquared();
    return Math<Real>::Sqrt(fSqrDist);
}

Real DistVector3Frustum3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistPoint3Ellipsoid3<Real>::Get ()
{
    return Math<Real>::Sqrt(GetSquared());
}

Real DistPoint3Tetrahedron3<Real>::Get ()
{
    return Math<Real>::Sqrt( GetSquared() );
}

Real DistLine3Triangle3<Real>::Get ()
{
    Real fSqrDist = GetSquared();
    return Math<Real>::Sqrt(fSqrDist);
}