Really revert leapfrog

src/pe/cr/DEM.cpp

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