350 lines
9.4 KiB
C++
Executable File
350 lines
9.4 KiB
C++
Executable File
//-----------------------------------------------------------------------------
|
|
// Torque Game Engine
|
|
// Copyright (C) GarageGames.com, Inc.
|
|
//-----------------------------------------------------------------------------
|
|
|
|
#include "game/rigid.h"
|
|
#include "console/console.h"
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
Rigid::Rigid()
|
|
{
|
|
force.set(0.0f,0.0f,0.0f);
|
|
torque.set(0.0f,0.0f,0.0f);
|
|
linVelocity.set(0.0f,0.0f,0.0f);
|
|
linPosition.set(0.0f,0.0f,0.0f);
|
|
linMomentum.set(0.0f,0.0f,0.0f);
|
|
angVelocity.set(0.0f,0.0f,0.0f);
|
|
angMomentum.set(0.0f,0.0f,0.0f);
|
|
|
|
angPosition.identity();
|
|
invWorldInertia.identity();
|
|
|
|
centerOfMass.set(0.0f,0.0f,0.0f);
|
|
worldCenterOfMass = linPosition;
|
|
mass = oneOverMass = 1.0f;
|
|
invObjectInertia.identity();
|
|
restitution = 0.3f;
|
|
friction = 0.5f;
|
|
atRest = false;
|
|
}
|
|
|
|
void Rigid::clearForces()
|
|
{
|
|
force.set(0.0f,0.0f,0.0f);
|
|
torque.set(0.0f,0.0f,0.0f);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Rigid::integrate(F32 delta)
|
|
{
|
|
// Update Angular position
|
|
F32 angle = angVelocity.len();
|
|
if (angle != 0.0f) {
|
|
QuatF dq;
|
|
F32 sinHalfAngle;
|
|
mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w);
|
|
sinHalfAngle *= 1.0f / angle;
|
|
dq.x = angVelocity.x * sinHalfAngle;
|
|
dq.y = angVelocity.y * sinHalfAngle;
|
|
dq.z = angVelocity.z * sinHalfAngle;
|
|
QuatF tmp = angPosition;
|
|
angPosition.mul(tmp, dq);
|
|
angPosition.normalize();
|
|
|
|
// Rotate the position around the center of mass
|
|
Point3F lp = linPosition - worldCenterOfMass;
|
|
dq.mulP(lp,&linPosition);
|
|
linPosition += worldCenterOfMass;
|
|
}
|
|
|
|
// Update angular momentum
|
|
angMomentum = angMomentum + torque * delta;
|
|
|
|
// Update linear position, momentum
|
|
linPosition = linPosition + linVelocity * delta;
|
|
linMomentum = linMomentum + force * delta;
|
|
linVelocity = linMomentum * oneOverMass;
|
|
|
|
// Update dependent state variables
|
|
updateInertialTensor();
|
|
updateVelocity();
|
|
updateCenterOfMass();
|
|
}
|
|
|
|
void Rigid::updateVelocity()
|
|
{
|
|
linVelocity.x = linMomentum.x * oneOverMass;
|
|
linVelocity.y = linMomentum.y * oneOverMass;
|
|
linVelocity.z = linMomentum.z * oneOverMass;
|
|
invWorldInertia.mulV(angMomentum,&angVelocity);
|
|
}
|
|
|
|
void Rigid::updateInertialTensor()
|
|
{
|
|
MatrixF iv,qmat;
|
|
angPosition.setMatrix(&qmat);
|
|
iv.mul(qmat,invObjectInertia);
|
|
qmat.transpose();
|
|
invWorldInertia.mul(iv,qmat);
|
|
}
|
|
|
|
void Rigid::updateCenterOfMass()
|
|
{
|
|
// Move the center of mass into world space
|
|
angPosition.mulP(centerOfMass,&worldCenterOfMass);
|
|
worldCenterOfMass += linPosition;
|
|
}
|
|
|
|
void Rigid::applyImpulse(const Point3F &r, const Point3F &impulse)
|
|
{
|
|
atRest = false;
|
|
|
|
// Linear momentum and velocity
|
|
linMomentum += impulse;
|
|
linVelocity.x = linMomentum.x * oneOverMass;
|
|
linVelocity.y = linMomentum.y * oneOverMass;
|
|
linVelocity.z = linMomentum.z * oneOverMass;
|
|
|
|
// Rotational momentum and velocity
|
|
Point3F tv;
|
|
mCross(r,impulse,&tv);
|
|
angMomentum += tv;
|
|
invWorldInertia.mulV(angMomentum, &angVelocity);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
/** Resolve collision with another rigid body
|
|
Computes & applies the collision impulses needed to keep the bodies
|
|
from interpenetrating.
|
|
|
|
tg: This function was commented out... I uncommented it, but haven't
|
|
double checked the math.
|
|
*/
|
|
bool Rigid::resolveCollision(const Point3F& p, const Point3F &normal, Rigid* rigid)
|
|
{
|
|
atRest = false;
|
|
Point3F v1,v2,r1,r2;
|
|
getOriginVector(p,&r1);
|
|
getVelocity(r1,&v1);
|
|
rigid->getOriginVector(p,&r2);
|
|
rigid->getVelocity(r2,&v2);
|
|
|
|
// Make sure they are converging
|
|
F32 nv = mDot(v1,normal);
|
|
nv -= mDot(v2,normal);
|
|
if (nv > 0.0f)
|
|
return false;
|
|
|
|
// Compute impulse
|
|
F32 d, n = -nv * (1.0f + restitution * rigid->restitution);
|
|
Point3F a1,b1,c1;
|
|
mCross(r1,normal,&a1);
|
|
invWorldInertia.mulV(a1,&b1);
|
|
mCross(b1,r1,&c1);
|
|
|
|
Point3F a2,b2,c2;
|
|
mCross(r2,normal,&a2);
|
|
rigid->invWorldInertia.mulV(a2,&b2);
|
|
mCross(b2,r2,&c2);
|
|
|
|
Point3F c3 = c1 + c2;
|
|
d = oneOverMass + rigid->oneOverMass + mDot(c3,normal);
|
|
Point3F impulse = normal * (n / d);
|
|
|
|
applyImpulse(r1,impulse);
|
|
impulse.neg();
|
|
applyImpulse(r2,impulse);
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
/** Resolve collision with an immovable object
|
|
Computes & applies the collision impulse needed to keep the body
|
|
from penetrating the given surface.
|
|
*/
|
|
bool Rigid::resolveCollision(const Point3F& p, const Point3F &normal)
|
|
{
|
|
atRest = false;
|
|
Point3F v,r;
|
|
getOriginVector(p,&r);
|
|
getVelocity(r,&v);
|
|
F32 n = -mDot(v,normal);
|
|
if (n >= 0.0f) {
|
|
|
|
// Collision impulse, straight forward force stuff.
|
|
F32 d = getZeroImpulse(r,normal);
|
|
F32 j = n * (1.0f + restitution) * d;
|
|
Point3F impulse = normal * j;
|
|
|
|
// Friction impulse, calculated as a function of the
|
|
// amount of force it would take to stop the motion
|
|
// perpendicular to the normal.
|
|
Point3F uv = v + (normal * n);
|
|
F32 ul = uv.len();
|
|
if (ul) {
|
|
uv /= -ul;
|
|
F32 u = ul * getZeroImpulse(r,uv);
|
|
j *= friction;
|
|
if (u > j)
|
|
u = j;
|
|
impulse += uv * u;
|
|
}
|
|
|
|
//
|
|
applyImpulse(r,impulse);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
/** Calculate the inertia along the given vector
|
|
This function can be used to calculate the amount of force needed to
|
|
affect a change in velocity along the specified normal applied at
|
|
the given point.
|
|
*/
|
|
F32 Rigid::getZeroImpulse(const Point3F& r,const Point3F& normal)
|
|
{
|
|
Point3F a,b,c;
|
|
mCross(r,normal,&a);
|
|
invWorldInertia.mulV(a,&b);
|
|
mCross(b,r,&c);
|
|
return 1 / (oneOverMass + mDot(c,normal));
|
|
}
|
|
|
|
F32 Rigid::getKineticEnergy()
|
|
{
|
|
Point3F w;
|
|
QuatF qmat = angPosition;
|
|
qmat.inverse();
|
|
qmat.mulP(angVelocity,&w);
|
|
const F32* f = invObjectInertia;
|
|
return 0.5f * ((mass * mDot(linVelocity,linVelocity)) +
|
|
w.x * w.x / f[0] +
|
|
w.y * w.y / f[5] +
|
|
w.z * w.z / f[10]);
|
|
}
|
|
|
|
void Rigid::getOriginVector(const Point3F &p,Point3F* r)
|
|
{
|
|
*r = p - worldCenterOfMass;
|
|
}
|
|
|
|
void Rigid::setCenterOfMass(const Point3F &newCenter)
|
|
{
|
|
// Sets the center of mass relative to the origin.
|
|
centerOfMass = newCenter;
|
|
|
|
// Update world center of mass
|
|
angPosition.mulP(centerOfMass,&worldCenterOfMass);
|
|
worldCenterOfMass += linPosition;
|
|
}
|
|
|
|
void Rigid::translateCenterOfMass(const Point3F &oldPos,const Point3F &newPos)
|
|
{
|
|
// I + mass * (crossmatrix(centerOfMass)^2 - crossmatrix(newCenter)^2)
|
|
MatrixF oldx,newx;
|
|
oldx.setCrossProduct(oldPos);
|
|
newx.setCrossProduct(newPos);
|
|
for (int row = 0; row < 3; row++)
|
|
for (int col = 0; col < 3; col++) {
|
|
F32 n = newx(row,col), o = oldx(row,col);
|
|
objectInertia(row,col) += mass * ((o * o) - (n * n));
|
|
}
|
|
|
|
// Make sure the matrix is symetrical
|
|
objectInertia(1,0) = objectInertia(0,1);
|
|
objectInertia(2,0) = objectInertia(0,2);
|
|
objectInertia(2,1) = objectInertia(1,2);
|
|
}
|
|
|
|
void Rigid::getVelocity(const Point3F& r, Point3F* v)
|
|
{
|
|
mCross(angVelocity, r, v);
|
|
*v += linVelocity;
|
|
}
|
|
|
|
void Rigid::getTransform(MatrixF* mat)
|
|
{
|
|
angPosition.setMatrix(mat);
|
|
mat->setColumn(3,linPosition);
|
|
}
|
|
|
|
void Rigid::setTransform(const MatrixF& mat)
|
|
{
|
|
angPosition.set(mat);
|
|
mat.getColumn(3,&linPosition);
|
|
|
|
// Update center of mass
|
|
angPosition.mulP(centerOfMass,&worldCenterOfMass);
|
|
worldCenterOfMass += linPosition;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
/** Set the rigid body moment of inertia
|
|
The moment is calculated as a box with the given dimensions.
|
|
*/
|
|
void Rigid::setObjectInertia(const Point3F& r)
|
|
{
|
|
// Rotational moment of inertia of a box
|
|
F32 ot = mass / 12.0f;
|
|
F32 a = r.x * r.x;
|
|
F32 b = r.y * r.y;
|
|
F32 c = r.z * r.z;
|
|
|
|
objectInertia.identity();
|
|
F32* f = objectInertia;
|
|
f[0] = ot * (b + c);
|
|
f[5] = ot * (c + a);
|
|
f[10] = ot * (a + b);
|
|
|
|
invertObjectInertia();
|
|
updateInertialTensor();
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
/** Set the rigid body moment of inertia
|
|
The moment is calculated as a unit sphere.
|
|
*/
|
|
void Rigid::setObjectInertia()
|
|
{
|
|
objectInertia.identity();
|
|
F32 radius = 1.0f;
|
|
F32* f = objectInertia;
|
|
f[0] = f[5] = f[10] = (0.4f * mass * radius * radius);
|
|
invertObjectInertia();
|
|
updateInertialTensor();
|
|
}
|
|
|
|
void Rigid::invertObjectInertia()
|
|
{
|
|
invObjectInertia = objectInertia;
|
|
invObjectInertia.fullInverse();
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
bool Rigid::checkRestCondition()
|
|
{
|
|
// F32 k = getKineticEnergy(mWorldToObj);
|
|
// F32 G = -force.z * oneOverMass * 0.032;
|
|
// F32 Kg = 0.5 * mRigid.mass * G * G;
|
|
// if (k < Kg * restTol)
|
|
// mRigid.setAtRest();
|
|
return atRest;
|
|
}
|
|
|
|
void Rigid::setAtRest()
|
|
{
|
|
atRest = true;
|
|
linVelocity.set(0.0f,0.0f,0.0f);
|
|
linMomentum.set(0.0f,0.0f,0.0f);
|
|
angVelocity.set(0.0f,0.0f,0.0f);
|
|
angMomentum.set(0.0f,0.0f,0.0f);
|
|
}
|