From 5e8f1c8051bfc6eb8242fc68430915d6a7d053d0 Mon Sep 17 00:00:00 2001
From: Sebastian Eibl <sebastian.eibl@fau.de>
Date: Mon, 5 Feb 2018 10:04:48 +0100
Subject: [PATCH] added speed limiter also for angular velocity
---
src/pe/cr/HCSITS.impl.h | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/pe/cr/HCSITS.impl.h b/src/pe/cr/HCSITS.impl.h
index a67882dc3..457cd4c29 100644
--- a/src/pe/cr/HCSITS.impl.h
+++ b/src/pe/cr/HCSITS.impl.h
@@ -1810,6 +1810,13 @@ inline void HardContactSemiImplicitTimesteppingSolvers::integratePositions( Body
{
v = v * (edge * getSpeedLimitFactor() / dt / speed );
}
+
+ const real_t maxPhi = real_t(2) * math::PI * getSpeedLimitFactor();
+ const real_t phi = w.length() * dt;
+ if (phi > maxPhi)
+ {
+ w = w / phi * maxPhi;
+ }
}
// Calculating the translational displacement
@@ -1820,6 +1827,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::integratePositions( Body
// Calculating the new orientation
const Quat dq( phi, phi.length() );
+ WALBERLA_ASSERT(!math::isnan(dq), "dq: " << dq << " phi: " << phi << " w: " << w << " dt: " << dt << " angVel: " << body->getAngularVel());
body->setOrientation( dq * body->getQuaternion() );
// Storing the velocities back in the body properties
--
GitLab