basic/geometry/geo_rangesensor/MgcIntrLin3Box3.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/free.pdf

#include "MgcIntrLin3Box3.h"

//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcSegment3& rkSegment, const MgcBox3& rkBox)
{
    double fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
    MgcVector3 kSDir = 0.5*rkSegment.Direction();
    MgcVector3 kSCen = rkSegment.Origin() + kSDir;

    MgcVector3 kDiff = kSCen - rkBox.Center();

    fAWdU[0] = MgcMath::Abs(kSDir.Dot(rkBox.Axis(0)));
    fADdU[0] = MgcMath::Abs(kDiff.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(0) + fAWdU[0];
    if ( fADdU[0] > fRhs )
        return false;

    fAWdU[1] = MgcMath::Abs(kSDir.Dot(rkBox.Axis(1)));
    fADdU[1] = MgcMath::Abs(kDiff.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(1) + fAWdU[1];
    if ( fADdU[1] > fRhs )
        return false;

    fAWdU[2] = MgcMath::Abs(kSDir.Dot(rkBox.Axis(2)));
    fADdU[2] = MgcMath::Abs(kDiff.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(2) + fAWdU[2];
    if ( fADdU[2] > fRhs )
        return false;

    MgcVector3 kWxD = kSDir.Cross(kDiff);

    fAWxDdU[0] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
    if ( fAWxDdU[0] > fRhs )
        return false;

    fAWxDdU[1] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
    if ( fAWxDdU[1] > fRhs )
        return false;

    fAWxDdU[2] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
    if ( fAWxDdU[2] > fRhs )
        return false;

    return true;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcRay3& rkRay, const MgcBox3& rkBox)
{
    double fWdU[3], fAWdU[3], fDdU[3], fADdU[3], fAWxDdU[3], fRhs;

    MgcVector3 kDiff = rkRay.Origin() - rkBox.Center();

    fWdU[0] = rkRay.Direction().Dot(rkBox.Axis(0));
    fAWdU[0] = MgcMath::Abs(fWdU[0]);
    fDdU[0] = kDiff.Dot(rkBox.Axis(0));
    fADdU[0] = MgcMath::Abs(fDdU[0]);
    if ( fADdU[0] > rkBox.Extent(0) && fDdU[0]*fWdU[0] >= 0.0f )
        return false;

    fWdU[1] = rkRay.Direction().Dot(rkBox.Axis(1));
    fAWdU[1] = MgcMath::Abs(fWdU[1]);
    fDdU[1] = kDiff.Dot(rkBox.Axis(1));
    fADdU[1] = MgcMath::Abs(fDdU[1]);
    if ( fADdU[1] > rkBox.Extent(1) && fDdU[1]*fWdU[1] >= 0.0f )
        return false;

    fWdU[2] = rkRay.Direction().Dot(rkBox.Axis(2));
    fAWdU[2] = MgcMath::Abs(fWdU[2]);
    fDdU[2] = kDiff.Dot(rkBox.Axis(2));
    fADdU[2] = MgcMath::Abs(fDdU[2]);
    if ( fADdU[2] > rkBox.Extent(2) && fDdU[2]*fWdU[2] >= 0.0f )
        return false;

    MgcVector3 kWxD = rkRay.Direction().Cross(kDiff);

    fAWxDdU[0] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
    if ( fAWxDdU[0] > fRhs )
        return false;

    fAWxDdU[1] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
    if ( fAWxDdU[1] > fRhs )
        return false;

    fAWxDdU[2] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
    if ( fAWxDdU[2] > fRhs )
        return false;

    return true;
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcLine3& rkLine, const MgcBox3& rkBox)
{
    double fAWdU[3], fAWxDdU[3], fRhs;

    MgcVector3 kDiff = rkLine.Origin() - rkBox.Center();
    MgcVector3 kWxD = rkLine.Direction().Cross(kDiff);

    fAWdU[1] = MgcMath::Abs(rkLine.Direction().Dot(rkBox.Axis(1)));
    fAWdU[2] = MgcMath::Abs(rkLine.Direction().Dot(rkBox.Axis(2)));
    fAWxDdU[0] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
    if ( fAWxDdU[0] > fRhs )
        return false;

    fAWdU[0] = MgcMath::Abs(rkLine.Direction().Dot(rkBox.Axis(0)));
    fAWxDdU[1] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
    if ( fAWxDdU[1] > fRhs )
        return false;

    fAWxDdU[2] = MgcMath::Abs(kWxD.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
    if ( fAWxDdU[2] > fRhs )
        return false;

    return true;
}
//----------------------------------------------------------------------------
static bool Clip (MgcReal fDenom, MgcReal fNumer, MgcReal& rfT0,
    MgcReal& rfT1)
{
    // Return value is 'true' if line segment intersects the current test
    // plane.  Otherwise 'false' is returned in which case the line segment
    // is entirely clipped.

    if ( fDenom > 0.0 )
    {
        if ( fNumer > fDenom*rfT1 )
            return false;
        if ( fNumer > fDenom*rfT0 )
            rfT0 = fNumer/fDenom;
        return true;
    }
    else if ( fDenom < 0.0 )
    {
        if ( fNumer > fDenom*rfT0 )
            return false;
        if ( fNumer > fDenom*rfT1 )
            rfT1 = fNumer/fDenom;
        return true;
    }
    else
    {
        return fNumer <= 0.0;
    }
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcVector3& rkOrigin,
    const MgcVector3& rkDirection, const MgcReal afExtent[3], MgcReal& rfT0,
    MgcReal& rfT1)
{
    MgcReal fSaveT0 = rfT0, fSaveT1 = rfT1;

    bool bNotEntirelyClipped =
        Clip(+rkDirection.x,-rkOrigin.x-afExtent[0],rfT0,rfT1) &&
        Clip(-rkDirection.x,+rkOrigin.x-afExtent[0],rfT0,rfT1) &&
        Clip(+rkDirection.y,-rkOrigin.y-afExtent[1],rfT0,rfT1) &&
        Clip(-rkDirection.y,+rkOrigin.y-afExtent[1],rfT0,rfT1) &&
        Clip(+rkDirection.z,-rkOrigin.z-afExtent[2],rfT0,rfT1) &&
        Clip(-rkDirection.z,+rkOrigin.z-afExtent[2],rfT0,rfT1);

    return bNotEntirelyClipped && ( rfT0 != fSaveT0 || rfT1 != fSaveT1 );
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcSegment3& rkSegment, const MgcBox3& rkBox,
    int& riQuantity, MgcVector3 akPoint[2])
{
    // convert segment to box coordinates
    MgcVector3 kDiff = rkSegment.Origin() - rkBox.Center();
    MgcVector3 kOrigin(
        kDiff.Dot(rkBox.Axis(0)),
        kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2))
    );
    MgcVector3 kDirection(
        rkSegment.Direction().Dot(rkBox.Axis(0)),
        rkSegment.Direction().Dot(rkBox.Axis(1)),
        rkSegment.Direction().Dot(rkBox.Axis(2))
    );

    MgcReal fT0 = 0.0, fT1 = 1.0;
    bool bIntersects = MgcFindIntersection(kOrigin,kDirection,rkBox.Extents(),
        fT0,fT1);

    if ( bIntersects )
    {
        if ( fT0 > 0.0 )
        {
            if ( fT1 < 1.0 )
            {
                riQuantity = 2;
                akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
                akPoint[1] = rkSegment.Origin() + fT1*rkSegment.Direction();
            }
            else
            {
                riQuantity = 1;
                akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
            }
        }
        else  // fT0 == 0.0
        {
            if ( fT1 < 1.0 )
            {
                riQuantity = 1;
                akPoint[0] = rkSegment.Origin() + fT1*rkSegment.Direction();
            }
            else  // fT1 == 1.0
            {
                // segment entirely in box
                riQuantity = 0;
            }
        }
    }
    else
    {
        riQuantity = 0;
    }

    return bIntersects;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcRay3& rkRay, const MgcBox3& rkBox,double &t)
{
        //int riQuantity;
        //MgcVector3 akPoint[2];
    // convert ray to box coordinates
    MgcVector3 kDiff = rkRay.Origin() - rkBox.Center();
    MgcVector3 kOrigin(
        kDiff.Dot(rkBox.Axis(0)),
        kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2))
    );
    MgcVector3 kDirection(
        rkRay.Direction().Dot(rkBox.Axis(0)),
        rkRay.Direction().Dot(rkBox.Axis(1)),
        rkRay.Direction().Dot(rkBox.Axis(2))
    );

    MgcReal fT0 = 0.0, fT1 = MgcMath::INFINITY;
    bool bIntersects = MgcFindIntersection(kOrigin,kDirection,rkBox.Extents(),
        fT0,fT1);

    if (( fT0 > 0.0 ) && ( fT0 < fT1 ))
    {
                t = fT0;
        } else if ( fT1 > 0.0 )
        {
                t = fT1;
        }

    return bIntersects;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcLine3& rkLine, const MgcBox3& rkBox,
    int& riQuantity, MgcVector3 akPoint[2])
{
    // convert line to box coordinates
    MgcVector3 kDiff = rkLine.Origin() - rkBox.Center();
    MgcVector3 kOrigin(
        kDiff.Dot(rkBox.Axis(0)),
        kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2))
    );
    MgcVector3 kDirection(
        rkLine.Direction().Dot(rkBox.Axis(0)),
        rkLine.Direction().Dot(rkBox.Axis(1)),
        rkLine.Direction().Dot(rkBox.Axis(2))
    );

    MgcReal fT0 = -MgcMath::INFINITY, fT1 = MgcMath::INFINITY;
    bool bIntersects = MgcFindIntersection(kOrigin,kDirection,rkBox.Extents(),
        fT0,fT1);

    if ( bIntersects )
    {
        if ( fT0 != fT1 )
        {
            riQuantity = 2;
            akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
            akPoint[1] = rkLine.Origin() + fT1*rkLine.Direction();
        }
        else
        {
            riQuantity = 1;
            akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
        }
    }
    else
    {
        riQuantity = 0;
    }

    return bIntersects;
}
//----------------------------------------------------------------------------

---