/* * 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); } } }