basic/geometry/geo_rangesensor/MgcIntrLin3Cyln.cpp

Go to the documentation of this file.

// 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.html/free.pdf

#include "MgcDistLin3Lin3.h"
#include "MgcIntrLin3Cyln.h"
#include "MgcRTLib.h"

static const MgcReal gs_fEpsilon = 1e-12;

//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcSegment3& rkSegment,
    const MgcCylinder& rkCylinder)
{
    MgcSegment3 kCylnSeg = rkCylinder.GetSegment();
    MgcReal fSqrDist = MgcSqrDistance(rkSegment,kCylnSeg);
    if ( fSqrDist > rkCylinder.Radius() )
        return false;

    // For Dot(N,D) not zero, line P+t*D intersects bottom plane of cylinder
    // at t0 = [Dot(N,C-P)+h/2]/Dot(N,D) where C is cylinder center and h is
    // cylinder height.  Line intersects top plane of cylinder at
    // t1 = [Dot(N,C-P)+h/2]/Dot(N,D).  Intersection occurs if and only
    // if [0,1] and [t0,t1] overlap.  Thus, intersection occurs when t0 >= 0
    // and t1 <= 1.  The implementation avoids the division by Dot(N,D) by
    // multiplying through by that term.  When Dot(N,D) is zero, the segment
    // and cylinder intersect when |Dot(N,C-P)| <= h/2.  The test for
    // Dot(N,D) > 0 degenerates to the test for Dot(N,D) = 0, so no special
    // handling must be done to trap Dot(N,D) within 'epsilon' of zero.

    MgcReal fNdD = rkCylinder.Direction().Dot(rkSegment.Direction());
    MgcVector3 kDiff = rkCylinder.Center() - rkSegment.Origin();
    MgcReal fTest  = kDiff.Dot(rkCylinder.Direction());
    MgcReal fHalfHeight = 0.5*rkCylinder.Height();
    if ( fNdD >= 0.0 )
        return -fHalfHeight <= fTest && fTest <= fNdD + fHalfHeight;
    else
        return fNdD - fHalfHeight <= fTest && fTest <= fHalfHeight;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcRay3& rkRay, const MgcCylinder& rkCylinder)
{
    MgcSegment3 kCylnSeg = rkCylinder.GetSegment();
    MgcReal fSqrDist = MgcSqrDistance(rkRay,kCylnSeg);
    if ( fSqrDist > rkCylinder.Radius() )
        return false;

    MgcReal fNdD = rkCylinder.Direction().Dot(rkRay.Direction());
    MgcVector3 kDiff = rkCylinder.Center() - rkRay.Origin();
    MgcReal fTest  = kDiff.Dot(rkCylinder.Direction());
    MgcReal fHalfHeight = 0.5*rkCylinder.Height();
    if ( fNdD > 0.0 )
        return fTest >= -fHalfHeight;
    else if ( fNdD < 0.0 )
        return fTest <= fHalfHeight;
    else
        return MgcMath::Abs(fTest) <= fHalfHeight;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcLine3& rkLine,
    const MgcCylinder& rkCylinder)
{
    MgcSegment3 kCylnSeg = rkCylinder.GetSegment();
    MgcReal fSqrDist = MgcSqrDistance(rkLine,kCylnSeg);
    return fSqrDist <= rkCylinder.Radius();
}
//----------------------------------------------------------------------------
static int FindIntersection (const MgcVector3& rkOrigin,
    const MgcVector3& rkDirection, const MgcCylinder& rkCylinder,
    MgcReal afT[2])
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kU, kV, kW = rkCylinder.Direction();
    MgcVector3::GenerateOrthonormalBasis(kU,kV,kW);
    MgcVector3 kD(kU.Dot(rkDirection),kV.Dot(rkDirection),
        kW.Dot(rkDirection));
    MgcReal fDLength = kD.Unitize();
    MgcReal fInvDLength = 1.0/fDLength;
    MgcVector3 kDiff = rkOrigin - rkCylinder.Center();
    MgcVector3 kP(kU.Dot(kDiff),kV.Dot(kDiff),kW.Dot(kDiff));
    MgcReal fHalfHeight = 0.5*rkCylinder.Height();
    MgcReal fRadiusSqr = rkCylinder.Radius()*rkCylinder.Radius();

    MgcReal fInv, fA, fB, fC, fDiscr, fRoot, fT, fT0, fT1, fTmp0, fTmp1;

    if ( MgcMath::Abs(kD.z) >= 1.0 - gs_fEpsilon )
    {
        // line is parallel to cylinder axis
        if ( kP.x*kP.x+kP.y*kP.y <= fRadiusSqr )
        {
            fTmp0 = fInvDLength/kD.z;
            afT[0] = (+fHalfHeight - kP.z)*fTmp0;
            afT[1] = (-fHalfHeight - kP.z)*fTmp0;
            return 2;
        }
        else
        {
            return 0;
        }
    }

    if ( MgcMath::Abs(kD.z) <= gs_fEpsilon )
    {
        // line is perpendicular to axis of cylinder
        if ( MgcMath::Abs(kP.z) > fHalfHeight )
        {
            // line is outside the planar caps of cylinder
            return 0;
        }

        fA = kD.x*kD.x + kD.y*kD.y;
        fB = kP.x*kD.x + kP.y*kD.y;
        fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
        fDiscr = fB*fB - fA*fC;
        if ( fDiscr < 0.0 )
        {
            // line does not intersect cylinder wall
            return 0;
        }
        else if ( fDiscr > 0.0 )
        {
            fRoot = MgcMath::Sqrt(fDiscr);
            fTmp0 = fInvDLength/fA;
            afT[0] = (-fB - fRoot)*fTmp0;
            afT[1] = (-fB + fRoot)*fTmp0;
            return 2;
        }
        else
        {
            afT[0] = -fB*fInvDLength/fA;
            return 1;
        }
    }

    // test plane intersections first
    int iQuantity = 0;
    fInv = 1.0/kD.z;
    fT0 = (+fHalfHeight - kP.z)*fInv;
    fTmp0 = kP.x + fT0*kD.x;
    fTmp1 = kP.y + fT0*kD.y;
    if ( fTmp0*fTmp0 + fTmp1*fTmp1 <= fRadiusSqr )
        afT[iQuantity++] = fT0*fInvDLength;

    fT1 = (-fHalfHeight - kP.z)*fInv;
    fTmp0 = kP.x + fT1*kD.x;
    fTmp1 = kP.y + fT1*kD.y;
    if ( fTmp0*fTmp0 + fTmp1*fTmp1 <= fRadiusSqr )
        afT[iQuantity++] = fT1*fInvDLength;

    if ( iQuantity == 2 )
    {
        // line intersects both top and bottom
        return 2;
    }

    // If iQuantity == 1, then line must intersect cylinder wall
    // somewhere between caps in a single point.  This case is detected
    // in the following code that tests for intersection between line and
    // cylinder wall.

    fA = kD.x*kD.x + kD.y*kD.y;
    fB = kP.x*kD.x + kP.y*kD.y;
    fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
    fDiscr = fB*fB - fA*fC;
    if ( fDiscr < 0.0 )
    {
        // line does not intersect cylinder wall
        assert( iQuantity == 0 );
        return 0;
    }
    else if ( fDiscr > 0.0 )
    {
        fRoot = MgcMath::Sqrt(fDiscr);
        fInv = 1.0/fA;
        fT = (-fB - fRoot)*fInv;
        if ( fT0 <= fT1 )
        {
            if ( fT0 <= fT && fT <= fT1 )
                afT[iQuantity++] = fT*fInvDLength;
        }
        else
        {
            if ( fT1 <= fT && fT <= fT0 )
                afT[iQuantity++] = fT*fInvDLength;
        }

        if ( iQuantity == 2 )
        {
            // Line intersects one of top/bottom of cylinder and once on
            // cylinder wall.
            return 2;
        }

        fT = (-fB + fRoot)*fInv;
        if ( fT0 <= fT1 )
        {
            if ( fT0 <= fT && fT <= fT1 )
                afT[iQuantity++] = fT*fInvDLength;
        }
        else
        {
            if ( fT1 <= fT && fT <= fT0 )
                afT[iQuantity++] = fT*fInvDLength;
        }
    }
    else
    {
        fT = -fB/fA;
        if ( fT0 <= fT1 )
        {
            if ( fT0 <= fT && fT <= fT1 )
                afT[iQuantity++] = fT*fInvDLength;
        }
        else
        {
            if ( fT1 <= fT && fT <= fT0 )
                afT[iQuantity++] = fT*fInvDLength;
        }
    }

    return iQuantity;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcSegment3& rkSegment,
    const MgcCylinder& rkCylinder, int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkSegment.Origin(),
        rkSegment.Direction(),rkCylinder,afT);

    int iClipQuantity = 0;
    for (int i = 0; i < riQuantity; i++)
    {
        if ( 0.0 <= afT[i] && afT[i] <= 1.0 )
        {
            akPoint[iClipQuantity++] = rkSegment.Origin() +
                afT[i]*rkSegment.Direction();
        }
    }

    riQuantity = iClipQuantity;
    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcRay3& rkRay,
    const MgcCylinder& rkCylinder, double &t)
{
    MgcReal afT[2];
    int riQuantity;
    int retvalue = 0;

    riQuantity = FindIntersection(rkRay.Origin(),rkRay.Direction(),
        rkCylinder,afT);

    t = DBL_MAX;

        for (int i = 0; i < riQuantity; i++)
    {
        if (( afT[i] > 0 ) && (afT[i] < t))
        {
           t = afT[i];
           retvalue++;
        }
    }

    return (retvalue > 0);
}
//----------------------------------------------------------------------------
bool MgcFindIntersection2 (const MgcRay3& rkRay,
    const MgcCylinder& rkCylinder, int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkRay.Origin(),rkRay.Direction(),
        rkCylinder,afT);

    int iClipQuantity = 0;
    for (int i = 0; i < riQuantity; i++)
    {
        if ( afT[i] >= 0.0 )
        {
            akPoint[iClipQuantity++] = rkRay.Origin() +
                afT[i]*rkRay.Direction();
        }
    }

    riQuantity = iClipQuantity;
    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcLine3& rkLine,
    const MgcCylinder& rkCylinder, int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkLine.Origin(),rkLine.Direction(),
        rkCylinder,afT);

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

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

---