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;
}
TEFUNC void computeCorrectValuesForOne() {
simulationTime += interval;
double scalar = exp(lambda * simulationTime);
correctPosition = scalar * positionInit;
Vec3 axis;
float angle;
orientationInit.getAxisAngle(axis, angle);
correctOrientation = Quaternion(scalar * angle, axis);
correctAngularVelocity = scalar * angularVelocityInit;
referenceBody->setPosition(correctPosition);
referenceBody->setVelocity(correctVelocity);
referenceBody->setOrientation(correctOrientation);
referenceBody->setAngularVelocity(correctAngularVelocity);
referenceBody->getOrientation().getAxisAngle(axis, angle);
correctRotationAxis = axis;
correctRotationAngle = angle;
cout << "Correct Values:" << endl
<< " Position: " << correctPosition << endl
<< " Velocity: " << correctVelocity << endl
<< " Orientation: " << correctOrientation << endl
<< " AngularVelocity: " << correctAngularVelocity << endl
<< " RotationAngle: " << correctRotationAngle << endl
<< " RotationAxis: " << correctRotationAxis
<< endl << endl;
}
TEFUNC void computeCorrectValuesForThree() {
simulationTime += interval;
double scalar = exp(lambda * simulationTime);
Vec3 correctRotation(orientationInit.getEulerRotation() +
lambda * simulationTime *
scalar * angularVelocityInit);
correctPosition = positionInit +
lambda * simulationTime *
scalar * velocityInit;
correctOrientation = Quaternion(correctRotation[0],
correctRotation[1],
correctRotation[2]);
Vec3 axis;
float angle;
referenceBody->getOrientation().getAxisAngle(axis, angle);
correctRotationAxis = axis;
correctRotationAngle = angle;
referenceBody->setPosition(correctPosition);
referenceBody->setVelocity(correctVelocity);
referenceBody->setOrientation(correctOrientation);
referenceBody->setAngularVelocity(correctAngularVelocity);
cout << "Correct Values:" << endl
<< " Position: " << correctPosition << endl
<< " Orientation: " << correctOrientation << endl
<< " RotationAngle: " << correctRotationAngle << endl
<< " RotationAxis: " << correctRotationAxis
<< endl << endl;
}
//! 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());
}
