📄 mgcdistlin3pgm3.cpp
字号:
// 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 "MgcDistLin3Lin3.h"
#include "MgcDistVec3Pgm3.h"#include "MgcDistLin3Pgm3.h"
#include "MgcRTLib.h"
static const MgcReal gs_fTolerance = 1e-05;
//----------------------------------------------------------------------------MgcReal MgcSqrDistance (const MgcLine3& rkLine,
const MgcParallelogram3& rkPgm, MgcReal* pfLinP, MgcReal* pfPgmP0,
MgcReal* pfPgmP1)
{
MgcVector3 kDiff = rkPgm.Origin() - rkLine.Origin();
MgcReal fA00 = rkLine.Direction().SquaredLength();
MgcReal fA01 = -rkLine.Direction().Dot(rkPgm.Edge0());
MgcReal fA02 = -rkLine.Direction().Dot(rkPgm.Edge1());
MgcReal fA11 = rkPgm.Edge0().SquaredLength();
MgcReal fA12 = rkPgm.Edge0().Dot(rkPgm.Edge1());
MgcReal fA22 = rkPgm.Edge1().SquaredLength();
MgcReal fB0 = -kDiff.Dot(rkLine.Direction());
MgcReal fB1 = kDiff.Dot(rkPgm.Edge0());
MgcReal fB2 = kDiff.Dot(rkPgm.Edge1());
MgcReal fCof00 = fA11*fA22-fA12*fA12;
MgcReal fCof01 = fA02*fA12-fA01*fA22;
MgcReal fCof02 = fA01*fA12-fA02*fA11;
MgcReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
MgcSegment3 kSegPgm;
MgcVector3 kPt;
MgcReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
if ( MgcMath::Abs(fDet) >= gs_fTolerance )
{
MgcReal fCof11 = fA00*fA22-fA02*fA02;
MgcReal fCof12 = fA02*fA01-fA00*fA12;
MgcReal fCof22 = fA00*fA11-fA01*fA01;
MgcReal fInvDet = 1.0/fDet;
MgcReal fRhs0 = -fB0*fInvDet;
MgcReal fRhs1 = -fB1*fInvDet;
MgcReal 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 < 0.0 )
{
if ( fT < 0.0 ) // region 6
{
// min on face s=0 or t=0
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 0.0;
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
fT0 = 0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= 1.0 ) // region 5
{
// min on face s=0
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 0.0;
}
else // region 4
{
// min on face s=0 or t=1
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 0.0;
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
fT0 = 1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
}
else if ( fS <= 1.0 )
{
if ( fT < 0.0 ) // region 7
{
// min on face t=0
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fS);
fT = 0.0;
}
else if ( fT <= 1.0 ) // region 0
{
// line intersects parallelogram
fSqrDist = 0.0;
}
else // region 3
{
// min on face t=1
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fS);
fT = 1.0;
}
}
else
{
if ( fT < 0.0 ) // region 8
{
// min on face s=1 or t=0
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 1.0;
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
fT0 = 0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= 1.0 ) // region 1
{
// min on face s=1
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 1.0;
}
else // region 2
{
// min on face s=1 or t=1
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
fS = 1.0;
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
fT0 = 1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
}
}
}
}
else
{
// line and parallelogram are parallel
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist = MgcSqrDistance(rkLine,kSegPgm,&fR,&fS);
fT = 0.0;
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fT0);
fS0 = 0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fS0);
fT0 = 1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fT0);
fS0 = 1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
if ( pfLinP )
*pfLinP = fR;
if ( pfPgmP0 )
*pfPgmP0 = fS;
if ( pfPgmP1 )
*pfPgmP1 = fT;
return MgcMath::Abs(fSqrDist);
}
//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcRay3& rkRay, const MgcParallelogram3& rkPgm,
MgcReal* pfRayP, MgcReal* pfPgmP0, MgcReal* pfPgmP1)
{
MgcVector3 kDiff = rkPgm.Origin() - rkRay.Origin();
MgcReal fA00 = rkRay.Direction().SquaredLength();
MgcReal fA01 = -rkRay.Direction().Dot(rkPgm.Edge0());
MgcReal fA02 = -rkRay.Direction().Dot(rkPgm.Edge1());
MgcReal fA11 = rkPgm.Edge0().SquaredLength();
MgcReal fA12 = rkPgm.Edge0().Dot(rkPgm.Edge1());
MgcReal fA22 = rkPgm.Edge1().SquaredLength();
MgcReal fB0 = -kDiff.Dot(rkRay.Direction());
MgcReal fB1 = kDiff.Dot(rkPgm.Edge0());
MgcReal fB2 = kDiff.Dot(rkPgm.Edge1());
MgcReal fCof00 = fA11*fA22-fA12*fA12;
MgcReal fCof01 = fA02*fA12-fA01*fA22;
MgcReal fCof02 = fA01*fA12-fA02*fA11;
MgcReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
MgcSegment3 kSegPgm;
MgcVector3 kPt;
MgcReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
if ( MgcMath::Abs(fDet) >= gs_fTolerance )
{
MgcReal fCof11 = fA00*fA22-fA02*fA02;
MgcReal fCof12 = fA02*fA01-fA00*fA12;
MgcReal fCof22 = fA00*fA11-fA01*fA01;
MgcReal fInvDet = 1.0/fDet;
MgcReal fRhs0 = -fB0*fInvDet;
MgcReal fRhs1 = -fB1*fInvDet;
MgcReal 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 <= 0.0 )
{
if ( fS < 0.0 )
{
if ( fT < 0.0 ) // region 6m
{
// min on face s=0 or t=0 or r=0
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = 0.0;
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkRay,kSegPgm,&fR0,&fS0);
fT0 = 0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
fSqrDist0 = MgcSqrDistance(rkRay.Origin(),rkPgm,&fS0,
&fT0);
fR0 = 0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
}
}
else if ( fT <= 1.0 ) // region 5m
{
// min on face s=0 or r=0
kSegPgm.Origin() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = 0.0;
fSqrDist0 = MgcSqrDistance(rkRay.Origin(),rkPgm,&fS0,
&fT0);
fR0 = 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() = rkPgm.Origin();
kSegPgm.Direction() = rkPgm.Edge1();
fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
fS = 0.0;
kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
kSegPgm.Direction() = rkPgm.Edge0();
fSqrDist0 = MgcSqrDistance(rkRay,kSegPgm,&fR0,&fS0);
fT0 = 1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fR = fR0;
fS = fS0;
fT = fT0;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -