Merge pull request microsoft#2473 from madratman/PR/2357

FastPhysicsEngine: body isGrounded logic fix
madratman · Mar 23, 2020 · 4cddf3f · 4cddf3f
2 parents f84fdd0 + ea73cd4
commit 4cddf3f
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp
@@ -324,10 +324,11 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);
 
         if (body.isGrounded()) {
-            // make it stick to the ground until we see body wrench force greater than gravity.
-            float normalizedForce = body_wrench.force.squaredNorm();
-            float normalizedGravity = body.getEnvironment().getState().gravity.squaredNorm();
-            if (normalizedForce >= normalizedGravity)
+            // make it stick to the ground until the magnitude of net external force on body exceeds its weight.
+            float external_force_magnitude = body_wrench.force.squaredNorm();
+            Vector3r weight = body.getMass() * body.getEnvironment().getState().gravity;
+            float weight_magnitude = weight.squaredNorm();
+            if (external_force_magnitude >= weight_magnitude)
             {
                 //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
 				body.setGrounded(false);