Merge pull request #4182 from AndrewMeadows/isentropic

fix bad integration of rapidly spinning objects

libraries/entities/src/EntityItem.cpp

@@ -14,6 +14,7 @@
 #include <ByteCountCoding.h>
 #include <GLMHelpers.h>
 #include <Octree.h>
+#include <PhysicsHelpers.h>
 #include <RegisteredMetaTypes.h>
 #include <SharedUtil.h> // usecTimestampNow()
 
@@ -711,10 +712,22 @@ void EntityItem::simulateKinematicMotion(float timeElapsed) {
         } else {
             // NOTE: angularSpeed is currently in degrees/sec!!!
             // TODO: Andrew to convert to radians/sec
-            float angle = timeElapsed * glm::radians(angularSpeed);
-            glm::vec3 axis = _angularVelocity / angularSpeed;
-            glm::quat  dQ = glm::angleAxis(angle, axis);
-            glm::quat rotation = glm::normalize(dQ * getRotation());
+            glm::vec3 angularVelocity = glm::radians(_angularVelocity);
+            // for improved agreement with the way Bullet integrates rotations we use an approximation 
+            // and break the integration into bullet-sized substeps 
+            glm::quat rotation = getRotation();
+            float dt = timeElapsed;
+            while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) {
+                glm::quat  dQ = computeBulletRotationStep(angularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
+                rotation = glm::normalize(dQ * rotation);
+                dt -= PHYSICS_ENGINE_FIXED_SUBSTEP;
+            }
+            // NOTE: this final partial substep can drift away from a real Bullet simulation however 
+            // it only becomes significant for rapidly rotating objects
+            // (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec).
+            glm::quat  dQ = computeBulletRotationStep(angularVelocity, dt);
+            rotation = glm::normalize(dQ * rotation);
+
             setRotation(rotation);
         }
     }

libraries/physics/src/EntityMotionState.cpp

@@ -15,6 +15,7 @@
 #include "BulletUtil.h"
 #include "EntityMotionState.h"
 #include "PhysicsEngine.h"
+#include "PhysicsHelpers.h"
 
 
 QSet<EntityItem*>* _outgoingEntityList;

libraries/physics/src/ObjectMotionState.cpp

@@ -14,6 +14,7 @@
 #include "BulletUtil.h"
 #include "ObjectMotionState.h"
 #include "PhysicsEngine.h"
+#include "PhysicsHelpers.h"
 
 const float DEFAULT_FRICTION = 0.5f;
 const float MAX_FRICTION = 10.0f;
@@ -107,6 +108,7 @@ 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;
@@ -118,7 +120,8 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     glm::vec3 wasAngularVelocity = _sentAngularVelocity;
     #endif
 
-    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();
 
@@ -170,16 +173,16 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     
     if (glm::length2(_sentAngularVelocity) > 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(computeBulletRotationStep(_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());
 
     #ifdef WANT_DEBUG

libraries/physics/src/PhysicsEngine.cpp

@@ -11,6 +11,7 @@
 
 #include "PhysicsEngine.h"
 #include "ShapeInfoUtil.h"
+#include "PhysicsHelpers.h"
 #include "ThreadSafeDynamicsWorld.h"
 
 static uint32_t _numSubsteps;
@@ -29,23 +30,27 @@ PhysicsEngine::~PhysicsEngine() {
 
 // begin EntitySimulation overrides
 void PhysicsEngine::updateEntitiesInternal(const quint64& now) {
-    // NOTE: the grand order of operations is:
-    // (1) relay incoming changes
-    // (2) step simulation
-    // (3) synchronize outgoing motion states
-    // (4) send outgoing packets
-
-    // this is step (4)
-    QSet<ObjectMotionState*>::iterator stateItr = _outgoingPackets.begin();
-    while (stateItr != _outgoingPackets.end()) {
-        ObjectMotionState* state = *stateItr;
-        if (state->doesNotNeedToSendUpdate()) {
-            stateItr = _outgoingPackets.erase(stateItr);
-        } else if (state->shouldSendUpdate(_numSubsteps)) {
-            state->sendUpdate(_entityPacketSender, _numSubsteps);
-            ++stateItr;
-        } else {
-            ++stateItr;
+    // no need to send updates unless the physics simulation has actually stepped
+    if (_lastNumSubstepsAtUpdateInternal != _numSubsteps) {
+        _lastNumSubstepsAtUpdateInternal = _numSubsteps;
+        // NOTE: the grand order of operations is:
+        // (1) relay incoming changes
+        // (2) step simulation
+        // (3) synchronize outgoing motion states
+        // (4) send outgoing packets
+    
+        // this is step (4)
+        QSet<ObjectMotionState*>::iterator stateItr = _outgoingPackets.begin();
+        while (stateItr != _outgoingPackets.end()) {
+            ObjectMotionState* state = *stateItr;
+            if (state->doesNotNeedToSendUpdate()) {
+                stateItr = _outgoingPackets.erase(stateItr);
+            } else if (state->shouldSendUpdate(_numSubsteps)) {
+                state->sendUpdate(_entityPacketSender, _numSubsteps);
+                ++stateItr;
+            } else {
+                ++stateItr;
+            }
         }
     }
 }

libraries/physics/src/PhysicsEngine.h

@@ -14,8 +14,6 @@
 
 #include <stdint.h>
 
-const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f;
-
 #include <QSet>
 #include <btBulletDynamicsCommon.h>
 
@@ -114,6 +112,7 @@ private:
 
     ContactMap _contactMap;
     uint32_t _numContactFrames = 0;
+    uint32_t _lastNumSubstepsAtUpdateInternal = 0;
 };
 
 #endif // hifi_PhysicsEngine_h

libraries/shared/src/PhysicsHelpers.cpp

@@ -0,0 +1,58 @@
+//
+//  PhysicsHelpers.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2015.01.27
+//  Unless otherwise copyrighted: Copyright 2015 High Fidelity, Inc.
+//
+//  Unless otherwise licensced: Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "PhysicsHelpers.h"
+#include "SharedUtil.h"
+
+// This chunk of code 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.
+ * */
+glm::quat computeBulletRotationStep(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
+    const float ANGULAR_MOTION_THRESHOLD = 0.5f * PI_OVER_TWO;
+    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/shared/src/PhysicsHelpers.h

@@ -0,0 +1,23 @@
+//
+//  PhysicsHelpers.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2015.01.27
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_PhysicsHelpers_h
+#define hifi_PhysicsHelpers_h
+
+#include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
+const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f;
+
+// return incremental rotation (Bullet-style) caused by angularVelocity over timeStep
+glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep);
+
+#endif // hifi_PhysicsHelpers_h