📄 wmldistlin3rct3.cpp
字号:
// Magic Software, Inc.
// http://www.magic-software.com
// http://www.wild-magic.com
// Copyright (c) 2003. All Rights Reserved
//
// The Wild Magic Library (WML) source code is supplied under the terms of
// the license agreement http://www.magic-software.com/License/WildMagic.pdf
// and may not be copied or disclosed except in accordance with the terms of
// that agreement.
#include "WmlDistLin3Lin3.h"
#include "WmlDistLin3Rct3.h"
#include "WmlDistVec3Rct3.h"
using namespace Wml;
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Line3<Real>& rkLine,
const Rectangle3<Real>& rkRct, Real* pfLinP, Real* pfRctP0, Real* pfRctP1)
{
Vector3<Real> kDiff = rkRct.Origin() - rkLine.Origin();
Real fA00 = rkLine.Direction().SquaredLength();
Real fA01 = -rkLine.Direction().Dot(rkRct.Edge0());
Real fA02 = -rkLine.Direction().Dot(rkRct.Edge1());
Real fA11 = rkRct.Edge0().SquaredLength();
Real fA22 = rkRct.Edge1().SquaredLength();
Real fB0 = -kDiff.Dot(rkLine.Direction());
Real fB1 = kDiff.Dot(rkRct.Edge0());
Real fB2 = kDiff.Dot(rkRct.Edge1());
Real fCof00 = fA11*fA22;
Real fCof01 = -fA01*fA22;
Real fCof02 = -fA02*fA11;
Real fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
Segment3<Real> kSegPgm;
Real fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
if ( Math<Real>::FAbs(fDet) >= Math<Real>::EPSILON )
{
Real fCof11 = fA00*fA22-fA02*fA02;
Real fCof12 = fA02*fA01;
Real fCof22 = fA00*fA11-fA01*fA01;
Real fInvDet = ((Real)1.0)/fDet;
Real fRhs0 = -fB0*fInvDet;
Real fRhs1 = -fB1*fInvDet;
Real fRhs2 = -fB2*fInvDet;
fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
if ( fS < (Real)0.0 )
{
if ( fT < (Real)0.0 ) // region 6
{
// min on face s=0 or t=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)0.0;
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS0);
fT0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= (Real)1.0 ) // region 5
{
// min on face s=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)0.0;
}
else // region 4
{
// min on face s=0 or t=1
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)0.0;
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
}
else if ( fS <= (Real)1.0 )
{
if ( fT < (Real)0.0 ) // region 7
{
// min on face t=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS);
fT = (Real)0.0;
}
else if ( fT <= (Real)1.0 ) // region 0
{
// line intersects rectangle
fSqrDist = (Real)0.0;
}
else // region 3
{
// min on face t=1
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS);
fT = (Real)1.0;
}
}
else
{
if ( fT < (Real)0.0 ) // region 8
{
// min on face s=1 or t=0
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)1.0;
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS0);
fT0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= (Real)1.0 ) // region 1
{
// min on face s=1
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)1.0;
}
else // region 2
{
// min on face s=1 or t=1
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fT);
fS = (Real)1.0;
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance<Real>(rkLine,kSegPgm,NULL,&fS0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
}
}
else
{
// line and rectangle are parallel
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist = SqrDistance(rkLine,kSegPgm,&fR,&fS);
fT = (Real)0.0;
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist0 = SqrDistance(rkLine,kSegPgm,&fR0,&fT0);
fS0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance(rkLine,kSegPgm,&fR0,&fS0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist0 = SqrDistance(rkLine,kSegPgm,&fR0,&fT0);
fS0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
if ( pfLinP )
*pfLinP = fR;
if ( pfRctP0 )
*pfRctP0 = fS;
if ( pfRctP1 )
*pfRctP1 = fT;
return Math<Real>::FAbs(fSqrDist);
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Ray3<Real>& rkRay,
const Rectangle3<Real>& rkRct, Real* pfRayP, Real* pfRctP0, Real* pfRctP1)
{
Vector3<Real> kDiff = rkRct.Origin() - rkRay.Origin();
Real fA00 = rkRay.Direction().SquaredLength();
Real fA01 = -rkRay.Direction().Dot(rkRct.Edge0());
Real fA02 = -rkRay.Direction().Dot(rkRct.Edge1());
Real fA11 = rkRct.Edge0().SquaredLength();
Real fA22 = rkRct.Edge1().SquaredLength();
Real fB0 = -kDiff.Dot(rkRay.Direction());
Real fB1 = kDiff.Dot(rkRct.Edge0());
Real fB2 = kDiff.Dot(rkRct.Edge1());
Real fCof00 = fA11*fA22;
Real fCof01 = -fA01*fA22;
Real fCof02 = -fA02*fA11;
Real fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
Segment3<Real> kSegPgm;
Real fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
if ( Math<Real>::FAbs(fDet) >= Math<Real>::EPSILON )
{
Real fCof11 = fA00*fA22-fA02*fA02;
Real fCof12 = fA02*fA01;
Real fCof22 = fA00*fA11-fA01*fA01;
Real fInvDet = ((Real)1.0)/fDet;
Real fRhs0 = -fB0*fInvDet;
Real fRhs1 = -fB1*fInvDet;
Real fRhs2 = -fB2*fInvDet;
fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
if ( fR <= (Real)0.0 )
{
if ( fS < (Real)0.0 )
{
if ( fT < (Real)0.0 ) // region 6m
{
// min on face s=0 or t=0 or r=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = (Real)0.0;
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance(rkRay,kSegPgm,&fR0,&fS0);
fT0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
fSqrDist0 = SqrDistance(rkRay.Origin(),rkRct,&fS0,&fT0);
fR0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= (Real)1.0 ) // region 5m
{
// min on face s=0 or r=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = (Real)0.0;
fSqrDist0 = SqrDistance(rkRay.Origin(),rkRct,&fS0,&fT0);
fR0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
else // region 4m
{
// min on face s=0 or t=1 or r=0
kSegPgm.Origin() = rkRct.Origin();
kSegPgm.Direction() = rkRct.Edge1();
fSqrDist = SqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = (Real)0.0;
kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
kSegPgm.Direction() = rkRct.Edge0();
fSqrDist0 = SqrDistance(rkRay,kSegPgm,&fR0,&fS0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
fSqrDist0 = SqrDistance(rkRay.Origin(),rkRct,&fS0,&fT0);
fR0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -