mgcminsphere.cpp

3D Game Engine Design Source Code非常棒

// 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 "MgcMinSphere.h"
#include "MgcRTLib.h"

// indices of points that support current minimum volume sphere
class Support
{
public:
    int m_iQuantity;
    int m_aiIndex[4];

    bool Contains (int iIndex)
    {
        for (int i = 0; i < m_iQuantity; i++)
        {
            if ( iIndex == m_aiIndex[i] )
                return true;
        }
        return false;
    }
};

// error checking
static const MgcReal gs_fEpsilon = 1e-12;

// All internal minimal sphere calculations store the squared radius in the
// radius member of MgcSphere.  Only at the end is a sqrt computed.

//----------------------------------------------------------------------------
static bool PointInsideSphere (const MgcVector3& rkP, const MgcSphere& rkS)
{
    MgcVector3 kDiff = rkP - rkS.Center();
    MgcReal fTest = kDiff.SquaredLength() - rkS.Radius();
    return fTest <= gs_fEpsilon;  // theoretical is:  test <= 0
}
//----------------------------------------------------------------------------
static MgcSphere ExactSphere1 (const MgcVector3& rkP)
{
    MgcSphere kMinimal;
    kMinimal.Center() = rkP;
    kMinimal.Radius() = 0.0;
    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere ExactSphere2 (const MgcVector3& rkP0, const MgcVector3& rkP1)
{
    MgcSphere kMinimal;
    kMinimal.Center() = 0.5*(rkP0+rkP1);
    MgcVector3 kDiff = rkP1 - rkP0;
    kMinimal.Radius() = 0.25*kDiff.SquaredLength();
    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere ExactSphere3 (const MgcVector3& rkP0,
    const MgcVector3& rkP1, const MgcVector3& rkP2)
{
    // Compute the circle (in 3D) containing p0, p1, and p2.  The Center() in
    // barycentric coordinates is K = u0*p0+u1*p1+u2*p2 where u0+u1+u2=1.
    // The Center() is equidistant from the three points, so |K-p0| = |K-p1| =
    // |K-p2| = R where R is the radius of the circle.
    //
    // From these conditions,
    //   K-p0 = u0*A + u1*B - A
    //   K-p1 = u0*A + u1*B - B
    //   K-p2 = u0*A + u1*B
    // where A = p0-p2 and B = p1-p2, which leads to
    //   r^2 = |u0*A+u1*B|^2 - 2*Dot(A,u0*A+u1*B) + |A|^2
    //   r^2 = |u0*A+u1*B|^2 - 2*Dot(B,u0*A+u1*B) + |B|^2
    //   r^2 = |u0*A+u1*B|^2
    // Subtracting the last equation from the first two and writing
    // the equations as a linear system,
    //
    // +-                 -++   -+       +-        -+
    // | Dot(A,A) Dot(A,B) || u0 | = 0.5 | Dot(A,A) |
    // | Dot(B,A) Dot(B,B) || u1 |       | Dot(B,B) |
    // +-                 -++   -+       +-        -+
    //
    // The following code solves this system for u0 and u1, then
    // evaluates the third equation in r^2 to obtain r.

    MgcVector3 kA = rkP0 - rkP2;
    MgcVector3 kB = rkP1 - rkP2;
    MgcReal fAdA = kA.Dot(kA);
    MgcReal fAdB = kA.Dot(kB);
    MgcReal fBdB = kB.Dot(kB);
    MgcReal fDet = fAdA*fBdB-fAdB*fAdB;
    assert( MgcMath::Abs(fDet) > gs_fEpsilon );

    MgcSphere kMinimal;

    MgcReal fHalfInvDet = 0.5/fDet;
    MgcReal fU0 = fHalfInvDet*fBdB*(fAdA-fAdB);
    MgcReal fU1 = fHalfInvDet*fAdA*(fBdB-fAdB);
    MgcReal fU2 = 1.0-fU0-fU1;
    kMinimal.Center() = fU0*rkP0 + fU1*rkP1 + fU2*rkP2;
    MgcVector3 kTmp = fU0*kA + fU1*kB;
    kMinimal.Radius() = kTmp.SquaredLength();

    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere ExactSphere4 (const MgcVector3& rkP0,
    const MgcVector3& rkP1, const MgcVector3& rkP2, const MgcVector3& rkP3)
{
    // Compute the sphere containing p0, p1, p2, and p3.  The Center() in
    // barycentric coordinates is K = u0*p0+u1*p1+u2*p2+u3*p3 where
    // u0+u1+u2+u3=1.  The Center() is equidistant from the three points, so
    // |K-p0| = |K-p1| = |K-p2| = |K-p3| = R where R is the radius of the
    // sphere.
    //
    // From these conditions,
    //   K-p0 = u0*A + u1*B + u2*C - A
    //   K-p1 = u0*A + u1*B + u2*C - B
    //   K-p2 = u0*A + u1*B + u2*C - C
    //   K-p3 = u0*A + u1*B + u2*C
    // where A = p0-p3, B = p1-p3, and C = p2-p3 which leads to
    //   r^2 = |u0*A+u1*B+u2*C|^2 - 2*Dot(A,u0*A+u1*B+u2*C) + |A|^2
    //   r^2 = |u0*A+u1*B+u2*C|^2 - 2*Dot(B,u0*A+u1*B+u2*C) + |B|^2
    //   r^2 = |u0*A+u1*B+u2*C|^2 - 2*Dot(C,u0*A+u1*B+u2*C) + |C|^2
    //   r^2 = |u0*A+u1*B+u2*C|^2
    // Subtracting the last equation from the first three and writing
    // the equations as a linear system,
    //
    // +-                          -++   -+       +-        -+
    // | Dot(A,A) Dot(A,B) Dot(A,C) || u0 | = 0.5 | Dot(A,A) |
    // | Dot(B,A) Dot(B,B) Dot(B,C) || u1 |       | Dot(B,B) |
    // | Dot(C,A) Dot(C,B) Dot(C,C) || u2 |       | Dot(C,C) |
    // +-                          -++   -+       +-        -+
    //
    // The following code solves this system for u0, u1, and u2, then
    // evaluates the fourth equation in r^2 to obtain r.

    MgcVector3 kE10 = rkP0 - rkP3;
    MgcVector3 kE20 = rkP1 - rkP3;
    MgcVector3 kE30 = rkP2 - rkP3;

    MgcReal aafA[3][3];
    aafA[0][0] = kE10.Dot(kE10);
    aafA[0][1] = kE10.Dot(kE20);
    aafA[0][2] = kE10.Dot(kE30);
    aafA[1][0] = aafA[0][1];
    aafA[1][1] = kE20.Dot(kE20);
    aafA[1][2] = kE20.Dot(kE30);
    aafA[2][0] = aafA[0][2];
    aafA[2][1] = aafA[1][2];
    aafA[2][2] = kE30.Dot(kE30);

    MgcReal afB[3];
    afB[0] = 0.5*aafA[0][0];
    afB[1] = 0.5*aafA[1][1];
    afB[2] = 0.5*aafA[2][2];

    MgcReal aafAInv[3][3];
    aafAInv[0][0] = aafA[1][1]*aafA[2][2]-aafA[1][2]*aafA[2][1];
    aafAInv[0][1] = aafA[0][2]*aafA[2][1]-aafA[0][1]*aafA[2][2];
    aafAInv[0][2] = aafA[0][1]*aafA[1][2]-aafA[0][2]*aafA[1][1];
    aafAInv[1][0] = aafA[1][2]*aafA[2][0]-aafA[1][0]*aafA[2][2];
    aafAInv[1][1] = aafA[0][0]*aafA[2][2]-aafA[0][2]*aafA[2][0];
    aafAInv[1][2] = aafA[0][2]*aafA[1][0]-aafA[0][0]*aafA[1][2];
    aafAInv[2][0] = aafA[1][0]*aafA[2][1]-aafA[1][1]*aafA[2][0];
    aafAInv[2][1] = aafA[0][1]*aafA[2][0]-aafA[0][0]*aafA[2][1];
    aafAInv[2][2] = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0];
    MgcReal fDet = aafA[0][0]*aafAInv[0][0] + aafA[0][1]*aafAInv[1][0] +
        aafA[0][2]*aafAInv[2][0];
    assert( MgcMath::Abs(fDet) > gs_fEpsilon );

    MgcSphere kMinimal;

    MgcReal fInvDet = 1.0/fDet;
    int iRow, iCol;
    for (iRow = 0; iRow < 3; iRow++)
    {
        for (iCol = 0; iCol < 3; iCol++)
            aafAInv[iRow][iCol] *= fInvDet;
    }
        
    MgcReal afU[4];
    for (iRow = 0; iRow < 3; iRow++)
    {
        afU[iRow] = 0.0;
        for (iCol = 0; iCol < 3; iCol++)
            afU[iRow] += aafAInv[iRow][iCol]*afB[iCol];
    }
    afU[3] = 1.0 - afU[0] - afU[1] - afU[2];
        
    kMinimal.Center() = afU[0]*rkP0 + afU[1]*rkP1 + afU[2]*rkP2 + afU[3]*rkP3;
    MgcVector3 kTmp = afU[0]*kE10 + afU[1]*kE20 + afU[2]*kE30;
    kMinimal.Radius() = kTmp.SquaredLength();

    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere UpdateSupport1 (int i, MgcVector3** apkPerm, Support& rkSupp)
{
    const MgcVector3& rkP0 = *apkPerm[rkSupp.m_aiIndex[0]];
    const MgcVector3& rkP1 = *apkPerm[i];

    MgcSphere kMinimal = ExactSphere2(rkP0,rkP1);
    rkSupp.m_iQuantity = 2;
    rkSupp.m_aiIndex[1] = i;

    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere UpdateSupport2 (int i, MgcVector3** apkPerm, Support& rkSupp)
{
    const MgcVector3& rkP0 = *apkPerm[rkSupp.m_aiIndex[0]];
    const MgcVector3& rkP1 = *apkPerm[rkSupp.m_aiIndex[1]];
    const MgcVector3& rkP2 = *apkPerm[i];

    MgcSphere akS[3];
    MgcReal fMinRSqr = MgcMath::INFINITY;
    int iIndex = -1;

    akS[0] = ExactSphere2(rkP0,rkP2);
    if ( PointInsideSphere(rkP1,akS[0]) )
    {
        fMinRSqr = akS[0].Radius();
        iIndex = 0;
    }

    akS[1] = ExactSphere2(rkP1,rkP2);
    if ( PointInsideSphere(rkP0,akS[1]) )
    {
        if ( akS[1].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[1].Radius();
            iIndex = 1;
        }
    }

    MgcSphere kMinimal;

    if ( iIndex != -1 )
    {
        kMinimal = akS[iIndex];
        rkSupp.m_aiIndex[1-iIndex] = i;
    }
    else
    {
        kMinimal = ExactSphere3(rkP0,rkP1,rkP2);
        assert( kMinimal.Radius() <= fMinRSqr );
        rkSupp.m_iQuantity = 3;
        rkSupp.m_aiIndex[2] = i;
    }

    return kMinimal;
}
//----------------------------------------------------------------------------
static MgcSphere UpdateSupport3 (int i, MgcVector3** apkPerm, Support& rkSupp)
{
    const MgcVector3& rkP0 = *apkPerm[rkSupp.m_aiIndex[0]];
    const MgcVector3& rkP1 = *apkPerm[rkSupp.m_aiIndex[1]];
    const MgcVector3& rkP2 = *apkPerm[rkSupp.m_aiIndex[2]];
    const MgcVector3& rkP3 = *apkPerm[i];

    MgcSphere akS[6];
    MgcReal fMinRSqr = MgcMath::INFINITY;
    int iIndex = -1;

    akS[0] = ExactSphere2(rkP0,rkP3);
    if ( PointInsideSphere(rkP1,akS[0]) && PointInsideSphere(rkP2,akS[0]) )
    {
        fMinRSqr = akS[0].Radius();
        iIndex = 0;
    }

    akS[1] = ExactSphere2(rkP1,rkP3);
    if ( PointInsideSphere(rkP0,akS[1]) && PointInsideSphere(rkP2,akS[1]) )
    {
        if ( akS[1].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[1].Radius();
            iIndex = 1;
        }
    }

    akS[2] = ExactSphere2(rkP2,rkP3);
    if ( PointInsideSphere(rkP0,akS[2]) && PointInsideSphere(rkP1,akS[2]) )
    {
        if ( akS[2].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[2].Radius();
            iIndex = 2;
        }
    }

    akS[3] = ExactSphere3(rkP0,rkP1,rkP3);
    if ( PointInsideSphere(rkP2,akS[3]) )
    {
        if ( akS[3].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[3].Radius();
            iIndex = 3;
        }
    }

    akS[4] = ExactSphere3(rkP0,rkP2,rkP3);
    if ( PointInsideSphere(rkP1,akS[4]) )
    {
        if ( akS[4].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[4].Radius();
            iIndex = 4;
        }
    }

    akS[5] = ExactSphere3(rkP1,rkP2,rkP3);
    if ( PointInsideSphere(rkP0,akS[5]) )
    {
        if ( akS[5].Radius() < fMinRSqr )
        {
            fMinRSqr = akS[5].Radius();
            iIndex = 5;
        }
    }

    MgcSphere kMinimal;

    switch ( iIndex )
    {
    case 0:
        kMinimal = akS[0];
        rkSupp.m_iQuantity = 2;
        rkSupp.m_aiIndex[1] = i;
        break;
    case 1: