void hsMatrix44::MakeCameraMatrices(const hsPoint3& from, const hsPoint3& at, const hsVector3& up, hsMatrix44& worldToCamera, hsMatrix44& cameraToWorld)
{
hsVector3 dirZ(&at, &from);
hsVector3 dirX(dirZ % up);
dirX.Normalize();
hsVector3 dirY(dirX % dirZ);
dirY.Normalize();
worldToCamera.Reset(false);
cameraToWorld.Reset(false);
int i;
for( i = 0; i < 3; i++ )
{
worldToCamera.fMap[0][i] = dirX[i];
worldToCamera.fMap[1][i] = dirY[i];
worldToCamera.fMap[2][i] = dirZ[i];
cameraToWorld.fMap[i][0] = dirX[i];
cameraToWorld.fMap[i][1] = dirY[i];
cameraToWorld.fMap[i][2] = dirZ[i];
}
hsPoint3 trans = -from;
worldToCamera.fMap[0][3] = dirX.InnerProduct(trans);
worldToCamera.fMap[1][3] = dirY.InnerProduct(trans);
worldToCamera.fMap[2][3] = dirZ.InnerProduct(trans);
cameraToWorld.fMap[0][3] = from.fX;
cameraToWorld.fMap[1][3] = from.fY;
cameraToWorld.fMap[2][3] = from.fZ;
}
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 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);
}
static void ClearMatrix(hsMatrix44 &m)
{
m.fMap[0][0] = 0.0f; m.fMap[0][1] = 0.0f; m.fMap[0][2] = 0.0f; m.fMap[0][3] = 0.0f;
m.fMap[1][0] = 0.0f; m.fMap[1][1] = 0.0f; m.fMap[1][2] = 0.0f; m.fMap[1][3] = 0.0f;
m.fMap[2][0] = 0.0f; m.fMap[2][1] = 0.0f; m.fMap[2][2] = 0.0f; m.fMap[2][3] = 0.0f;
m.fMap[3][0] = 0.0f; m.fMap[3][1] = 0.0f; m.fMap[3][2] = 0.0f; m.fMap[3][3] = 0.0f;
m.NotIdentity();
}
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 plPXPhysical::ISetTransformGlobal(const hsMatrix44& l2w)
{
hsAssert(fActor->isDynamic(), "Shouldn't move a static actor");
// If we wake up normal dynamic actors, they might explode.
// However, kinematics won't update if they are asleep. Thankfully, kinematics don't
// explode, move, or cause spontaneous nuclear warfare.
if (fActor->readBodyFlag(NX_BF_KINEMATIC))
fActor->wakeUp();
NxMat34 mat;
if (fWorldKey)
{
plSceneObject* so = plSceneObject::ConvertNoRef(fWorldKey->ObjectIsLoaded());
hsAssert(so, "Scene object not loaded while accessing subworld.");
// physical to subworld (simulation space)
hsMatrix44 p2s = so->GetCoordinateInterface()->GetWorldToLocal() * l2w;
plPXConvert::Matrix(p2s, mat);
if (fProxyGen)
{
hsMatrix44 w2l;
p2s.GetInverse(&w2l);
fProxyGen->SetTransform(p2s, w2l);
}
}
// No need to localize
else
{
plPXConvert::Matrix(l2w, mat);
if (fProxyGen)
{
hsMatrix44 w2l;
l2w.GetInverse(&w2l);
fProxyGen->SetTransform(l2w, w2l);
}
}
// This used to check for the kPhysAnim flag, however animated detectors
// are also kinematic but not kPhysAnim, therefore, this would break on PhysX
// SDKs (yes, I'm looking at you, 2.6.4) that actually obey the ***GlobalPose
// rules set forth in the SDK documentation.
if (fActor->readBodyFlag(NX_BF_KINEMATIC))
fActor->moveGlobalPose(mat);
else
fActor->setGlobalPose(mat);
}
// GETTRANSFORM
void plPXPhysical::GetTransform(hsMatrix44& l2w, hsMatrix44& w2l)
{
IGetTransformGlobal(l2w);
l2w.GetInverse(&w2l);
}