mgcdistlin3cir3.cpp

来自「《3D游戏引擎设计》的源码」· C++ 代码 · 共 85 行

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcDistLin3Cir3.h"
#include "MgcDistVec3Cir3.h"
#include "MgcPolynomial.h"

//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcLine3& rkLine, const MgcCircle3& rkCircle,
    MgcVector3* pkLineClosest, MgcVector3* pkCircleClosest)
{
    MgcVector3 kDiff = rkLine.Origin() - rkCircle.Center();
    MgcReal fDSqrLen = kDiff.SquaredLength();
    MgcReal fMdM = rkLine.Direction().SquaredLength();
    MgcReal fDdM = kDiff.Dot(rkLine.Direction());
    MgcReal fNdM = rkCircle.N().Dot(rkLine.Direction());
    MgcReal fDdN = kDiff.Dot(rkCircle.N());

    MgcReal fA0 = fDdM;
    MgcReal fA1 = fMdM;
    MgcReal fB0 = fDdM - fNdM*fDdN;
    MgcReal fB1 = fMdM - fNdM*fNdM;
    MgcReal fC0 = fDSqrLen - fDdN*fDdN;
    MgcReal fC1 = fB0;
    MgcReal fC2 = fB1;
    MgcReal fRSqr = rkCircle.Radius()*rkCircle.Radius();

    MgcReal fA0Sqr = fA0*fA0;
    MgcReal fA1Sqr = fA1*fA1;
    MgcReal fTwoA0A1 = 2.0f*fA0*fA1;
    MgcReal fB0Sqr = fB0*fB0;
    MgcReal fB1Sqr = fB1*fB1;
    MgcReal fTwoB0B1 = 2.0f*fB0*fB1;
    MgcReal fTwoC1 = 2.0f*fC1;

    // The minimum point B+t*M occurs when t is a root of the quartic
    // equation whose coefficients are defined below.
    MgcPolynomial kPoly(5);
    kPoly[0] = fA0Sqr*fC0 - fB0Sqr*fRSqr;
    kPoly[1] = fTwoA0A1*fC0 + fA0Sqr*fTwoC1 - fTwoB0B1*fRSqr;
    kPoly[2] = fA1Sqr*fC0 + fTwoA0A1*fTwoC1 + fA0Sqr*fC2 - fB1Sqr*fRSqr;
    kPoly[3] = fA1Sqr*fTwoC1 + fTwoA0A1*fC2;
    kPoly[4] = fA1Sqr*fC2;

    int iNumRoots;
    MgcReal afRoot[4];
    kPoly.GetAllRoots(iNumRoots,afRoot);

    MgcReal fMinSqrDist = MgcMath::INFINITY;
    for (int i = 0; i < iNumRoots; i++)
    {
        // compute distance from P(t) to circle
        MgcVector3 kP = rkLine.Origin() + afRoot[i]*rkLine.Direction();
        MgcVector3 kCircleClosest;
        MgcReal fSqrDist = MgcSqrDistance(kP,rkCircle,&kCircleClosest);
        if ( fSqrDist < fMinSqrDist )
        {
            fMinSqrDist = fSqrDist;
            if ( pkLineClosest )
                *pkLineClosest = kP;
            if ( pkCircleClosest )
                *pkCircleClosest = kCircleClosest;
        }
    }

    return fMinSqrDist;
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcLine3& rkLine, const MgcCircle3& rkCircle,
    MgcVector3* pkLineClosest, MgcVector3* pkCircleClosest)
{
    return MgcMath::Sqrt(MgcSqrDistance(rkLine,rkCircle,pkLineClosest,
        pkCircleClosest));
}
//----------------------------------------------------------------------------