Bullet-style rotation integration for kinematics

and for dead reckoning after wire-transfer

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 = bulletRotationStep(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 = bulletRotationStep(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;

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;

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>
 

libraries/shared/src/PhysicsHelpers.cpp

@@ -1,9 +1,18 @@
 //
-//  BulletUtil.cpp
-//  libraries/physcis/src
+//  PhysicsHelpers.cpp
+//  libraries/shared/src
 //
-// The implementation of bulletRotationStep() was copied from Bullet-2.82 so we include the Bullet license here:
+//  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
@@ -22,10 +31,6 @@
  *
  * 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.
@@ -36,6 +41,7 @@ glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) {
 
     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;
     }

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 bulletRotationStep(const glm::vec3& angularVelocity, float timeStep);
+
+#endif // hifi_PhysicsHelpers_h