diff --git a/src/pe/cr/DEM.cpp b/src/pe/cr/DEM.cpp index c6aebbc26d39bbf8c43f7f2b115637d84117aecd..e4915c755efb69c49e0d9499db4e98f5ca1f90ad 100644 --- a/src/pe/cr/DEM.cpp +++ b/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();