Commit c68d4fa8 authored by Felix Winterhalter's avatar Felix Winterhalter
Browse files

Really revert leapfrog

parent 947c2f86
......@@ -245,23 +245,23 @@ void DEM::move( BodyID id, real_t dt )
// R * Iinv * R^T * torque
const Vec3 wdot( id->getInvInertia() * id->getTorque() );
// Updating the linear velocity
id->setLinearVel(id->getLinearVel() + vdot * dt);
// Updating the angular velocity
id->setAngularVel( id->getAngularVel() + wdot * dt );
// Calculating the translational displacement
id->setPosition( id->getPosition() + id->getLinearVel() * dt );
id->setPosition( id->getPosition() + id->getLinearVel() * dt + 0.5 * vdot * dt * dt );
// Calculating the rotation angle
const Vec3 phi( id->getAngularVel() * dt );
const Vec3 phi( id->getAngularVel() * dt + 0.5 * wdot * dt * dt);
// Calculating the new orientation
if (!floatIsEqual(phi.length(), 0))
id->rotate( Quat( phi, phi.length() ) );
WALBERLA_ASSERT_FLOAT_EQUAL( id->getRotation().getDeterminant(), real_c(1 ), "Corrupted rotation matrix determinant" );
// Updating the linear velocity
id->setLinearVel(id->getLinearVel() + vdot * dt);
// Updating the angular velocity
id->setAngularVel( id->getAngularVel() + wdot * dt );
// Setting the axis-aligned bounding box
id->calcBoundingBox();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment