mgccontlozenge.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 "MgcContLozenge.h"
#include "MgcDistVec3Lin3.h"
#include "MgcGaussPointsFit.h"

//----------------------------------------------------------------------------
MgcLozenge MgcContLozenge (int iQuantity, const MgcVector3* akPoint)
{
    MgcLozenge kLozenge;

    // Fit with Gaussian.  Axis(0) corresponds to the smallest eigenvalue.
    MgcVector3 kCenter, akAxis[3];
    MgcReal afExtent[3];
    MgcGaussPointsFit(iQuantity,akPoint,kCenter,akAxis,afExtent);

    MgcVector3 kDiff = akPoint[0] - kCenter;
    MgcReal fWMin = akAxis[0].Dot(kDiff), fWMax = fWMin, fW;
    int i;
    for (i = 1; i < iQuantity; i++)
    {
        kDiff = akPoint[i] - kCenter;
        fW = akAxis[0].Dot(kDiff);
        if ( fW < fWMin )
            fWMin = fW;
        else if ( fW > fWMax )
            fWMax = fW;
    }

    MgcReal fRadius = 0.5*(fWMax - fWMin);
    MgcReal fRSqr = fRadius*fRadius;
    kCenter += (0.5*(fWMax + fWMin))*akAxis[0];

    MgcReal fAMin = MgcMath::INFINITY, fAMax = -MgcMath::INFINITY;
    MgcReal fBMin = MgcMath::INFINITY, fBMax = -MgcMath::INFINITY;
    MgcReal fDiscr, fRadical, fU, fV, fTest;
    for (i = 0; i < iQuantity; i++)
    {
        kDiff = akPoint[i] - kCenter;
        fU = akAxis[2].Dot(kDiff);
        fV = akAxis[1].Dot(kDiff);
        fW = akAxis[0].Dot(kDiff);
        fDiscr = fRSqr - fW*fW;
        fRadical = MgcMath::Sqrt(MgcMath::Abs(fDiscr));

        fTest = fU + fRadical;
        if ( fTest < fAMin )
            fAMin = fTest;

        fTest = fU - fRadical;
        if ( fTest > fAMax )
            fAMax = fTest;

        fTest = fV + fRadical;
        if ( fTest < fBMin )
            fBMin = fTest;

        fTest = fV - fRadical;
        if ( fTest > fBMax )
            fBMax = fTest;
    }

    // enclosing region might be a capsule or a sphere
    if ( fAMin >= fAMax )
    {
        fTest = 0.5*(fAMin + fAMax);
        fAMin = fTest;
        fAMax = fTest;
    }
    if ( fBMin >= fBMax )
    {
        fTest = 0.5*(fBMin + fBMax);
        fBMin = fTest;
        fBMax = fTest;
    }

    // Make correction for points inside mitered corner but outside quarter
    // sphere.
    for (i = 0; i < iQuantity; i++)
    {
        kDiff = akPoint[i] - kCenter;
        fU = akAxis[2].Dot(kDiff);
        fV = akAxis[1].Dot(kDiff);

        MgcReal* pfAExtreme = 0;
        MgcReal* pfBExtreme = 0;

        if ( fU > fAMax )
        {
            if ( fV > fBMax )
            {
                pfAExtreme = &fAMax;
                pfBExtreme = &fBMax;
            }
            else if ( fV < fBMin )
            {
                pfAExtreme = &fAMax;
                pfBExtreme = &fBMin;
            }
        }
        else if ( fU < fAMin )
        {
            if ( fV > fBMax )
            {
                pfAExtreme = &fAMin;
                pfBExtreme = &fBMax;
            }
            else if ( fV < fBMin )
            {
                pfAExtreme = &fAMin;
                pfBExtreme = &fBMin;
            }
        }

        if ( pfAExtreme )
        {
            MgcReal fDeltaU = fU - *pfAExtreme;
            MgcReal fDeltaV = fV - *pfBExtreme;
            MgcReal fDeltaSumSqr = fDeltaU*fDeltaU + fDeltaV*fDeltaV;
            fW = akAxis[0].Dot(kDiff);
            MgcReal fWSqr = fW*fW;
            fTest = fDeltaSumSqr + fWSqr;
            if ( fTest > fRSqr )
            {
                fDiscr = (fRSqr - fWSqr)/fDeltaSumSqr;
                MgcReal fT = -MgcMath::Sqrt(MgcMath::Abs(fDiscr));
                *pfAExtreme = fU + fT*fDeltaU;
                *pfBExtreme = fV + fT*fDeltaV;
            }
        }
    }

    if ( fAMin < fAMax )
    {
        if ( fBMin < fBMax )
        {
            kLozenge.Origin() = kCenter + fAMin*akAxis[2] + fBMin*akAxis[1];
            kLozenge.Edge0() = (fAMax - fAMin)*akAxis[2];
            kLozenge.Edge1() = (fBMax - fBMin)*akAxis[1];
        }
        else
        {
            // enclosing lozenge is really a capsule
            kLozenge.Origin() = kCenter + fAMin*akAxis[2] +
                (0.5*(fBMin+fBMax))*akAxis[1];
            kLozenge.Edge0() = (fAMax - fAMin)*akAxis[2];
            kLozenge.Edge1() = MgcVector3::ZERO;
        }
    }
    else
    {
        if ( fBMin < fBMax )
        {
            // enclosing lozenge is really a capsule
            kLozenge.Origin() = kCenter + (0.5*(fAMin+fAMax))*akAxis[2] +
                fBMin*akAxis[1];
            kLozenge.Edge0() = MgcVector3::ZERO;
            kLozenge.Edge1() = (fBMax - fBMin)*akAxis[1];
        }
        else
        {
            // enclosing lozenge is really a sphere
            kLozenge.Origin() = kCenter + (0.5*(fAMin+fAMax))*akAxis[2] +
                (0.5*(fBMin+fBMax))*akAxis[1];
            kLozenge.Edge0() = MgcVector3::ZERO;
            kLozenge.Edge1() = MgcVector3::ZERO;
        }
    }

    kLozenge.Radius() = fRadius;

    return kLozenge;
}
//----------------------------------------------------------------------------
bool MgcContLozenge (int iQuantity, const MgcVector3* akPoint,
    const bool* abValid, MgcLozenge& rkLozenge)
{
    // Fit with Gaussian.  Axis(0) corresponds to the smallest eigenvalue.
    MgcVector3 kCenter, akAxis[3];
    MgcReal afExtent[3];
    if ( !MgcGaussPointsFit(iQuantity,akPoint,abValid,kCenter,akAxis,
        afExtent) )
    {
        return false;
    }

    MgcVector3 kDiff;
    MgcReal fWMin, fWMax, fW;
    int i;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            kDiff = akPoint[i] - kCenter;
            fWMin = akAxis[0].Dot(kDiff);
            fWMax = fWMin;
            break;
        }
    }

    for (i++; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            kDiff = akPoint[i] - kCenter;
            fW = akAxis[0].Dot(kDiff);
            if ( fW < fWMin )
                fWMin = fW;
            else if ( fW > fWMax )
                fWMax = fW;
        }
    }

    MgcReal fRadius = 0.5*(fWMax - fWMin);
    MgcReal fRSqr = fRadius*fRadius;
    kCenter += (0.5*(fWMax + fWMin))*akAxis[0];

    MgcReal fAMin = MgcMath::INFINITY, fAMax = -MgcMath::INFINITY;
    MgcReal fBMin = MgcMath::INFINITY, fBMax = -MgcMath::INFINITY;
    MgcReal fDiscr, fRadical, fU, fV, fTest;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            kDiff = akPoint[i] - kCenter;
            fU = akAxis[2].Dot(kDiff);
            fV = akAxis[1].Dot(kDiff);
            fW = akAxis[0].Dot(kDiff);
            fDiscr = fRSqr - fW*fW;
            fRadical = MgcMath::Sqrt(MgcMath::Abs(fDiscr));

            fTest = fU + fRadical;
            if ( fTest < fAMin )
                fAMin = fTest;

            fTest = fU - fRadical;
            if ( fTest > fAMax )
                fAMax = fTest;

            fTest = fV + fRadical;
            if ( fTest < fBMin )
                fBMin = fTest;

            fTest = fV - fRadical;
            if ( fTest > fBMax )
                fBMax = fTest;
        }
    }

    // enclosing region might be a capsule or a sphere
    if ( fAMin >= fAMax )
    {
        fTest = 0.5*(fAMin + fAMax);
        fAMin = fTest;
        fAMax = fTest;
    }
    if ( fBMin >= fBMax )
    {
        fTest = 0.5*(fBMin + fBMax);
        fBMin = fTest;
        fBMax = fTest;
    }

    // Make correction for points inside mitered corner but outside quarter
    // sphere.
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            kDiff = akPoint[i] - kCenter;
            fU = akAxis[2].Dot(kDiff);
            fV = akAxis[1].Dot(kDiff);

            MgcReal* pfAExtreme = 0;
            MgcReal* pfBExtreme = 0;

            if ( fU > fAMax )
            {
                if ( fV > fBMax )
                {
                    pfAExtreme = &fAMax;
                    pfBExtreme = &fBMax;
                }
                else if ( fV < fBMin )
                {
                    pfAExtreme = &fAMax;
                    pfBExtreme = &fBMin;
                }
            }
            else if ( fU < fAMin )
            {
                if ( fV > fBMax )
                {
                    pfAExtreme = &fAMin;
                    pfBExtreme = &fBMax;
                }
                else if ( fV < fBMin )
                {
                    pfAExtreme = &fAMin;
                    pfBExtreme = &fBMin;
                }
            }

            if ( pfAExtreme )
            {
                MgcReal fDeltaU = fU - *pfAExtreme;
                MgcReal fDeltaV = fV - *pfBExtreme;
                MgcReal fDeltaSumSqr = fDeltaU*fDeltaU + fDeltaV*fDeltaV;
                fW = akAxis[0].Dot(kDiff);
                MgcReal fWSqr = fW*fW;
                fTest = fDeltaSumSqr + fWSqr;
                if ( fTest > fRSqr )
                {
                    fDiscr = (fRSqr - fWSqr)/fDeltaSumSqr;
                    MgcReal fT = -MgcMath::Sqrt(MgcMath::Abs(fDiscr));
                    *pfAExtreme = fU + fT*fDeltaU;
                    *pfBExtreme = fV + fT*fDeltaV;
                }
            }
        }
    }

    if ( fAMin < fAMax )
    {
        if ( fBMin < fBMax )
        {
            rkLozenge.Origin() = kCenter + fAMin*akAxis[2] + fBMin*akAxis[1];
            rkLozenge.Edge0() = (fAMax - fAMin)*akAxis[2];
            rkLozenge.Edge1() = (fBMax - fBMin)*akAxis[1];
        }
        else
        {
            // enclosing lozenge is really a capsule
            rkLozenge.Origin() = kCenter + fAMin*akAxis[2] +
                (0.5*(fBMin+fBMax))*akAxis[1];
            rkLozenge.Edge0() = (fAMax - fAMin)*akAxis[2];
            rkLozenge.Edge1() = MgcVector3::ZERO;
        }
    }
    else
    {
        if ( fBMin < fBMax )
        {
            // enclosing lozenge is really a capsule
            rkLozenge.Origin() = kCenter + (0.5*(fAMin+fAMax))*akAxis[2] +
                fBMin*akAxis[1];
            rkLozenge.Edge0() = MgcVector3::ZERO;
            rkLozenge.Edge1() = (fBMax - fBMin)*akAxis[1];
        }
        else
        {
            // enclosing lozenge is really a sphere
            rkLozenge.Origin() = kCenter + (0.5*(fAMin+fAMax))*akAxis[2] +
                (0.5*(fBMin+fBMax))*akAxis[1];
            rkLozenge.Edge0() = MgcVector3::ZERO;
            rkLozenge.Edge1() = MgcVector3::ZERO;
        }
    }

    rkLozenge.Radius() = fRadius;

    return true;
}
//----------------------------------------------------------------------------