void plPXPhysicalControllerCore::ISetKinematicLoc(const hsMatrix44& l2w)
{
hsPoint3 kPos;
// Update our subworld position and rotation
const plCoordinateInterface* subworldCI = GetSubworldCI();
if (subworldCI)
{
const hsMatrix44& w2s = subworldCI->GetWorldToLocal();
hsMatrix44 l2s = w2s * l2w;
l2s.GetTranslate(&kPos);
}
else
{
l2w.GetTranslate(&kPos);
}
hsMatrix44 w2l;
l2w.GetInverse(&w2l);
if (fProxyGen)
fProxyGen->SetTransform(l2w, w2l);
// add z offset
kPos.fZ += kPhysZOffset;
// Update the physical position of kinematic
if (fKinematicActor->readBodyFlag(NX_BF_KINEMATIC))
fKinematicActor->moveGlobalPosition(plPXConvert::Point(kPos));
else
fKinematicActor->setGlobalPosition(plPXConvert::Point(kPos));
}
void plPXPhysicalControllerCore::ISetGlobalLoc(const hsMatrix44& l2w)
{
fLastGlobalLoc = l2w;
// Update our subworld position and rotation
const plCoordinateInterface* subworldCI = GetSubworldCI();
if (subworldCI)
{
const hsMatrix44& w2s = fPrevSubworldW2L;
hsMatrix44 l2s = w2s * l2w;
l2s.GetTranslate(&fLocalPosition);
fLocalRotation.SetFromMatrix44(l2s);
}
else
{
l2w.GetTranslate(&fLocalPosition);
fLocalRotation.SetFromMatrix44(l2w);
}
hsMatrix44 w2l;
l2w.GetInverse(&w2l);
if (fProxyGen)
fProxyGen->SetTransform(l2w, w2l);
// Update the physical position
NxExtendedVec3 nxPos(fLocalPosition.fX, fLocalPosition.fY, fLocalPosition.fZ + kPhysZOffset);
fController->setPosition(nxPos);
IMatchKinematicToController();
}
void plAnimatedMovementStrategy::IRecalcLinearVelocity(float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat)
{
hsPoint3 startPos(0.0f, 0.0f, 0.0f); // default position (at start of anim)
hsPoint3 prevPos = prevMat.GetTranslate(); // position previous frame
hsPoint3 nowPos = curMat.GetTranslate(); // position current frame
hsVector3 prev2Now = (hsVector3)(nowPos - prevPos); // frame-to-frame delta
if (fabs(prev2Now.fX) < 0.0001f && fabs(prev2Now.fY) < 0.0001f && fabs(prev2Now.fZ) < 0.0001f)
{
fAnimLinearVel.Set(0.f, 0.f, 0.f);
}
else
{
hsVector3 start2Now = (hsVector3)(nowPos - startPos); // start-to-frame delta
float prev2NowMagSqr = prev2Now.MagnitudeSquared();
float start2NowMagSqr = start2Now.MagnitudeSquared();
float dot = prev2Now.InnerProduct(start2Now);
// HANDLING ANIMATION WRAPPING:
// the vector from the animation origin to the current frame should point in roughly
// the same direction as the vector from the previous animation position to the
// current animation position.
//
// If they don't agree (dot < 0,) then we probably mpst wrapped around.
// The right answer would be to compare the current frame to the start of
// the anim loop, but it's cheaper to cheat and use the previous frame's velocity.
if (dot > 0.0f)
{
prev2Now /= elapsed;
float xfabs = fabs(prev2Now.fX);
float yfabs = fabs(prev2Now.fY);
float zfabs = fabs(prev2Now.fZ);
static const float maxVel = 20.0f;
bool valid = xfabs < maxVel && yfabs < maxVel && zfabs < maxVel;
if (valid)
{
fAnimLinearVel = prev2Now;
}
}
}
}
void GetPositionAndRotation(hsMatrix44 transform, hsScalarTriple *position, hsQuat *rotation)
{
hsPoint3 p = (hsPoint3)transform.GetTranslate();
position->fX = p.fX; position->fY = p.fY; position->fZ = p.fZ;
transform.RemoveScale();
rotation->SetFromMatrix(&transform);
rotation->Normalize();
float angle;
hsVector3 axis;
rotation->GetAngleAxis(&angle, &axis);
}