wmldistlin3lin3.cpp

3D Game Engine Design Source Code非常棒

    if ( fDet >= Math<Real>::EPSILON )
    {
        // ray and segment are not parallel
        fB1 = -kDiff.Dot(rkSeg.Direction());
        fS = fA01*fB1-fA11*fB0;
        fT = fA01*fB0-fA00*fB1;

        if ( fS >= (Real)0.0 )
        {
            if ( fT >= (Real)0.0 )
            {
                if ( fT <= fDet )  // region 0
                {
                    // minimum at interior points of ray and segment
                    Real fInvDet = ((Real)1.0)/fDet;
                    fS *= fInvDet;
                    fT *= fInvDet;
                    fSqrDist = fS*(fA00*fS+fA01*fT+((Real)2.0)*fB0) +
                        fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
                }
                else  // region 1
                {
                    fT = (Real)1.0;
                    if ( fB0 >= -fA01 )
                    {
                        fS = (Real)0.0;
                        fSqrDist = fA11+((Real)2.0)*fB1+fC;
                    }
                    else
                    {
                        fTmp = fA01 + fB0;
                        fS = -fTmp/fA00;
                        fSqrDist = fTmp*fS+fA11+((Real)2.0)*fB1+fC;
                    }
                }
            }
            else  // region 5
            {
                fT = (Real)0.0;
                if ( fB0 >= (Real)0.0 )
                {
                    fS = (Real)0.0;
                    fSqrDist = fC;
                }
                else
                {
                    fS = -fB0/fA00;
                    fSqrDist = fB0*fS+fC;
                }
            }
        }
        else
        {
            if ( fT <= (Real)0.0 )  // region 4
            {
                if ( fB0 < (Real)0.0 )
                {
                    fS = -fB0/fA00;
                    fT = (Real)0.0;
                    fSqrDist = fB0*fS+fC;
                }
                else
                {
                    fS = (Real)0.0;
                    if ( fB1 >= (Real)0.0 )
                    {
                        fT = (Real)0.0;
                        fSqrDist = fC;
                    }
                    else if ( -fB1 >= fA11 )
                    {
                        fT = (Real)1.0;
                        fSqrDist = fA11+((Real)2.0)*fB1+fC;
                    }
                    else
                    {
                        fT = -fB1/fA11;
                        fSqrDist = fB1*fT+fC;
                    }
                }
            }
            else if ( fT <= fDet )  // region 3
            {
                fS = (Real)0.0;
                if ( fB1 >= (Real)0.0 )
                {
                    fT = (Real)0.0;
                    fSqrDist = fC;
                }
                else if ( -fB1 >= fA11 )
                {
                    fT = (Real)1.0;
                    fSqrDist = fA11+((Real)2.0)*fB1+fC;
                }
                else
                {
                    fT = -fB1/fA11;
                    fSqrDist = fB1*fT+fC;
                }
            }
            else  // region 2
            {
                fTmp = fA01+fB0;
                if ( fTmp < (Real)0.0 )
                {
                    fS = -fTmp/fA00;
                    fT = (Real)1.0;
                    fSqrDist = fTmp*fS+fA11+((Real)2.0)*fB1+fC;
                }
                else
                {
                    fS = (Real)0.0;
                    if ( fB1 >= (Real)0.0 )
                    {
                        fT = (Real)0.0;
                        fSqrDist = fC;
                    }
                    else if ( -fB1 >= fA11 )
                    {
                        fT = (Real)1.0;
                        fSqrDist = fA11+((Real)2.0)*fB1+fC;
                    }
                    else
                    {
                        fT = -fB1/fA11;
                        fSqrDist = fB1*fT+fC;
                    }
                }
            }
        }
    }
    else
    {
        // ray and segment are parallel
        if ( fA01 > (Real)0.0 )
        {
            // opposite direction vectors
            fT = (Real)0.0;
            if ( fB0 >= (Real)0.0 )
            {
                fS = (Real)0.0;
                fSqrDist = fC;
            }
            else
            {
                fS = -fB0/fA00;
                fSqrDist = fB0*fS+fC;
            }
        }
        else
        {
            // same direction vectors
            fB1 = -kDiff.Dot(rkSeg.Direction());
            fT = (Real)1.0;
            fTmp = fA01+fB0;
            if ( fTmp >= (Real)0.0 )
            {
                fS = (Real)0.0;
                fSqrDist = fA11+((Real)2.0)*fB1+fC;
            }
            else
            {
                fS = -fTmp/fA00;
                fSqrDist = fTmp*fS+fA11+((Real)2.0)*fB1+fC;
            }
        }
    }

    if ( pfRayP )
        *pfRayP = fS;

    if ( pfSegP )
        *pfSegP = fT;

    return Math<Real>::FAbs(fSqrDist);
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Segment3<Real>& rkSeg0, const Segment3<Real>& rkSeg1,
    Real* pfSegP0, Real* pfSegP1)
{
    Vector3<Real> kDiff = rkSeg0.Origin() - rkSeg1.Origin();
    Real fA00 = rkSeg0.Direction().SquaredLength();
    Real fA01 = -rkSeg0.Direction().Dot(rkSeg1.Direction());
    Real fA11 = rkSeg1.Direction().SquaredLength();
    Real fB0 = kDiff.Dot(rkSeg0.Direction());
    Real fC = kDiff.SquaredLength();
    Real fDet = Math<Real>::FAbs(fA00*fA11-fA01*fA01);
    Real fB1, fS, fT, fSqrDist, fTmp;

    if ( fDet >= Math<Real>::EPSILON )
    {
        // line segments are not parallel
        fB1 = -kDiff.Dot(rkSeg1.Direction());
        fS = fA01*fB1-fA11*fB0;
        fT = fA01*fB0-fA00*fB1;
        
        if ( fS >= (Real)0.0 )
        {
            if ( fS <= fDet )
            {
                if ( fT >= (Real)0.0 )
                {
                    if ( fT <= fDet )  // region 0 (interior)
                    {
                        // minimum at two interior points of 3D lines
                        Real fInvDet = ((Real)1.0)/fDet;
                        fS *= fInvDet;
                        fT *= fInvDet;
                        fSqrDist = fS*(fA00*fS+fA01*fT+((Real)2.0)*fB0) +
                            fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
                    }
                    else  // region 3 (side)
                    {
                        fT = (Real)1.0;
                        fTmp = fA01+fB0;
                        if ( fTmp >= (Real)0.0 )
                        {
                            fS = (Real)0.0;
                            fSqrDist = fA11+((Real)2.0)*fB1+fC;
                        }
                        else if ( -fTmp >= fA00 )
                        {
                            fS = (Real)1.0;
                            fSqrDist = fA00+fA11+fC+((Real)2.0)*(fB1+fTmp);
                        }
                        else
                        {
                            fS = -fTmp/fA00;
                            fSqrDist = fTmp*fS+fA11+((Real)2.0)*fB1+fC;
                        }
                    }
                }
                else  // region 7 (side)
                {
                    fT = (Real)0.0;
                    if ( fB0 >= (Real)0.0 )
                    {
                        fS = (Real)0.0;
                        fSqrDist = fC;
                    }
                    else if ( -fB0 >= fA00 )
                    {
                        fS = (Real)1.0;
                        fSqrDist = fA00+((Real)2.0)*fB0+fC;
                    }
                    else
                    {
                        fS = -fB0/fA00;
                        fSqrDist = fB0*fS+fC;
                    }
                }
            }
            else
            {
                if ( fT >= (Real)0.0 )
                {
                    if ( fT <= fDet )  // region 1 (side)
                    {
                        fS = (Real)1.0;
                        fTmp = fA01+fB1;
                        if ( fTmp >= (Real)0.0 )
                        {
                            fT = (Real)0.0;
                            fSqrDist = fA00+((Real)2.0)*fB0+fC;
                        }
                        else if ( -fTmp >= fA11 )
                        {
                            fT = (Real)1.0;
                            fSqrDist = fA00+fA11+fC+((Real)2.0)*(fB0+fTmp);
                        }
                        else
                        {
                            fT = -fTmp/fA11;
                            fSqrDist = fTmp*fT+fA00+((Real)2.0)*fB0+fC;
                        }
                    }
                    else  // region 2 (corner)
                    {
                        fTmp = fA01+fB0;
                        if ( -fTmp <= fA00 )
                        {
                            fT = (Real)1.0;
                            if ( fTmp >= (Real)0.0 )
                            {
                                fS = (Real)0.0;
                                fSqrDist = fA11+((Real)2.0)*fB1+fC;
                            }
                            else
                            {
                                 fS = -fTmp/fA00;
                                 fSqrDist = fTmp*fS+fA11+((Real)2.0)*fB1+fC;
                            }
                        }
                        else
                        {
                            fS = (Real)1.0;
                            fTmp = fA01+fB1;
                            if ( fTmp >= (Real)0.0 )
                            {
                                fT = (Real)0.0;
                                fSqrDist = fA00+((Real)2.0)*fB0+fC;
                            }
                            else if ( -fTmp >= fA11 )
                            {
                                fT = (Real)1.0;
                                fSqrDist = fA00+fA11+fC+
                                    ((Real)2.0)*(fB0+fTmp);
                            }
                            else
                            {
                                fT = -fTmp/fA11;
                                fSqrDist = fTmp*fT+fA00+((Real)2.0)*fB0+fC;
                            }
                        }
                    }
                }