mgcintrlin3elp3.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 "MgcIntrLin3Elp3.h"

//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcSegment3& rkSegment,
    const MgcEllipsoid& rkEllipsoid)
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkSegment.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkSegment.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkSegment.Direction().Dot(kMatDir);
    MgcReal fB = rkSegment.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    // no intersection if Q(t) has no real roots
    MgcReal fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
        return false;

    // test if line origin is inside ellipsoid
    if ( fC <= 0.0 )
        return true;

    // At this point fC > 0 and Q(t) has real roots.  No intersection if
    // Q'(0) >= 0.
    if ( fB >= 0.0 )
        return false;

    // Need to determine if Q(t) has real roots on [0,1].  Effectively is
    // a test for sign changes of Sturm polynomials.
    MgcReal fSum = fA + fB;
    if ( fSum >= 0.0 )
        return true;

    return fSum + fB + fC <= 0.0;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcRay3& rkRay,
    const MgcEllipsoid& rkEllipsoid)
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkRay.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkRay.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkRay.Direction().Dot(kMatDir);  // fA > 0 is necessary
    MgcReal fB = rkRay.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    // no intersection if Q(t) has no real roots
    MgcReal fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
        return false;

    // test if ray origin is inside ellipsoid
    if ( fC <= 0.0 )
        return true;

    // At this point, fC > 0 and Q(t) has real roots.  Intersection occurs
    // if Q'(0) < 0.
    return fB < 0.0;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcLine3& rkLine,
    const MgcEllipsoid& rkEllipsoid)
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkLine.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkLine.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkLine.Direction().Dot(kMatDir);
    MgcReal fB = rkLine.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    // intersection occurs if Q(t) has real roots
    MgcReal fDiscr = fB*fB - fA*fC;
    return fDiscr >= 0.0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcSegment3& rkSegment,
    const MgcEllipsoid& rkEllipsoid, int& riQuantity, MgcVector3 akPoint[2])
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkSegment.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkSegment.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkSegment.Direction().Dot(kMatDir);
    MgcReal fB = rkSegment.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    // no intersection if Q(t) has no real roots
    MgcReal afT[2];
    MgcReal fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
    {
        riQuantity = 0;
    }
    else if ( fDiscr > 0.0 )
    {
        MgcReal fRoot = MgcMath::Sqrt(fDiscr);
        MgcReal fInvA = 1.0/fA;
        afT[0] = (-fB - fRoot)*fInvA;
        afT[1] = (-fB + fRoot)*fInvA;

        if ( afT[0] > 1.0 )
            riQuantity = 0;
        else if ( afT[0] >= 0.0 )
            riQuantity = ( afT[1] > 1.0 ? 1 : 2 );
        else if ( afT[1] >= 0.0 )
            riQuantity = 1;
        else
            riQuantity = 0;
    }
    else
    {
        afT[0] = -fB/fA;
        riQuantity = ( 0.0 <= afT[0] && afT[0] <= 1.0 ? 1 : 0 );
    }

    for (int i = 0; i < riQuantity; i++)
        akPoint[i] = rkSegment.Origin() + afT[i]*rkSegment.Direction();

    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcRay3& rkRay,
    const MgcEllipsoid& rkEllipsoid, int& riQuantity, MgcVector3 akPoint[2])
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkRay.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkRay.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkRay.Direction().Dot(kMatDir);  // fA > 0 is necessary
    MgcReal fB = rkRay.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    MgcReal afT[2];
    MgcReal fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
    {
        riQuantity = 0;
    }
    else if ( fDiscr > 0.0 )
    {
        MgcReal fRoot = MgcMath::Sqrt(fDiscr);
        MgcReal fInvA = 1.0/fA;
        afT[0] = (-fB - fRoot)*fInvA;
        afT[1] = (-fB + fRoot)*fInvA;

        if ( afT[0] >= 0.0 )
            riQuantity = 2;
        else if ( afT[1] >= 0.0 )
            riQuantity = 1;
        else
            riQuantity = 0;
    }
    else
    {
        afT[0] = -fB/fA;
        riQuantity = ( afT[0] >= 0.0 ? 1 : 0 );
    }

    for (int i = 0; i < riQuantity; i++)
        akPoint[i] = rkRay.Origin() + afT[i]*rkRay.Direction();

    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcLine3& rkLine,
    const MgcEllipsoid& rkEllipsoid, int& riQuantity, MgcVector3 akPoint[2])
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kDiff = rkLine.Origin() - rkEllipsoid.Center();
    MgcVector3 kMatDir = rkEllipsoid.A()*rkLine.Direction();
    MgcVector3 kMatDiff = rkEllipsoid.A()*kDiff;
    MgcReal fA = rkLine.Direction().Dot(kMatDir);
    MgcReal fB = rkLine.Direction().Dot(kMatDiff);
    MgcReal fC = kDiff.Dot(kMatDiff) - 1.0;

    MgcReal afT[2];
    MgcReal fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
    {
        riQuantity = 0;
    }
    else if ( fDiscr > 0.0 )
    {
        MgcReal fRoot = MgcMath::Sqrt(fDiscr);
        MgcReal fInvA = 1.0/fA;
        riQuantity = 2;
        afT[0] = (-fB - fRoot)*fInvA;
        afT[1] = (-fB + fRoot)*fInvA;
    }
    else
    {
        riQuantity = 1;
        afT[0] = -fB/fA;
    }

    for (int i = 0; i < riQuantity; i++)
        akPoint[i] = rkLine.Origin() + afT[i]*rkLine.Direction();

    return riQuantity > 0;
}
//----------------------------------------------------------------------------