You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
701 lines
20 KiB
701 lines
20 KiB
/*==LICENSE==* |
|
|
|
CyanWorlds.com Engine - MMOG client, server and tools |
|
Copyright (C) 2011 Cyan Worlds, Inc. |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
Additional permissions under GNU GPL version 3 section 7 |
|
|
|
If you modify this Program, or any covered work, by linking or |
|
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK, |
|
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent |
|
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK |
|
(or a modified version of those libraries), |
|
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA, |
|
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG |
|
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the |
|
licensors of this Program grant you additional |
|
permission to convey the resulting work. Corresponding Source for a |
|
non-source form of such a combination shall include the source code for |
|
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered |
|
work. |
|
|
|
You can contact Cyan Worlds, Inc. by email legal@cyan.com |
|
or by snail mail at: |
|
Cyan Worlds, Inc. |
|
14617 N Newport Hwy |
|
Mead, WA 99021 |
|
|
|
*==LICENSE==*/ |
|
|
|
#include "HeadSpin.h" |
|
#include "hsGeometry3.h" |
|
#include "plClosest.h" |
|
#include "hsFastMath.h" |
|
|
|
|
|
static const float kRealSmall = 1.e-5f; |
|
|
|
// Find the closest point on a line (or segment) to a point. |
|
uint32_t plClosest::PointOnLine(const hsPoint3& p0, |
|
const hsPoint3& p1, const hsVector3& v1, |
|
hsPoint3& cp, |
|
uint32_t clamp) |
|
{ |
|
float invV1Sq = v1.MagnitudeSquared(); |
|
// v1 is also zero length. The two input points are the only options for output. |
|
if( invV1Sq < kRealSmall ) |
|
{ |
|
cp = p1; |
|
return kClamp; |
|
} |
|
float t = v1.InnerProduct(p0 - p1) / invV1Sq; |
|
cp = p1; |
|
// clamp to the ends of segment v1. |
|
if( (clamp & kClampLower1) && (t < 0) ) |
|
{ |
|
return kClampLower1; |
|
} |
|
if( (clamp & kClampUpper1) && (t > 1.f) ) |
|
{ |
|
cp += v1; |
|
return kClampUpper1; |
|
} |
|
|
|
cp += v1 * t; |
|
return 0; |
|
} |
|
|
|
// Find closest points to each other from two lines (or segments). |
|
uint32_t plClosest::PointsOnLines(const hsPoint3& p0, const hsVector3& v0, |
|
const hsPoint3& p1, const hsVector3& v1, |
|
hsPoint3& cp0, hsPoint3& cp1, |
|
uint32_t clamp) |
|
{ |
|
float invV0Sq = v0.MagnitudeSquared(); |
|
// First handle degenerate cases. |
|
// v0 is zero length. Resolves to finding closest point on p1+v1 to p0 |
|
if( invV0Sq < kRealSmall ) |
|
{ |
|
cp0 = p0; |
|
return kClamp0 | PointOnLine(p0, p1, v1, cp1, clamp); |
|
} |
|
invV0Sq = 1.f / invV0Sq; |
|
|
|
// The real thing here, two non-zero length segments. (v1 can |
|
// be zero length, it doesn't affect the math like |v0|=0 does, |
|
// so we don't even bother to check. Only means maybe doing extra |
|
// work, since we're using segment-segment math when all we really |
|
// need is point-segment.) |
|
|
|
// The parameterized points for along each of the segments are |
|
// P(t0) = p0 + v0*t0 |
|
// P(t1) = p1 + v1*t1 |
|
// |
|
// The closest point on p0+v0 to P(t1) is: |
|
// cp0 = p0 + ((P(t1) - p0) dot v0) * v0 / ||v0|| ||x|| is mag squared here |
|
// cp0 = p0 + v0*t0 => t0 = ((P(t1) - p0) dot v0 ) / ||v0|| |
|
// t0 = ((p1 + v1*t1 - p0) dot v0) / ||v0|| |
|
// |
|
// The distance squared from P(t1) to cp0 is: |
|
// (cp0 - P(t1)) dot (cp0 - P(t1)) |
|
// |
|
// This expands out to: |
|
// |
|
// CV0 dot CV0 + 2 CV0 dot DV0 * t1 + (DV0 dot DV0) * t1^2 |
|
// |
|
// where |
|
// |
|
// CV0 = p0 - p1 + ((p1 - p0) dot v0) / ||v0||) * v0 == vector from p1 to closest point on p0+v0 |
|
// and |
|
// DV0 = ((v1 dot v0) / ||v0||) * v0 - v1 == ortho divergence vector of v1 from v0 negated. |
|
// |
|
// Taking the first derivative to find the local minimum of the function gives |
|
// |
|
// t1 = - (CV0 dot DV0) / (DV0 dot DV0) |
|
// and |
|
// t0 = ((p1 - v1 * t1 - p0) dot v0) / ||v0|| |
|
// |
|
// which seems kind of obvious in retrospect. |
|
|
|
hsVector3 p0subp1(&p0, &p1); |
|
|
|
hsVector3 CV0 = p0subp1; |
|
CV0 += v0 * p0subp1.InnerProduct(v0) * -invV0Sq; |
|
|
|
hsVector3 DV0 = v0 * (v1.InnerProduct(v0) * invV0Sq) - v1; |
|
|
|
// Check for the vectors v0 and v1 being parallel, in which case |
|
// following the lines won't get us to any closer point. |
|
float DV0dotDV0 = DV0.InnerProduct(DV0); |
|
if( DV0dotDV0 < kRealSmall ) |
|
{ |
|
// If neither is clamped, return any two corresponding points. |
|
// If one is clamped, return closest points in its clamp range. |
|
// If both are clamped, well, both are clamped. The distance between |
|
// points will no longer be the distance between lines. |
|
// In any case, the distance between the points should be correct. |
|
uint32_t clamp1 = PointOnLine(p0, p1, v1, cp1, clamp); |
|
uint32_t clamp0 = PointOnLine(cp1, p0, v0, cp0, clamp >> 1); |
|
return clamp1 | (clamp0 << 1); |
|
} |
|
|
|
uint32_t retVal = 0; |
|
|
|
float t1 = - (CV0.InnerProduct(DV0)) / DV0dotDV0; |
|
if( (clamp & kClampLower1) && (t1 <= 0) ) |
|
{ |
|
t1 = 0; |
|
retVal |= kClampLower1; |
|
} |
|
else if( (clamp & kClampUpper1) && (t1 >= 1.f) ) |
|
{ |
|
t1 = 1.f; |
|
retVal |= kClampUpper1; |
|
} |
|
|
|
float t0 = v0.InnerProduct(p0subp1 - v1 * t1) * -invV0Sq; |
|
cp0 = p0; |
|
if( (clamp & kClampUpper0) && (t0 >= 1.f) ) |
|
{ |
|
cp0 += v0; |
|
retVal |= kClampUpper0; |
|
} |
|
else if( !(clamp & kClampLower0) || (t0 > 0) ) |
|
{ |
|
cp0 += v0 * t0; |
|
} |
|
else |
|
{ |
|
retVal |= kClampLower0; |
|
} |
|
|
|
// If we clamped t0, we need to recalc t1 because the original |
|
// calculation of t1 was based on an infinite p0+v0. |
|
if( retVal & kClamp0 ) |
|
{ |
|
t1 = v1.InnerProduct(cp0 - p1) / v1.MagnitudeSquared(); |
|
retVal &= ~kClamp1; |
|
if( (clamp & kClampLower1) && (t1 <= 0) ) |
|
{ |
|
t1 = 0; |
|
retVal |= kClampLower1; |
|
} |
|
else if( (clamp & kClampUpper1) && (t1 >= 1.f) ) |
|
{ |
|
t1 = 1.f; |
|
retVal |= kClampUpper1; |
|
} |
|
} |
|
|
|
cp1 = p1; |
|
cp1 += v1 * t1; |
|
|
|
return retVal;; |
|
} |
|
|
|
bool plClosest::PointOnSphere(const hsPoint3& p0, |
|
const hsPoint3& center, float rad, |
|
hsPoint3& cp) |
|
{ |
|
hsVector3 del(&p0, ¢er); |
|
float dist = hsFastMath::InvSqrtAppr(del.MagnitudeSquared()); |
|
dist *= rad; |
|
del *= dist; |
|
cp = center; |
|
cp += del; |
|
return dist <= 1.f; |
|
} |
|
|
|
bool plClosest::PointOnBox(const hsPoint3& p0, |
|
const hsPoint3& corner, |
|
const hsVector3& axis0, |
|
const hsVector3& axis1, |
|
const hsVector3& axis2, |
|
hsPoint3& cp) |
|
{ |
|
uint32_t clamps = 0; |
|
hsPoint3 currPt = corner; |
|
clamps |= PointOnLine(p0, currPt, axis0, cp, kClamp); |
|
currPt = cp; |
|
clamps |= PointOnLine(p0, currPt, axis1, cp, kClamp); |
|
currPt = cp; |
|
clamps |= PointOnLine(p0, currPt, axis2, cp, kClamp); |
|
|
|
return !clamps; |
|
} |
|
|
|
bool plClosest::PointOnSphere(const hsPoint3& p0, const hsVector3& v0, |
|
const hsPoint3& center, float rad, |
|
hsPoint3& cp, |
|
uint32_t clamp) |
|
{ |
|
// Does the line hit the sphere? If it does, we return the entry point in cp, |
|
// otherwise we find the closest point on the sphere to the line. |
|
/* |
|
((p0 + v0*t) - center)^2 = rad |
|
v0*v0 * t*t + 2 * v0*t * (p0-c) + (p0-c)^2 - rad = 0 |
|
|
|
t = (-2 * v0*(p0-c) +- sqrt(4 * (v0*(p0-c))^2 - 4 * v0*v0 * ((p0-c)^2 - rad) / 2 * v0 * v0 |
|
|
|
t = (-v0*(p0-c) +- sqrt((v0*(p0-c))^2 - v0*v0 * ((p0-c)^2 - rad) / v0 * v0 |
|
|
|
So, line hits the sphere if |
|
(v0*(p0-c))^2 > v0*v0 * ((p0-c)^2 - rad) |
|
|
|
If clamped, need additional checks on t before returning true |
|
|
|
If line doesn't hit the sphere, we find the closest point on the line |
|
to the center of the sphere, and return the intersection of the segment |
|
connecting that point and the center with the sphere. |
|
*/ |
|
float termA = v0.InnerProduct(v0); |
|
if( termA < kRealSmall ) |
|
{ |
|
return PointOnSphere(p0, center, rad, cp); |
|
} |
|
hsVector3 p0Subc(&p0, ¢er); |
|
float termB = v0.InnerProduct(p0Subc); |
|
float termC = p0Subc.InnerProduct(p0Subc) - rad; |
|
float disc = termB * termB - 4 * termA * termC; |
|
if( disc >= 0 ) |
|
{ |
|
disc = sqrt(disc); |
|
float t = (-termB - disc) / (2.f * termA); |
|
if( (t < 0) && (clamp & kClampLower0) ) |
|
{ |
|
float tOut = (-termB + disc) / (2.f * termA); |
|
if( tOut < 0 ) |
|
{ |
|
// Both isects are before beginning of clamped line. |
|
cp = p0; |
|
cp += v0 * tOut; |
|
return false; |
|
} |
|
if( (tOut > 1.f) && (clamp & kClampUpper0) ) |
|
{ |
|
// The segment is entirely within the sphere. Take the closer end. |
|
if( -t < tOut - 1.f ) |
|
{ |
|
cp = p0; |
|
cp += v0 * t; |
|
} |
|
else |
|
{ |
|
cp = p0; |
|
cp += v0 * tOut; |
|
} |
|
return true; |
|
} |
|
// We pierce the sphere from inside. |
|
cp = p0; |
|
cp += v0 * tOut; |
|
return true; |
|
} |
|
cp = p0; |
|
cp += v0 * t; |
|
if( (t > 1.f) && (clamp & kClampUpper0) ) |
|
{ |
|
return false; |
|
} |
|
return true; |
|
} |
|
|
|
// Okay, missed the sphere, find closest point. |
|
hsPoint3 lp; |
|
PointOnLine(center, p0, v0, lp, clamp); |
|
PointOnSphere(lp, center, rad, cp); |
|
|
|
return false; |
|
} |
|
|
|
bool plClosest::PointOnBox(const hsPoint3& p0, const hsVector3& v0, |
|
const hsPoint3& corner, |
|
const hsVector3& axis0, |
|
const hsVector3& axis1, |
|
const hsVector3& axis2, |
|
hsPoint3& cp, |
|
uint32_t clamp) |
|
{ |
|
uint32_t clampRes = 0; |
|
|
|
hsPoint3 cp0, cp1; |
|
hsPoint3 currPt = corner; |
|
|
|
clampRes |= PointsOnLines(p0, v0, currPt, axis0, cp0, cp1, clamp); |
|
currPt = cp1; |
|
|
|
clampRes |= PointsOnLines(p0, v0, currPt, axis1, cp0, cp1, clamp); |
|
currPt = cp1; |
|
|
|
clampRes |= PointsOnLines(p0, v0, currPt, axis2, cp0, cp1, clamp); |
|
currPt = cp1; |
|
|
|
return !clampRes; |
|
} |
|
|
|
bool plClosest::PointOnPlane(const hsPoint3& p0, |
|
const hsPoint3& pPln, const hsVector3& n, |
|
hsPoint3& cp) |
|
{ |
|
/* |
|
p' = p - ((p-pPln)*n)/|n| * n/|n| |
|
p' = p + ((pPln-p)*n) * n / |n|^2 |
|
*/ |
|
float invNLen = hsFastMath::InvSqrt(n.MagnitudeSquared()); |
|
|
|
float nDotp = n.InnerProduct(pPln - p0); |
|
cp = p0 + n * (nDotp * invNLen); |
|
|
|
return nDotp >= 0; |
|
} |
|
|
|
bool plClosest::PointOnPlane(const hsPoint3& p0, const hsVector3& v0, |
|
const hsPoint3& pPln, const hsVector3& n, |
|
hsPoint3& cp, |
|
uint32_t clamp) |
|
{ |
|
/* |
|
p0 + v0*t is on plane, i.e. |
|
(p0 + v0*t) * n = pPln * n |
|
|
|
p0 * n + v0 * n * t = pPln * n |
|
v0 * n * t = (pPln - p0) * n |
|
t = (pPln - p0) * n / (v0 * n) |
|
|
|
Then clamp appropriately, garnish, and serve with wild rice. |
|
*/ |
|
bool retVal = true; |
|
float pDotn = n.InnerProduct(pPln - p0); |
|
float v0Dotn = n.InnerProduct(v0); |
|
if( (v0Dotn < -kRealSmall) || (v0Dotn > kRealSmall) ) |
|
{ |
|
float t = pDotn / v0Dotn; |
|
|
|
if( (clamp & kClampLower) && (t < 0) ) |
|
{ |
|
t = 0; |
|
retVal = false; |
|
} |
|
else if( (clamp & kClampUpper) && (t > 1.f) ) |
|
{ |
|
t = 1.f; |
|
retVal = false; |
|
} |
|
cp = p0; |
|
cp += v0 * t; |
|
|
|
} |
|
else |
|
{ |
|
cp = p0 + v0 * 0.5f; |
|
retVal = (pDotn > -kRealSmall) && (pDotn < kRealSmall); |
|
} |
|
|
|
return retVal; |
|
} |
|
|
|
bool plClosest::PointBetweenBoxes(const hsPoint3& aCorner, |
|
const hsVector3& aAxis0, |
|
const hsVector3& aAxis1, |
|
const hsVector3& aAxis2, |
|
const hsPoint3& bCorner, |
|
const hsVector3& bAxis0, |
|
const hsVector3& bAxis1, |
|
const hsVector3& bAxis2, |
|
hsPoint3& cp0, hsPoint3& cp1) |
|
{ |
|
const hsVector3* aAxes[3] = { &aAxis0, &aAxis1, &aAxis2 }; |
|
const hsVector3* bAxes[3] = { &bAxis0, &bAxis1, &bAxis2 }; |
|
|
|
return PointBetweenBoxes(aCorner, aAxes, bCorner, bAxes, cp0, cp1); |
|
} |
|
|
|
#if 0 // TRASH THIS |
|
bool plClosest::PointBetweenBoxes(const hsPoint3& aCorner, |
|
const hsVector3* aAxes[3], |
|
const hsPoint3& bCorner, |
|
const hsVector3* bAxes[3], |
|
hsPoint3& cp0, hsPoint3& cp1) |
|
{ |
|
hsPoint3 aCurrPt = aCorner; |
|
hsPoint3 bCurrPt = bCorner; |
|
|
|
hsPoint3 bStartPt[3]; |
|
bStartPt[0] = bStartPt[1] = bStartPt[2] = bCorner; |
|
|
|
bool retVal = true; |
|
int i, j; |
|
for( i = 0; i < 3; i++ ) |
|
{ |
|
hsPoint3 aBestPt; |
|
hsPoint3 bBestPt; |
|
|
|
float minDistSq = 1.e33f; |
|
for( j = 0; j < 3; j++ ) |
|
{ |
|
hsPoint3 aNextPt, bNextPt; |
|
PointsOnLines(aCurrPt, *aAxes[i], |
|
bStartPt[j], *bAxes[j], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
float distSq = hsVector3(&aNextPt, &bNextPt).MagnitudeSquared(); |
|
if( distSq < minDistSq ) |
|
{ |
|
aBestPt = aNextPt; |
|
bBestPt = bNextPt; |
|
|
|
if( distSq < kRealSmall ) |
|
retVal = true; |
|
|
|
minDistSq = distSq; |
|
} |
|
hsVector3 bMove(&bNextPt, &bStartPt[j]); |
|
int k; |
|
for( k = 0; k < 3; k++ ) |
|
{ |
|
if( k != j ) |
|
bStartPt[k] += bMove; |
|
} |
|
} |
|
aCurrPt = aBestPt; |
|
bCurrPt = bBestPt; |
|
} |
|
cp0 = aCurrPt; |
|
cp1 = bCurrPt; |
|
|
|
return retVal; |
|
} |
|
#elif 0 // TRASH THIS |
|
|
|
bool plClosest::PointBetweenBoxes(const hsPoint3& aCorner, |
|
const hsVector3* aAxes[3], |
|
const hsPoint3& bCorner, |
|
const hsVector3* bAxes[3], |
|
hsPoint3& cp0, hsPoint3& cp1) |
|
{ |
|
/* |
|
Six combinations to try to go through every possible |
|
combination of axes from A and B |
|
|
|
00 00 01 01 02 02 |
|
11 12 12 10 10 11 |
|
22 21 20 22 21 20 |
|
*/ |
|
|
|
int bIdx0 = 0; |
|
int bIdx1 = 1; |
|
int bIdx2 = 2; |
|
|
|
hsPoint3 aBestPt, bBestPt; |
|
float minDistSq = 1.e33f; |
|
|
|
bool retVal = false; |
|
|
|
int i; |
|
for( i = 0; i < 6; i++ ) |
|
{ |
|
hsPoint3 aCurrPt = aCorner; |
|
hsPoint3 bCurrPt = bCorner; |
|
|
|
hsPoint3 aNextPt, bNextPt; |
|
PointsOnLines(aCurrPt, *aAxes[0], |
|
bCurrPt, *bAxes[bIdx0], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
aCurrPt = aNextPt; |
|
bCurrPt = bNextPt; |
|
|
|
PointsOnLines(aCurrPt, *aAxes[1], |
|
bCurrPt, *bAxes[bIdx1], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
aCurrPt = aNextPt; |
|
bCurrPt = bNextPt; |
|
|
|
PointsOnLines(aCurrPt, *aAxes[2], |
|
bCurrPt, *bAxes[bIdx2], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
|
|
float distSq = hsVector3(&aNextPt, &bNextPt).MagnitudeSquared(); |
|
if( distSq < minDistSq ) |
|
{ |
|
aBestPt = aNextPt; |
|
bBestPt = bNextPt; |
|
|
|
if( distSq < kRealSmall ) |
|
retVal = true; |
|
|
|
minDistSq = distSq; |
|
} |
|
|
|
if( i & 0x1 ) |
|
{ |
|
bIdx0++; |
|
bIdx1 = bIdx0 < 2 ? bIdx0+1 : 0; |
|
bIdx2 = bIdx1 < 2 ? bIdx1+1 : 0; |
|
} |
|
else |
|
{ |
|
int t = bIdx1; |
|
bIdx1 = bIdx2; |
|
bIdx2 = t; |
|
} |
|
} |
|
cp0 = aBestPt; |
|
cp1 = bBestPt; |
|
|
|
return retVal; |
|
} |
|
|
|
#else // TRASH THIS |
|
|
|
bool plClosest::PointBetweenBoxes(const hsPoint3& aCorner, |
|
const hsVector3* aAxes[3], |
|
const hsPoint3& bCorner, |
|
const hsVector3* bAxes[3], |
|
hsPoint3& cp0, hsPoint3& cp1) |
|
{ |
|
/* |
|
Six combinations to try to go through every possible |
|
combination of axes from A and B |
|
|
|
00 00 01 01 02 02 |
|
11 12 12 10 10 11 |
|
22 21 20 22 21 20 |
|
*/ |
|
|
|
struct trial { |
|
int aIdx[3]; |
|
int bIdx[3]; |
|
} trials[36]; |
|
|
|
|
|
int tNext = 0; |
|
int k,l; |
|
for( k = 0; k < 3; k++ ) |
|
{ |
|
for( l = 0; l < 3; l++ ) |
|
{ |
|
int kPlus = k < 2 ? k+1 : 0; |
|
int kPlusPlus = kPlus < 2 ? kPlus+1 : 0; |
|
|
|
int lPlus = l < 2 ? l+1 : 0; |
|
int lPlusPlus = lPlus < 2 ? lPlus+1 : 0; |
|
|
|
trials[tNext].aIdx[0] = k; |
|
trials[tNext].bIdx[0] = l; |
|
|
|
trials[tNext].aIdx[1] = kPlus; |
|
trials[tNext].bIdx[1] = lPlus; |
|
|
|
trials[tNext].aIdx[2] = kPlusPlus; |
|
trials[tNext].bIdx[2] = lPlusPlus; |
|
|
|
tNext++; |
|
|
|
trials[tNext].aIdx[0] = k; |
|
trials[tNext].bIdx[0] = l; |
|
|
|
trials[tNext].aIdx[1] = kPlusPlus; |
|
trials[tNext].bIdx[1] = lPlusPlus; |
|
|
|
trials[tNext].aIdx[2] = kPlus; |
|
trials[tNext].bIdx[2] = lPlus; |
|
|
|
tNext++; |
|
|
|
trials[tNext].aIdx[0] = k; |
|
trials[tNext].bIdx[0] = l; |
|
|
|
trials[tNext].aIdx[1] = kPlus; |
|
trials[tNext].bIdx[1] = lPlusPlus; |
|
|
|
trials[tNext].aIdx[2] = kPlusPlus; |
|
trials[tNext].bIdx[2] = lPlus; |
|
|
|
tNext++; |
|
|
|
trials[tNext].aIdx[0] = k; |
|
trials[tNext].bIdx[0] = l; |
|
|
|
trials[tNext].aIdx[1] = kPlusPlus; |
|
trials[tNext].bIdx[1] = lPlus; |
|
|
|
trials[tNext].aIdx[2] = kPlus; |
|
trials[tNext].bIdx[2] = lPlusPlus; |
|
|
|
tNext++; |
|
} |
|
} |
|
|
|
hsPoint3 aBestPt, bBestPt; |
|
float minDistSq = 1.e33f; |
|
|
|
bool retVal = false; |
|
|
|
int i; |
|
for( i = 0; i < 36; i++ ) |
|
{ |
|
hsPoint3 aCurrPt = aCorner; |
|
hsPoint3 bCurrPt = bCorner; |
|
|
|
hsPoint3 aNextPt, bNextPt; |
|
PointsOnLines(aCurrPt, *aAxes[trials[i].aIdx[0]], |
|
bCurrPt, *bAxes[trials[i].bIdx[0]], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
aCurrPt = aNextPt; |
|
bCurrPt = bNextPt; |
|
|
|
PointsOnLines(aCurrPt, *aAxes[trials[i].aIdx[1]], |
|
bCurrPt, *bAxes[trials[i].bIdx[1]], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
aCurrPt = aNextPt; |
|
bCurrPt = bNextPt; |
|
|
|
PointsOnLines(aCurrPt, *aAxes[trials[i].aIdx[2]], |
|
bCurrPt, *bAxes[trials[i].bIdx[2]], |
|
aNextPt, bNextPt, |
|
plClosest::kClamp); |
|
|
|
|
|
float distSq = hsVector3(&aNextPt, &bNextPt).MagnitudeSquared(); |
|
if( distSq < minDistSq ) |
|
{ |
|
aBestPt = aNextPt; |
|
bBestPt = bNextPt; |
|
|
|
if( distSq < kRealSmall ) |
|
retVal = true; |
|
|
|
minDistSq = distSq; |
|
} |
|
|
|
} |
|
cp0 = aBestPt; |
|
cp1 = bBestPt; |
|
|
|
return retVal; |
|
} |
|
#endif // TRASH THIS
|