math/Segment.cpp

Go to the documentation of this file.

#include <assert.h>
#include <math.h>
#include "Segment.h"

//=============================================================================
// Constructor
//
// Description: Constructor
//=============================================================================
Segment::Segment( const Vector4& p0, const Vector4& p1 )
:
    p0( p0 ),
    p1( p1 )
{
    //nothing
}

//=============================================================================
// Distance
//
// Description: distance between point and this segment
//=============================================================================
double Segment::Distance( const Vector4& p ) const
{
    return sqrt( DistanceSquared( p ) );
}

//=============================================================================
// Distance
//
// Description: distance between segment and this segment
//=============================================================================
double Segment::Distance( const Segment& s ) const
{
    return sqrt( DistanceSquared( s ) );
}

//=============================================================================
// DistanceSquared
//
// Description: distance between segment and this segment
//=============================================================================
double Segment::DistanceSquared( const Segment& s ) const
{
    Vector4 kDiff = this->p0 - s.p0;
    Vector4 d0 = p1 - p0;
    Vector4 d1 = s.p1 - s.p0;
    double fA00 = d0.MagSquared();
    double fA01 = -d0.Dot( d1 );
    double fA11 = d1.MagSquared();
    double fB0 = kDiff.Dot( d0 );
    double fC = kDiff.MagSquared();
    double fDet = fabs( fA00 * fA11 - fA01 * fA01 );
    double fB1, fS, fT, fSqrDist, fTmp;

    //HACK: what is this value really?
    double gs_fTolerance = 0.00000001;
    if ( fDet >= gs_fTolerance )
    {
        // line segments are not parallel
        fB1 = -kDiff.Dot( d1 );
        fS = fA01*fB1-fA11*fB0;
        fT = fA01*fB0-fA00*fB1;
        
        if ( fS >= 0.0f )
        {
            if ( fS <= fDet )
            {
                if ( fT >= 0.0f )
                {
                    if ( fT <= fDet )  // region 0 (interior)
                    {
                        // minimum at two interior points of 3D lines
                        double fInvDet = 1.0f/fDet;
                        fS *= fInvDet;
                        fT *= fInvDet;
                        fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
                            fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
                    }
                    else  // region 3 (side)
                    {
                        fT = 1.0f;
                        fTmp = fA01+fB0;
                        if ( fTmp >= 0.0f )
                        {
                            fS = 0.0f;
                            fSqrDist = fA11+2.0f*fB1+fC;
                        }
                        else if ( -fTmp >= fA00 )
                        {
                            fS = 1.0f;
                            fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
                        }
                        else
                        {
                            fS = -fTmp/fA00;
                            fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
                        }
                    }
                }
                else  // region 7 (side)
                {
                    fT = 0.0f;
                    if ( fB0 >= 0.0f )
                    {
                        fS = 0.0f;
                        fSqrDist = fC;
                    }
                    else if ( -fB0 >= fA00 )
                    {
                        fS = 1.0f;
                        fSqrDist = fA00+2.0f*fB0+fC;
                    }
                    else
                    {
                        fS = -fB0/fA00;
                        fSqrDist = fB0*fS+fC;
                    }
                }
            }
            else
            {
                if ( fT >= 0.0 )
                {
                    if ( fT <= fDet )  // region 1 (side)
                    {
                        fS = 1.0f;
                        fTmp = fA01+fB1;
                        if ( fTmp >= 0.0f )
                        {
                            fT = 0.0f;
                            fSqrDist = fA00+2.0f*fB0+fC;
                        }
                        else if ( -fTmp >= fA11 )
                        {
                            fT = 1.0f;
                            fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
                        }
                        else
                        {
                            fT = -fTmp/fA11;
                            fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
                        }
                    }
                    else  // region 2 (corner)
                    {
                        fTmp = fA01+fB0;
                        if ( -fTmp <= fA00 )
                        {
                            fT = 1.0f;
                            if ( fTmp >= 0.0f )
                            {
                                fS = 0.0f;
                                fSqrDist = fA11+2.0f*fB1+fC;
                            }
                            else
                            {
                                 fS = -fTmp/fA00;
                                 fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
                            }
                        }
                        else
                        {
                            fS = 1.0f;
                            fTmp = fA01+fB1;
                            if ( fTmp >= 0.0f )
                            {
                                fT = 0.0f;
                                fSqrDist = fA00+2.0f*fB0+fC;
                            }
                            else if ( -fTmp >= fA11 )
                            {
                                fT = 1.0f;
                                fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
                            }
                            else
                            {
                                fT = -fTmp/fA11;
                                fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
                            }
                        }
                    }
                }
                else  // region 8 (corner)
                {
                    if ( -fB0 < fA00 )
                    {
                        fT = 0.0f;
                        if ( fB0 >= 0.0f )
                        {
                            fS = 0.0f;
                            fSqrDist = fC;
                        }
                        else
                        {
                            fS = -fB0/fA00;
                            fSqrDist = fB0*fS+fC;
                        }
                    }
                    else
                    {
                        fS = 1.0f;
                        fTmp = fA01+fB1;
                        if ( fTmp >= 0.0f )
                        {
                            fT = 0.0f;
                            fSqrDist = fA00+2.0f*fB0+fC;
                        }
                        else if ( -fTmp >= fA11 )
                        {
                            fT = 1.0f;
                            fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
                        }
                        else
                        {
                            fT = -fTmp/fA11;
                            fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
                        }
                    }
                }
            }
        }
        else 
        {
            if ( fT >= 0.0f )
            {
                if ( fT <= fDet )  // region 5 (side)
                {
                    fS = 0.0f;
                    if ( fB1 >= 0.0f )
                    {
                        fT = 0.0f;
                        fSqrDist = fC;
                    }
                    else if ( -fB1 >= fA11 )
                    {
                        fT = 1.0f;
                        fSqrDist = fA11+2.0f*fB1+fC;
                    }
                    else
                    {
                        fT = -fB1/fA11;
                        fSqrDist = fB1*fT+fC;
                    }
                }
                else  // region 4 (corner)
                {
                    fTmp = fA01+fB0;
                    if ( fTmp < 0.0f )
                    {
                        fT = 1.0f;
                        if ( -fTmp >= fA00 )
                        {
                            fS = 1.0f;
                            fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
                        }
                        else
                        {
                            fS = -fTmp/fA00;
                            fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
                        }
                    }
                    else
                    {
                        fS = 0.0f;
                        if ( fB1 >= 0.0f )
                        {
                            fT = 0.0f;
                            fSqrDist = fC;
                        }
                        else if ( -fB1 >= fA11 )
                        {
                            fT = 1.0f;
                            fSqrDist = fA11+2.0f*fB1+fC;
                        }
                        else
                        {
                            fT = -fB1/fA11;
                            fSqrDist = fB1*fT+fC;
                        }
                    }
                }
            }
            else   // region 6 (corner)
            {
                if ( fB0 < 0.0f )
                {
                    fT = 0.0f;
                    if ( -fB0 >= fA00 )
                    {
                        fS = 1.0f;
                        fSqrDist = fA00+2.0f*fB0+fC;
                    }
                    else
                    {
                        fS = -fB0/fA00;
                        fSqrDist = fB0*fS+fC;
                    }
                }
                else
                {
                    fS = 0.0f;
                    if ( fB1 >= 0.0f )
                    {
                        fT = 0.0f;
                        fSqrDist = fC;
                    }
                    else if ( -fB1 >= fA11 )
                    {
                        fT = 1.0f;
                        fSqrDist = fA11+2.0f*fB1+fC;
                    }
                    else
                    {
                        fT = -fB1/fA11;
                        fSqrDist = fB1*fT+fC;
                    }
                }
            }
        }
    }
    else
    {
        // line segments are parallel
        if ( fA01 > 0.0f )
        {
            // direction vectors form an obtuse angle
            if ( fB0 >= 0.0f )
            {
                fS = 0.0f;
                fT = 0.0f;
                fSqrDist = fC;
            }
            else if ( -fB0 <= fA00 )
            {
                fS = -fB0/fA00;
                fT = 0.0f;
                fSqrDist = fB0*fS+fC;
            }
            else
            {
                fB1 = -kDiff.Dot( d1 );
                fS = 1.0f;
                fTmp = fA00+fB0;
                if ( -fTmp >= fA01 )
                {
                    fT = 1.0f;
                    fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
                }
                else
                {
                    fT = -fTmp/fA01;
                    fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
                }
            }
        }
        else
        {
            // direction vectors form an acute angle
            if ( -fB0 >= fA00 )
            {
                fS = 1.0f;
                fT = 0.0f;
                fSqrDist = fA00+2.0f*fB0+fC;
            }
            else if ( fB0 <= 0.0f )
            {
                fS = -fB0/fA00;
                fT = 0.0f;
                fSqrDist = fB0*fS+fC;
            }
            else
            {
                fB1 = -kDiff.Dot( d1 );
                fS = 0.0f;
                if ( fB0 >= -fA01 )
                {
                    fT = 1.0f;
                    fSqrDist = fA11+2.0f*fB1+fC;
                }
                else
                {
                    fT = -fB0/fA01;
                    fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
                }
            }
        }
    }

    return fabs( fSqrDist );
}

//=============================================================================
// Distance
//
// Description: distance between point and this segment
//=============================================================================
double Segment::DistanceSquared( const Vector4& p ) const
{
    //debug checking code
#ifndef NDEBUG
    Segment s( p, p );
    double checkSegmentDistance = DistanceSquared( s );
#endif

    Vector4 segmentDirection = p1 - p0;
    Vector4 pointDirection = p - p0;
    double dot = segmentDirection.Dot( pointDirection );
    if( dot <= 0.0 )
    {
        //
        // Point is behind the origin
        //
        double returnMe = pointDirection.MagSquared();
        assert( returnMe == checkSegmentDistance );
        return returnMe;
    }

    double segmentLengthSqr = segmentDirection.MagSquared();
    if( dot >= segmentLengthSqr )
    {
        //
        // Point is behind the other target
        //
        pointDirection -= segmentDirection;
        double returnMe = pointDirection.MagSquared();
        assert( ( checkSegmentDistance - returnMe ) < 0.00000001 );
        return returnMe;
    }
    else
    {
        //
        // Point is closest to somewhere in the middle of the line
        //
        dot /= segmentLengthSqr;
        pointDirection -= segmentDirection * dot;
        double returnMe = pointDirection.MagSquared();
        assert( ( checkSegmentDistance - returnMe ) < 0.00000001 );
        return returnMe;
    }
}

---