// 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;
}
hsBool plConvexVolume::BouncePoint(hsPoint3 &pos, hsVector3 &velocity, float bounce, float friction) const
{
float minDist = 1.e33f;
int32_t minIndex = -1;
float currDist;
int i;
for (i = 0; i < fNumPlanes; i++)
{
currDist = -fWorldPlanes[i].fD - fWorldPlanes[i].fN.InnerProduct(pos);
if (currDist < 0)
return false; // We're not inside this plane, and thus outside the volume
if (currDist < minDist)
{
minDist = currDist;
minIndex = i;
}
}
pos += (-fWorldPlanes[minIndex].fD - fWorldPlanes[minIndex].fN.InnerProduct(pos)) * fWorldPlanes[minIndex].fN;
hsVector3 bnc = -velocity.InnerProduct(fWorldPlanes[minIndex].fN) * fWorldPlanes[minIndex].fN;
velocity += bnc;
velocity *= 1.f - friction;
velocity += bnc * bounce;
// velocity += (velocity.InnerProduct(fWorldPlanes[minIndex].fN) * -(1.f + bounce)) * fWorldPlanes[minIndex].fN;
return true;
}
bool plTriUtils::ProjectOntoPlaneAlongVector(const hsVector3& norm, float dist, const hsVector3& vec, hsPoint3& p)
{
float s = norm.InnerProduct(vec);
const float kAlmostZero = 1.e-5f;
if( (s > kAlmostZero)||(s < kPastZero) )
{
dist /= s;
p += vec * dist;
return true;
}
return false;
}
// 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);
}
plTriUtils::Bary plTriUtils::IComputeBarycentric(const hsVector3& v12, float invLenSq12, const hsVector3& v0, const hsVector3& v1, hsPoint3& out)
{
uint32_t state = 0;
float lenSq0 = v0.MagnitudeSquared();
if( lenSq0 < kAlmostZeroSquared )
{
// On edge p1-p2;
out[0] = 0;
state |= kOnEdge12;
}
else
{
out[0] = lenSq0 * invLenSq12;
out[0] = sqrt(out[0]);
//
if( v0.InnerProduct(v12) < 0 )
{
out[0] = -out[0];
state |= kOutsideTri;
}
else if( out[0] > kPastOne )
state |= kOutsideTri;
else if( out[0] > kAlmostOne )
state |= kOnVertex0;
}
float lenSq1 = v1.MagnitudeSquared();
if( lenSq1 < kAlmostZeroSquared )
{
// On edge p0-p2
out[1] = 0;
state |= kOnEdge02;
}
else
{
out[1] = lenSq1 * invLenSq12;
out[1] = sqrt(out[1]);
if( v1.InnerProduct(v12) < 0 )
{
out[1] = -out[1];
state |= kOutsideTri;
}
else if( out[1] > kPastOne )
state |= kOutsideTri;
else if( out[1] > kAlmostOne )
state |= kOnVertex1;
}
// Could make more robust against precision problems
// by repeating above for out[2], then normalizing
// so sum(out[i]) = 1.f
out[2] = 1.f - out[0] - out[1];
if( out[2] < kPastZero )
state |= kOutsideTri;
else if( out[2] < kAlmostZero )
state |= kOnEdge01;
else if( out[2] > kAlmostOne )
state |= kOnVertex2;
/*
if( a,b,c outside range [0..1] )
p is outside tri;
else if( a,b,c == 1 )
p is on vert;
else if( a,b,c == 0 )
p is on edge;
*/
if( state & kOutsideTri )
return kOutsideTri;
if( state & kOnVertex )
return Bary(state & kOnVertex);
if( state & kOnEdge )
return Bary(state & kOnEdge);
return kInsideTri;
}