// 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::ProjectOntoPlane(const hsVector3& norm, float dist, hsPoint3& p)
{
float normMagSq = norm.MagnitudeSquared();
if( normMagSq > kAlmostZero )
{
dist /= normMagSq;
p += norm * dist;
return true;
}
return false;
}
bool plPXPhysical::GetAngularVelocitySim(hsVector3& vel) const
{
bool result = false;
if (fActor->isDynamic())
{
vel = plPXConvert::Vector(fActor->getAngularVelocity());
result = true;
}
else
vel.Set(0, 0, 0);
return result;
}
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;
}
void plSwimStraightCurrentRegion::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
angularResult = 0.f;
if (elapsed <= 0.f || GetProperty(kDisable))
{
linearResult.Set(0.f, 0.f, 0.f);
return;
}
hsPoint3 center, pos;
hsVector3 current = fCurrentSO->GetLocalToWorld() * hsVector3(0.f, 1.f, 0.f);
hsScalarTriple xlate = fCurrentSO->GetLocalToWorld().GetTranslate();
center.Set(&xlate);
physical->GetPositionSim(pos);
plKey worldKey = physical->GetSubworld();
if (worldKey)
{
plSceneObject* so = plSceneObject::ConvertNoRef(worldKey->ObjectIsLoaded());
hsMatrix44 w2l = so->GetWorldToLocal();
center = w2l * center;
current = w2l * current;
}
hsVector3 pos2Center(center.fX - pos.fX, center.fY - pos.fY, 0.f);
float dist = current.InnerProduct(pos - center);
float pullVel;
if (dist <= fNearDist)
pullVel = fNearVel;
else if (dist >= fFarDist)
pullVel = fFarVel;
else
pullVel = fNearVel + (fFarVel - fNearVel) * (dist - fNearDist) / (fFarDist - fNearDist);
linearResult = current * pullVel;
}
// 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;
}
void plSwimRegionInterface::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
linearResult.Set(0.f, 0.f, 0.f);
angularResult = 0.f;
}
void plSwimCircularCurrentRegion::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
if (elapsed <= 0.f || fCurrentSO == nil || GetProperty(kDisable))
{
linearResult.Set(0.f, 0.f, 0.f);
angularResult = 0.f;
return;
}
hsPoint3 center, pos;
hsScalarTriple xlate = fCurrentSO->GetLocalToWorld().GetTranslate();
center.Set(&xlate);
plKey worldKey = physical->GetSubworld();
if (worldKey)
{
plSceneObject* so = plSceneObject::ConvertNoRef(worldKey->ObjectIsLoaded());
center = so->GetWorldToLocal() * center;
}
center.fZ = 0.f; // Just doing 2D
physical->GetPositionSim(pos);
hsVector3 pos2Center(center.fX - pos.fX, center.fY - pos.fY, 0.f);
float pullVel;
float distSq = pos2Center.MagnitudeSquared();
if (distSq < .5)
{
// Don't want to pull us too close to the center, or we
// get this annoying jitter.
pullVel = 0.f;
}
else if (distSq <= fPullNearDistSq)
pullVel = fPullNearVel;
else if (distSq >= fPullFarDistSq)
pullVel = fPullFarVel;
else
pullVel = fPullNearVel + (fPullFarVel - fPullNearVel) * (distSq - fPullNearDistSq) / (fPullFarDistSq - fPullNearDistSq);
hsVector3 pull = pos2Center;
pull.Normalize();
linearResult.Set(pull.fY, -pull.fX, pull.fZ);
pull *= pullVel;
linearResult *= fRotation;
linearResult += pull;
hsVector3 v1 = linearResult * elapsed - pos2Center;
hsVector3 v2 = -pos2Center;
float invCos = v1.InnerProduct(v2) / v1.Magnitude() / v2.Magnitude();
if (invCos > 1)
invCos = 1;
if (invCos < -1)
invCos = -1;
angularResult = acos(invCos) / elapsed;
// hsAssert(real_finite(linearResult.fX) &&
// real_finite(linearResult.fY) &&
// real_finite(linearResult.fZ) &&
// real_finite(angularResult), "Bad water current computation.");
}