/*
================
idPhysics_RigidBody::DropToFloorAndRest
Drops the object straight down to the floor and verifies if the object is at rest on the floor.
================
*/
void idPhysics_RigidBody::DropToFloorAndRest( void ) {
idVec3 down;
trace_t tr;
if( testSolid ) {
testSolid = false;
if( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) {
gameLocal.DWarning( "rigid body in solid for entity '%s' type '%s' at (%s)",
self->name.c_str(), self->GetType()->classname, current.i.position.ToString( 0 ) );
Rest();
dropToFloor = false;
return;
}
}
// put the body on the floor
down = current.i.position + gravityNormal * 128.0f;
gameLocal.clip.Translation( tr, current.i.position, down, clipModel, current.i.orientation, clipMask, self );
current.i.position = tr.endpos;
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), tr.endpos, current.i.orientation );
// if on the floor already
if( tr.fraction == 0.0f ) {
// test if we are really at rest
EvaluateContacts();
if( !TestIfAtRest() ) {
gameLocal.DWarning( "rigid body not at rest for entity '%s' type '%s' at (%s)",
self->name.c_str(), self->GetType()->classname, current.i.position.ToString( 0 ) );
}
Rest();
dropToFloor = false;
} else if( IsOutsideWorld() ) {
gameLocal.Warning( "rigid body outside world bounds for entity '%s' type '%s' at (%s)",
self->name.c_str(), self->GetType()->classname, current.i.position.ToString( 0 ) );
Rest();
dropToFloor = false;
}
}
/*
================
sdPhysics_Linear::Evaluate
================
*/
bool sdPhysics_Linear::Evaluate( int timeStepMSec, int endTimeMSec ) {
current.origin.FixDenormals();
idVec3 oldLocalOrigin, oldOrigin, masterOrigin;
idMat3 oldAxis, masterAxis;
isBlocked = false;
oldLocalOrigin = current.localOrigin;
oldOrigin = current.origin;
oldAxis = axis;
current.localOrigin = current.linearExtrapolation.GetCurrentValue( endTimeMSec );
current.origin = current.localOrigin;
if ( hasMaster ) {
self->GetMasterPosition( masterOrigin, masterAxis );
if ( masterAxis.IsRotated() ) {
current.origin = current.origin * masterAxis + masterOrigin;
if ( isOrientated ) {
axis *= masterAxis;
}
} else {
current.origin += masterOrigin;
}
}
if ( isPusher && ( oldOrigin != current.origin ) ) {
gameLocal.push.ClipPush( pushResults, self, pushFlags, oldOrigin, oldAxis, current.origin, axis, GetClipModel() );
if ( pushResults.fraction < 1.0f ) {
clipModel->Link( gameLocal.clip, self, 0, oldOrigin, oldAxis );
current.localOrigin = oldLocalOrigin;
current.origin = oldOrigin;
axis = oldAxis;
isBlocked = true;
return false;
}
}
if ( clipModel ) {
clipModel->Link( gameLocal.clip, self, 0, current.origin, axis );
}
current.time = endTimeMSec;
if ( TestIfAtRest() ) {
Rest();
}
return ( current.origin != oldOrigin );
}
/*
================
idPhysics_Parametric::Evaluate
================
*/
bool idPhysics_Parametric::Evaluate( int timeStepMSec, int endTimeMSec ) {
idVec3 oldLocalOrigin, oldOrigin, masterOrigin;
idAngles oldLocalAngles, oldAngles;
idMat3 oldAxis, masterAxis;
isBlocked = false;
oldLocalOrigin = current.localOrigin;
oldOrigin = current.origin;
oldLocalAngles = current.localAngles;
oldAngles = current.angles;
oldAxis = current.axis;
current.localOrigin.Zero();
current.localAngles.Zero();
if ( current.spline != NULL ) {
float length = current.splineInterpolate.GetCurrentValue( endTimeMSec );
float t = current.spline->GetTimeForLength( length, 0.01f );
current.localOrigin = current.spline->GetCurrentValue( t );
if ( current.useSplineAngles ) {
current.localAngles = current.spline->GetCurrentFirstDerivative( t ).ToAngles();
}
} else if ( current.linearInterpolation.GetDuration() != 0 ) {
current.localOrigin += current.linearInterpolation.GetCurrentValue( endTimeMSec );
} else {
current.localOrigin += current.linearExtrapolation.GetCurrentValue( endTimeMSec );
}
if ( current.angularInterpolation.GetDuration() != 0 ) {
current.localAngles += current.angularInterpolation.GetCurrentValue( endTimeMSec );
} else {
current.localAngles += current.angularExtrapolation.GetCurrentValue( endTimeMSec );
}
current.localAngles.Normalize360();
current.origin = current.localOrigin;
current.angles = current.localAngles;
current.axis = current.localAngles.ToMat3();
if ( hasMaster ) {
self->GetMasterPosition( masterOrigin, masterAxis );
if ( masterAxis.IsRotated() ) {
current.origin = current.origin * masterAxis + masterOrigin;
if ( isOrientated ) {
current.axis *= masterAxis;
current.angles = current.axis.ToAngles();
}
}
else {
current.origin += masterOrigin;
}
}
if ( isPusher ) {
gameLocal.push.ClipPush( pushResults, self, pushFlags, oldOrigin, oldAxis, current.origin, current.axis );
if ( pushResults.fraction < 1.0f ) {
// RAVEN BEGIN
// ddynerman: multiple clip worlds
clipModel->Link( self, 0, oldOrigin, oldAxis );
// RAVEN END
current.localOrigin = oldLocalOrigin;
current.origin = oldOrigin;
current.localAngles = oldLocalAngles;
current.angles = oldAngles;
current.axis = oldAxis;
isBlocked = true;
return false;
}
current.angles = current.axis.ToAngles();
}
if ( clipModel ) {
// RAVEN BEGIN
// abahr: a hack way of hiding gimble lock from movers.
clipModel->Link( self, 0, current.origin, UseAxisOffset() ? GetAxisOffset() * current.axis : current.axis );
// RAVEN END
}
current.time = endTimeMSec;
if ( TestIfAtRest() ) {
Rest();
}
return ( current.origin != oldOrigin || current.axis != oldAxis );
}
/*
================
idPhysics_RigidBody::Evaluate
Evaluate the impulse based rigid body physics.
When a collision occurs an impulse is applied at the moment of impact but
the remaining time after the collision is ignored.
================
*/
bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) {
rigidBodyPState_t next;
idAngles angles;
trace_t collision;
idVec3 impulse;
idEntity *ent;
idVec3 oldOrigin, masterOrigin;
idMat3 oldAxis, masterAxis;
float timeStep;
bool collided, cameToRest = false;
timeStep = MS2SEC( timeStepMSec );
current.lastTimeStep = timeStep;
if ( hasMaster ) {
oldOrigin = current.i.position;
oldAxis = current.i.orientation;
self->GetMasterPosition( masterOrigin, masterAxis );
current.i.position = masterOrigin + current.localOrigin * masterAxis;
if ( isOrientated ) {
current.i.orientation = current.localAxis * masterAxis;
}
else {
current.i.orientation = current.localAxis;
}
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
current.i.linearMomentum = mass * ( ( current.i.position - oldOrigin ) / timeStep );
current.i.angularMomentum = inertiaTensor * ( ( current.i.orientation * oldAxis.Transpose() ).ToAngularVelocity() / timeStep );
current.externalForce.Zero();
current.externalTorque.Zero();
return ( current.i.position != oldOrigin || current.i.orientation != oldAxis );
}
// if the body is at rest
if ( current.atRest >= 0 || timeStep <= 0.0f ) {
DebugDraw();
return false;
}
// if putting the body to rest
if ( dropToFloor ) {
DropToFloorAndRest();
current.externalForce.Zero();
current.externalTorque.Zero();
return true;
}
#ifdef RB_TIMINGS
timer_total.Start();
#endif
// move the rigid body velocity into the frame of a pusher
// current.i.linearMomentum -= current.pushVelocity.SubVec3( 0 ) * mass;
// current.i.angularMomentum -= current.pushVelocity.SubVec3( 1 ) * inertiaTensor;
clipModel->Unlink();
next = current;
// calculate next position and orientation
Integrate( timeStep, next );
#ifdef RB_TIMINGS
timer_collision.Start();
#endif
// check for collisions from the current to the next state
collided = CheckForCollisions( timeStep, next, collision );
#ifdef RB_TIMINGS
timer_collision.Stop();
#endif
// set the new state
current = next;
if ( collided ) {
// apply collision impulse
if ( CollisionImpulse( collision, impulse ) ) {
current.atRest = gameLocal.time;
}
}
// update the position of the clip model
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
DebugDraw();
if ( !noContact ) {
#ifdef RB_TIMINGS
timer_collision.Start();
#endif
// get contacts
EvaluateContacts();
#ifdef RB_TIMINGS
timer_collision.Stop();
#endif
// check if the body has come to rest
if ( TestIfAtRest() ) {
// put to rest
Rest();
cameToRest = true;
} else {
// apply contact friction
ContactFriction( timeStep );
}
}
if ( current.atRest < 0 ) {
ActivateContactEntities();
}
if ( collided ) {
// if the rigid body didn't come to rest or the other entity is not at rest
ent = gameLocal.entities[collision.c.entityNum];
if ( ent && ( !cameToRest || !ent->IsAtRest() ) ) {
// apply impact to other entity
ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
}
}
// move the rigid body velocity back into the world frame
// current.i.linearMomentum += current.pushVelocity.SubVec3( 0 ) * mass;
// current.i.angularMomentum += current.pushVelocity.SubVec3( 1 ) * inertiaTensor;
current.pushVelocity.Zero();
current.lastTimeStep = timeStep;
current.externalForce.Zero();
current.externalTorque.Zero();
if ( IsOutsideWorld() ) {
gameLocal.Warning( "rigid body moved outside world bounds for entity '%s' type '%s' at (%s)",
self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) );
Rest();
}
#ifdef RB_TIMINGS
timer_total.Stop();
if ( rb_showTimings->integer == 1 ) {
gameLocal.Printf( "%12s: t %u cd %u\n",
self->name.c_str(),
timer_total.Milliseconds(), timer_collision.Milliseconds() );
lastTimerReset = 0;
}
else if ( rb_showTimings->integer == 2 ) {
numRigidBodies++;
if ( endTimeMSec > lastTimerReset ) {
gameLocal.Printf( "rb %d: t %u cd %u\n",
numRigidBodies,
timer_total.Milliseconds(), timer_collision.Milliseconds() );
}
}
if ( endTimeMSec > lastTimerReset ) {
lastTimerReset = endTimeMSec;
numRigidBodies = 0;
timer_total.Clear();
timer_collision.Clear();
}
#endif
return true;
}
