//! Do update for the RB by useing the second test case
TEFUNC void updateRigidBodyForTwo(RigidBodyPtr body) {
body->setVelocity(lambda * body->getPosition());
body->setAngularVelocity(lambda * body->getOrientation().getEulerRotation());
body->setAngularAcceleration(Vec3(0,0,0));
}
//! Do update for the RB by useing the first test case
TEFUNC void updateRigidBodyForOne(RigidBodyPtr body) {
body->setVelocity(lambda * body->getPosition());
body->setAcceleration(lambda * body->getVelocity());
Vec3 axis;
float angle;
body->getOrientation().getAxisAngle(axis, angle);
Quaternion tmp(lambda * angle, axis);
body->setAngularVelocity(tmp.getEulerRotation());
body->setAngularAcceleration(lambda * body->getAngularVelocity());
}
TEFUNC void printRigidBody(RigidBodyPtr body) {
Vec3 axis;
float angle;
body->getOrientation().getAxisAngle(axis, angle);
cout << "Differences from " << body->getId() << " to reference:" << endl
<< " Position: " << body->getPosition() - correctPosition << endl
<< " Velocity: " << body->getVelocity() - correctVelocity << endl
//! \todo komponentenweise subtraktion für quaternionen
<< " Orientation: " << body->getOrientation() + (-1 * correctOrientation) << endl
<< " AngularVelocity: " << body->getAngularVelocity() - correctAngularVelocity << endl
<< " RotationAngle: " << angle - correctRotationAngle << endl
<< " RotationAxis: " << axis - correctRotationAxis
<< endl << endl;
}
