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()); }