mgccontcapsule.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 "MgcContCapsule.h"
#include "MgcDistVec3Lin3.h"
#include "MgcLineFit.h"

//----------------------------------------------------------------------------
MgcCapsule MgcContCapsule (int iQuantity, const MgcVector3* akPoint)
{
    MgcCapsule kCapsule;

    MgcLine3 kLine;
    MgcOrthogonalLineFit(iQuantity,akPoint,kLine.Origin(),kLine.Direction());

    MgcReal fMaxRadiusSqr = 0.0;
    int i;
    for (i = 0; i < iQuantity; i++)
    {
        MgcReal fRadiusSqr = MgcSqrDistance(akPoint[i],kLine);
        if ( fRadiusSqr > fMaxRadiusSqr )
            fMaxRadiusSqr = fRadiusSqr;
    }

    MgcVector3 kU, kV, kW = kLine.Direction();
    MgcVector3::GenerateOrthonormalBasis(kU,kV,kW);

    MgcReal fMin = MgcMath::INFINITY, fMax = -MgcMath::INFINITY;
    for (i = 0; i < iQuantity; i++)
    {
        MgcVector3 kDiff = akPoint[i] - kLine.Origin();
        MgcReal fU = kU.Dot(kDiff);
        MgcReal fV = kV.Dot(kDiff);
        MgcReal fW = kW.Dot(kDiff);
        MgcReal fDiscr = fMaxRadiusSqr - (fU*fU + fV*fV);
        MgcReal fRadical = MgcMath::Sqrt(MgcMath::Abs(fDiscr));

        MgcReal fTest = fW + fRadical;
        if ( fTest < fMin )
            fMin = fTest;

        fTest = fW - fRadical;
        if ( fTest > fMax )
            fMax = fTest;
    }

    if ( fMin < fMax )
    {
        kCapsule.Origin() = kLine.Origin() + fMin*kLine.Direction();
        kCapsule.Direction() = (fMax-fMin)*kLine.Direction();
    }
    else
    {
        // enclosing capsule is really a sphere
        kCapsule.Origin() = kLine.Origin() +
            (0.5*(fMin+fMax))*kLine.Direction();
        kCapsule.Direction() = MgcVector3::ZERO;
    }

    kCapsule.Radius() = MgcMath::Sqrt(fMaxRadiusSqr);

    return kCapsule;
}
//----------------------------------------------------------------------------
bool MgcContCapsule (int iQuantity, const MgcVector3* akPoint,
    const bool* abValid, MgcCapsule& rkCapsule)
{
    MgcLine3 kLine;
    if ( !MgcOrthogonalLineFit(iQuantity,akPoint,abValid,kLine.Origin(),
         kLine.Direction()) )
    {
        return false;
    }

    MgcReal fMaxRadiusSqr = 0.0;
    int i;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            MgcReal fRadiusSqr = MgcSqrDistance(akPoint[i],kLine);
            if ( fRadiusSqr > fMaxRadiusSqr )
                fMaxRadiusSqr = fRadiusSqr;
        }
    }

    MgcVector3 kU, kV, kW = kLine.Direction();
    MgcVector3::GenerateOrthonormalBasis(kU,kV,kW);

    MgcReal fMin = MgcMath::INFINITY, fMax = -MgcMath::INFINITY;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            MgcVector3 kDiff = akPoint[i] - kLine.Origin();
            MgcReal fU = kU.Dot(kDiff);
            MgcReal fV = kV.Dot(kDiff);
            MgcReal fW = kW.Dot(kDiff);
            MgcReal fDiscr = fMaxRadiusSqr - (fU*fU + fV*fV);
            MgcReal fRadical = MgcMath::Sqrt(MgcMath::Abs(fDiscr));

            MgcReal fTest = fW + fRadical;
            if ( fTest < fMin )
                fMin = fTest;

            fTest = fW - fRadical;
            if ( fTest > fMax )
                fMax = fTest;
        }
    }

    if ( fMin < fMax )
    {
        rkCapsule.Origin() = kLine.Origin() + fMin*kLine.Direction();
        rkCapsule.Direction() = (fMax-fMin)*kLine.Direction();
    }
    else
    {
        // enclosing capsule is really a sphere
        rkCapsule.Origin() = kLine.Origin() +
            (0.5*(fMin+fMax))*kLine.Direction();
        rkCapsule.Direction() = MgcVector3::ZERO;
    }

    rkCapsule.Radius() = MgcMath::Sqrt(fMaxRadiusSqr);

    return true;
}
//----------------------------------------------------------------------------
bool MgcInCapsule (const MgcVector3& rkPoint, const MgcCapsule& rkCapsule)
{
    MgcReal fRSqr = rkCapsule.Radius()*rkCapsule.Radius();
    return MgcSqrDistance(rkPoint,rkCapsule.Segment()) <= fRSqr;
}
//----------------------------------------------------------------------------
bool MgcInCapsule (const MgcSphere& rkSphere, const MgcCapsule& rkCapsule)
{
    MgcReal fRDiff = rkCapsule.Radius() - rkSphere.Radius();
    if ( fRDiff >= 0.0 )
    {
        return MgcSqrDistance(rkSphere.Center(),rkCapsule.Segment()) <=
            fRDiff*fRDiff;
    }
    else
    {
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcInCapsule (const MgcCapsule& rkTestCapsule,
    const MgcCapsule& rkCapsule)
{
    MgcSphere kSphere0, kSphere1;
    kSphere0.Center() = rkTestCapsule.Origin();
    kSphere0.Radius() = rkTestCapsule.Radius();
    kSphere1.Center() = rkTestCapsule.Origin() + rkTestCapsule.Direction();
    kSphere1.Radius() = rkTestCapsule.Radius();

    return MgcInCapsule(kSphere0,rkCapsule)
        && MgcInCapsule(kSphere1,rkCapsule);
}
//----------------------------------------------------------------------------
MgcCapsule MgcMergeCapsules (const MgcCapsule& rkCapsule0,
    const MgcCapsule& rkCapsule1)
{
    if ( MgcInCapsule(rkCapsule0,rkCapsule1) )
        return rkCapsule1;

    if ( MgcInCapsule(rkCapsule1,rkCapsule0) )
        return rkCapsule0;

    const MgcVector3& rkP0 = rkCapsule0.Origin();
    const MgcVector3& rkP1 = rkCapsule1.Origin();
    const MgcVector3& rkD0 = rkCapsule0.Direction();
    const MgcVector3& rkD1 = rkCapsule1.Direction();

    // axis of final capsule
    MgcLine3 kLine;

    // axis center is average of input axis centers
    kLine.Origin() = 0.5*(rkP0 + rkP1) + 0.25*(rkD0 + rkD1);

    // axis unit direction is average of input axis unit directions
    MgcVector3 kDirection0 = rkD0;
    MgcVector3 kDirection1 = rkD1;
    kDirection0.Unitize();
    kDirection1.Unitize();
    MgcVector3& rkLineDir = (MgcVector3&) kLine.Direction();
    if ( kDirection0.Dot(kDirection1) >= 0.0 )
        rkLineDir = kDirection0 + kDirection1;
    else
        rkLineDir = kDirection0 - kDirection1;
    rkLineDir.Unitize();

    // Cylinder with axis 'kLine' must contain the spheres centered at the
    // end points of the input capsules.
    MgcReal fRadius = MgcDistance(rkP0,kLine) + rkCapsule0.Radius();

    MgcReal fDist = MgcDistance(rkP1,kLine) + rkCapsule1.Radius();
    if ( fDist > fRadius )
        fRadius = fDist;

    MgcVector3 kP0D0 = rkP0 + rkD0;
    fDist = MgcDistance(kP0D0,kLine) + rkCapsule0.Radius();
    if ( fDist > fRadius )
        fRadius = fDist;

    MgcVector3 kP1D1 = rkP1 + rkD1;
    fDist = MgcDistance(kP1D1,kLine) + rkCapsule1.Radius();
    if ( fDist > fRadius )
        fRadius = fDist;

    // process sphere <P0,r0>
    MgcReal fRDiff = fRadius - rkCapsule0.Radius();
    MgcReal fRDiffSqr = fRDiff*fRDiff;
    MgcVector3 kDiff = kLine.Origin() - rkP0;
    MgcReal fK0 = kDiff.SquaredLength() - fRDiffSqr;
    MgcReal fK1 = kDiff.Dot(kLine.Direction());
    MgcReal fDiscr = fK1*fK1 - fK0;  // assert:  K1*K1-K0 >= 0
    MgcReal fRoot = MgcMath::Sqrt(MgcMath::Abs(fDiscr));
    MgcReal fTPos = fK1 - fRoot;
    MgcReal fTNeg = fK1 + fRoot;
    MgcReal fTmp;

    // process sphere <P0+D0,r0>
    kDiff = kLine.Origin() - kP0D0;
    fK0 = kDiff.SquaredLength() - fRDiffSqr;
    fK1 = kDiff.Dot(kLine.Direction());
    fDiscr = fK1*fK1 - fK0;  // assert:  K1*K1-K0 >= 0
    fRoot = MgcMath::Sqrt(MgcMath::Abs(fDiscr));
    fTmp = fK1 - fRoot;
    if ( fTmp > fTPos )
        fTPos = fTmp;
    fTmp = fK1 + fRoot;
    if ( fTmp < fTNeg )
        fTNeg = fTmp;

    // process sphere <P1,r1>
    fRDiff = fRadius - rkCapsule1.Radius();
    fRDiffSqr = fRDiff*fRDiff;
    kDiff = kLine.Origin() - rkP1;
    fK0 = kDiff.SquaredLength() - fRDiffSqr;
    fK1 = kDiff.Dot(kLine.Direction());
    fDiscr = fK1*fK1 - fK0;  // assert:  K1*K1-K0 >= 0
    fRoot = MgcMath::Sqrt(MgcMath::Abs(fDiscr));
    fTmp = fK1 - fRoot;
    if ( fTmp > fTPos )
        fTPos = fTmp;
    fTmp = fK1 + fRoot;
    if ( fTmp < fTNeg )
        fTNeg = fTmp;

    // process sphere <P1+D1,r1>
    kDiff = kLine.Origin() - kP1D1;
    fK0 = kDiff.SquaredLength() - fRDiffSqr;
    fK1 = kDiff.Dot(kLine.Direction());
    fDiscr = fK1*fK1 - fK0;  // assert:  K1*K1-K0 >= 0
    fRoot = MgcMath::Sqrt(MgcMath::Abs(fDiscr));
    fTmp = fK1 - fRoot;
    if ( fTmp > fTPos )
        fTPos = fTmp;
    fTmp = fK1 + fRoot;
    if ( fTmp < fTNeg )
        fTNeg = fTmp;

    if ( fTPos < fTNeg )
    {
        MgcReal fAverage = 0.5*(fTPos + fTNeg);
        fTPos = fAverage;
        fTNeg = fAverage;
    }

    MgcCapsule kCapsule;
    kCapsule.Radius() = fRadius;
    kCapsule.Origin() = kLine.Origin() + fTNeg*kLine.Direction();
    kCapsule.Direction() = (fTPos - fTNeg)*kLine.Direction();
    return kCapsule;
}
//----------------------------------------------------------------------------