/*
* Uses SIMPLE_INTEGRATION(duration), defined above.
*/
void Kinematic::integrate(const SteeringOutput& steer,
const SteeringOutput& drag,
real duration)
{
SIMPLE_INTEGRATION(duration, velocity, rotation);
velocity.x *= real_pow(drag.linear.x, duration);
velocity.y *= real_pow(drag.linear.y, duration);
velocity.z *= real_pow(drag.linear.z, duration);
rotation *= real_pow(drag.angular, duration);
velocity.x += steer.linear.x*duration;
velocity.y += steer.linear.y*duration;
velocity.z += steer.linear.z*duration;
rotation += steer.angular*duration;
}
/*
* Uses SIMPLE_INTEGRATION(duration), defined above.
*/
void Kinematic::integrate(const SteeringOutput& steer,
real drag,
real duration)
{
SIMPLE_INTEGRATION(duration, velocity, rotation);
// Slowing velocity and rotational velocity
drag = real_pow(drag, duration);
velocity *= drag;
rotation *= drag*drag;
velocity.x += steer.linear.x*duration;
velocity.y += steer.linear.y*duration;
velocity.z += steer.linear.z*duration;
rotation += steer.angular*duration;
}
//!Update features of RigidBody
void MainPhysics::updateFeatures(RigidBody * _body, real _duration)
{
if (!_body->hasFiniteMass()) {
return;
}
if (!_body->isAwake()) {
return;
}
// calculate linear acceleration from force
Vector3 lastAcceleration = _body->getAcceleration();
lastAcceleration.addScaledVector(_body->getForceAccum(), _body->getInverseMass());
_body->setLastFrameAcceleration(lastAcceleration);
// calculate angular acceleration from torque
Matrix3 inverseInertia = _body->getInverseInertiaTensorWorld();
Vector3 angularAcceleration = inverseInertia.transform(_body->getTorqueAccum());
// adjust velocities
// update linear velocity from both acceleration and impulse
Vector3 velocity = _body->getVelocity();
velocity.addScaledVector(_body->getLastFrameAcceleration(), _duration);
_body->setVelocity(velocity);
// update angular velocity from both acceleration and impulse
Vector3 rotation = _body->getRotation();
rotation.addScaledVector(angularAcceleration, _duration);
// impose drag
_body->setVelocity(_body->getVelocity()*real_pow(_body->getLinearDamping(), _duration));
_body->setRotation(_body->getRotation()*real_pow(_body->getAngularDamping(), _duration));
// adjust positions
// update linear position
Vector3 position = _body->getPosition();
position.addScaledVector(_body->getVelocity(), _duration);
_body->setPosition(position);
// update angular position.
Quaternion orientation = _body->getOrientation();
orientation.addScaledVector(_body->getRotation(), _duration);
_body->setOrientation(orientation);
// normalize the orientation, and update the matrices with the new position and orientation
_body->calculateDerivedData();
// printf("info, velocity-> x: %2.6f, y: %2.6f, z: %2.6f \n", _body->getVelocity()->x, _body->getVelocity()->y,_body->getVelocity()->z);
// printf("info, rotation-> x: %2.6f, y: %2.6f, z: %2.6f \n", _body->getRotation()->x, _body->getRotation()->y,_body->getRotation()->z);
// printf("info, force -> x: %2.6f, y: %2.6f, z: %2.6f \n", _body->getForceAccum()->x, _body->getForceAccum()->y,_body->getForceAccum()->z);
// printf("info, torque -> x: %2.6f, y: %2.6f, z: %2.6f \n", _body->getTorqueAccum()->x, _body->getTorqueAccum()->y,_body->getTorqueAccum()->z);
// clear accumulators.
_body->clearAccumulators();
// update the kinetic energy store, and possibly put the body to sleep
if (_body->isCanSleep()) {
real currentMotion = _body->getVelocity().scalarProduct(_body->getVelocity()) +
_body->getRotation().scalarProduct(_body->getRotation());
real bias = real_pow(0.5, _duration);
_body->setMotion(bias*_body->getMotion() + (1-bias)*currentMotion);
// printf("sleep, motion: %f, \n", _body->getMotion());
if (_body->getMotion() < SLEEP_EPSILON) {
_body->setAwake(false);
} else if (_body->getMotion() > 10.0f * SLEEP_EPSILON) {
_body->setMotion(10.0f * SLEEP_EPSILON);
}
}
}
