fix bad extrapolation of rapidly spinning objects

libraries/physics/src/BulletUtil.cpp

@@ -0,0 +1,52 @@
+//
+//  BulletUtil.cpp
+//  libraries/physcis/src
+//
+// The implementation of bulletRotationStep() was copied from Bullet-2.82 so we include the Bullet license here:
+//
+/*
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the use of this software.
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it freely, 
+ * subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. 
+ *    If you use this software in a product, an acknowledgment in the product documentation would be appreciated but 
+ *    is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * Copied and modified from LinearMath/btTransformUtil.h by AndrewMeadows on 2015.01.27.
+ * */
+
+#include <SharedUtil.h>
+#include "BulletUtil.h"
+
+glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) {
+    // Bullet uses an exponential map approximation technique to integrate rotation.
+    // The reason for this is to make it easy to compute the derivative of angular motion for various consraints.
+    // Here we reproduce the same approximation so that our extrapolations agree well with Bullet's integration.
+    //
+    // Exponential map
+    // google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
+
+    float speed = glm::length(angularVelocity);
+    // limit the angular motion because the exponential approximation fails for large steps
+    if (speed * timeStep > ANGULAR_MOTION_THRESHOLD) {
+        speed = ANGULAR_MOTION_THRESHOLD / timeStep;
+    }
+
+    glm::vec3 axis = angularVelocity;
+    if (speed < 0.001f) {
+        // use Taylor's expansions of sync function
+        axis *= (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f * speed * speed));
+    } else {
+        // sync(speed) = sin(c * speed)/t
+        axis *= (sinf(0.5f * speed * timeStep) / speed );
+    }
+    return glm::quat(cosf(0.5f * speed * timeStep), axis.x, axis.y, axis.z);
+}

libraries/physics/src/BulletUtil.h

@@ -32,4 +32,6 @@ inline btQuaternion glmToBullet(const glm::quat& g) {
     return btQuaternion(g.x, g.y, g.z, g.w);
 }
 
+glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float deltaTime);
+
 #endif // hifi_BulletUtil_h

libraries/physics/src/ObjectMotionState.cpp

@@ -107,12 +107,14 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     if (_sentFrame == 0) {
         _sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin());
         _sentVelocity = bulletToGLM(_body->getLinearVelocity());
+        _sentRotation = bulletToGLM(_body->getWorldTransform().getRotation());
         _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity());
         _sentFrame = simulationFrame;
         return false;
     }
 
-    float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+    int numFrames = simulationFrame - _sentFrame;
+    float dt = (float)(numFrames) * PHYSICS_ENGINE_FIXED_SUBSTEP;
     _sentFrame = simulationFrame;
     bool isActive = _body->isActive();
 
@@ -152,18 +154,19 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
         return true;
     }
 
-    if (glm::length2(_sentAngularVelocity) > 0.0f) {
+    float spin = glm::length(_sentAngularVelocity);
+    if (spin > 0.0f) {
         // compute rotation error
-        _sentAngularVelocity *= powf(1.0f - _angularDamping, dt);
-    
-        float spin = glm::length(_sentAngularVelocity);
-        const float MIN_SPIN = 1.0e-4f;
-        if (spin > MIN_SPIN) {
-            glm::vec3 axis = _sentAngularVelocity / spin;
-            _sentRotation = glm::normalize(glm::angleAxis(dt * spin, axis) * _sentRotation);
+        float attenuation = powf(1.0f - _angularDamping, dt);
+        _sentAngularVelocity *= attenuation;
+   
+        // Bullet caps the effective rotation velocity inside its rotation integration step, therefore 
+        // we must integrate with the same algorithm and timestep in order achieve similar results.
+        for (int i = 0; i < numFrames; ++i) {
+            _sentRotation = glm::normalize(bulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation);
         }
     }
-    const float MIN_ROTATION_DOT = 0.98f;
+    const float MIN_ROTATION_DOT = 0.99f; // 0.99 dot threshold coresponds to about 16 degrees of slop
     glm::quat actualRotation = bulletToGLM(worldTrans.getRotation());
     return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
 }