|
|
|
|
/*==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/>.
|
|
|
|
|
|
|
|
|
|
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 "hsTypes.h"
|
|
|
|
|
#include "hsGeometry3.h"
|
|
|
|
|
#include "plClosest.h"
|
|
|
|
|
#include "hsFastMath.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static const hsScalar kRealSmall = 1.e-5f;
|
|
|
|
|
|
|
|
|
|
// Find the closest point on a line (or segment) to a point.
|
|
|
|
|
UInt32 plClosest::PointOnLine(const hsPoint3& p0,
|
|
|
|
|
const hsPoint3& p1, const hsVector3& v1,
|
|
|
|
|
hsPoint3& cp,
|
|
|
|
|
UInt32 clamp)
|
|
|
|
|
{
|
|
|
|
|
hsScalar 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;
|
|
|
|
|
}
|
|
|
|
|
hsScalar 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 plClosest::PointsOnLines(const hsPoint3& p0, const hsVector3& v0,
|
|
|
|
|
const hsPoint3& p1, const hsVector3& v1,
|
|
|
|
|
hsPoint3& cp0, hsPoint3& cp1,
|
|
|
|
|
UInt32 clamp)
|
|
|
|
|
{
|
|
|
|
|
hsScalar 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.
|
|
|
|
|
hsScalar 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 clamp1 = PointOnLine(p0, p1, v1, cp1, clamp);
|
|
|
|
|
UInt32 clamp0 = PointOnLine(cp1, p0, v0, cp0, clamp >> 1);
|
|
|
|
|
return clamp1 | (clamp0 << 1);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
UInt32 retVal = 0;
|
|
|
|
|
|
|
|
|
|
hsScalar 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsScalar 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;;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool plClosest::PointOnSphere(const hsPoint3& p0,
|
|
|
|
|
const hsPoint3& center, hsScalar rad,
|
|
|
|
|
hsPoint3& cp)
|
|
|
|
|
{
|
|
|
|
|
hsVector3 del(&p0, ¢er);
|
|
|
|
|
hsScalar dist = hsFastMath::InvSqrtAppr(del.MagnitudeSquared());
|
|
|
|
|
dist *= rad;
|
|
|
|
|
del *= dist;
|
|
|
|
|
cp = center;
|
|
|
|
|
cp += del;
|
|
|
|
|
return dist <= 1.f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool plClosest::PointOnBox(const hsPoint3& p0,
|
|
|
|
|
const hsPoint3& corner,
|
|
|
|
|
const hsVector3& axis0,
|
|
|
|
|
const hsVector3& axis1,
|
|
|
|
|
const hsVector3& axis2,
|
|
|
|
|
hsPoint3& cp)
|
|
|
|
|
{
|
|
|
|
|
UInt32 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool plClosest::PointOnSphere(const hsPoint3& p0, const hsVector3& v0,
|
|
|
|
|
const hsPoint3& center, hsScalar rad,
|
|
|
|
|
hsPoint3& cp,
|
|
|
|
|
UInt32 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.
|
|
|
|
|
*/
|
|
|
|
|
hsScalar termA = v0.InnerProduct(v0);
|
|
|
|
|
if( termA < kRealSmall )
|
|
|
|
|
{
|
|
|
|
|
return PointOnSphere(p0, center, rad, cp);
|
|
|
|
|
}
|
|
|
|
|
hsVector3 p0Subc(&p0, ¢er);
|
|
|
|
|
hsScalar termB = v0.InnerProduct(p0Subc);
|
|
|
|
|
hsScalar termC = p0Subc.InnerProduct(p0Subc) - rad;
|
|
|
|
|
hsScalar disc = termB * termB - 4 * termA * termC;
|
|
|
|
|
if( disc >= 0 )
|
|
|
|
|
{
|
|
|
|
|
disc = hsSquareRoot(disc);
|
|
|
|
|
hsScalar t = (-termB - disc) / (2.f * termA);
|
|
|
|
|
if( (t < 0) && (clamp & kClampLower0) )
|
|
|
|
|
{
|
|
|
|
|
hsScalar 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool plClosest::PointOnBox(const hsPoint3& p0, const hsVector3& v0,
|
|
|
|
|
const hsPoint3& corner,
|
|
|
|
|
const hsVector3& axis0,
|
|
|
|
|
const hsVector3& axis1,
|
|
|
|
|
const hsVector3& axis2,
|
|
|
|
|
hsPoint3& cp,
|
|
|
|
|
UInt32 clamp)
|
|
|
|
|
{
|
|
|
|
|
UInt32 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool 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
|
|
|
|
|
*/
|
|
|
|
|
hsScalar invNLen = hsFastMath::InvSqrt(n.MagnitudeSquared());
|
|
|
|
|
|
|
|
|
|
hsScalar nDotp = n.InnerProduct(pPln - p0);
|
|
|
|
|
cp = p0 + n * (nDotp * invNLen);
|
|
|
|
|
|
|
|
|
|
return nDotp >= 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool plClosest::PointOnPlane(const hsPoint3& p0, const hsVector3& v0,
|
|
|
|
|
const hsPoint3& pPln, const hsVector3& n,
|
|
|
|
|
hsPoint3& cp,
|
|
|
|
|
UInt32 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.
|
|
|
|
|
*/
|
|
|
|
|
hsBool retVal = true;
|
|
|
|
|
hsScalar pDotn = n.InnerProduct(pPln - p0);
|
|
|
|
|
hsScalar v0Dotn = n.InnerProduct(v0);
|
|
|
|
|
if( (v0Dotn < -kRealSmall) || (v0Dotn > kRealSmall) )
|
|
|
|
|
{
|
|
|
|
|
hsScalar 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hsBool 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
|
|
|
|
|
hsBool 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;
|
|
|
|
|
|
|
|
|
|
hsBool retVal = true;
|
|
|
|
|
int i, j;
|
|
|
|
|
for( i = 0; i < 3; i++ )
|
|
|
|
|
{
|
|
|
|
|
hsPoint3 aBestPt;
|
|
|
|
|
hsPoint3 bBestPt;
|
|
|
|
|
|
|
|
|
|
hsScalar minDistSq = 1.e33f;
|
|
|
|
|
for( j = 0; j < 3; j++ )
|
|
|
|
|
{
|
|
|
|
|
hsPoint3 aNextPt, bNextPt;
|
|
|
|
|
PointsOnLines(aCurrPt, *aAxes[i],
|
|
|
|
|
bStartPt[j], *bAxes[j],
|
|
|
|
|
aNextPt, bNextPt,
|
|
|
|
|
plClosest::kClamp);
|
|
|
|
|
|
|
|
|
|
hsScalar 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
|
|
|
|
|
|
|
|
|
|
hsBool 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;
|
|
|
|
|
hsScalar minDistSq = 1.e33f;
|
|
|
|
|
|
|
|
|
|
hsBool 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);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
hsScalar 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
|
|
|
|
|
|
|
|
|
|
hsBool 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;
|
|
|
|
|
hsScalar minDistSq = 1.e33f;
|
|
|
|
|
|
|
|
|
|
hsBool 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);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
hsScalar 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
|
