add offset to physics simulation

we anticipate floating point error problems at distances far from origin
and we don't want the simulation to span a big space anyway
so world-frame objects are simulated in a local frame
and positions are translated back and forth as necessary

libraries/entities/src/EntityMotionState.cpp

@@ -14,6 +14,20 @@
 #include "EntityItem.h"
 #include "EntityMotionState.h"
 
+// TODO: store _cachedWorldOffset in a more central location -- VoxelTree and others also need to know about it
+// origin of physics simulation in world frame
+glm::vec3 _cachedWorldOffset(0.0f);
+
+// static 
+void EntityMotionState::setWorldOffset(const glm::vec3& offset) {
+    _cachedWorldOffset = offset;
+}
+
+// static 
+const glm::vec3& getWorldOffset() {
+    return _cachedWorldOffset;
+}
+
 EntityMotionState::EntityMotionState(EntityItem* entity) : _entity(entity) {
     assert(entity != NULL);
 }
@@ -28,10 +42,11 @@ EntityMotionState::~EntityMotionState() {
 //     it is an opportunity for outside code to update the position of the object
 void EntityMotionState::getWorldTransform (btTransform &worldTrans) const {
     btVector3 pos;
-    glmToBullet(_entity->getPosition(), pos);
+    glmToBullet(_entity->getPosition() - _cachedWorldOffset, pos);
+    worldTrans.setOrigin(pos);
+
     btQuaternion rot;
     glmToBullet(_entity->getRotation(), rot);
-    worldTrans.setOrigin(pos);
     worldTrans.setRotation(rot);
 }
 
@@ -40,7 +55,8 @@ void EntityMotionState::getWorldTransform (btTransform &worldTrans) const {
 void EntityMotionState::setWorldTransform (const btTransform &worldTrans) {
     glm::vec3 pos;
     bulletToGLM(worldTrans.getOrigin(), pos);
-    _entity->setPositionInMeters(pos);
+    _entity->setPositionInMeters(pos + _cachedWorldOffset);
+
     glm::quat rot;
     bulletToGLM(worldTrans.getRotation(), rot);
     _entity->setRotation(rot);

libraries/entities/src/EntityMotionState.h

@@ -18,6 +18,9 @@
 
 class EntityMotionState : public CustomMotionState {
 public:
+    static void setWorldOffset(const glm::vec3& offset);
+    static const glm::vec3& getWorldOffset();
+
     EntityMotionState(EntityItem* item);
     virtual ~EntityMotionState();
 

libraries/physics/src/PhysicsWorld.cpp

@@ -24,8 +24,8 @@ void PhysicsWorld::init() {
 
 bool PhysicsWorld::addVoxel(const glm::vec3& position, float scale) {
     glm::vec3 halfExtents = glm::vec3(0.5f * scale);
-    glm::vec3 center = position + halfExtents;
-    PositionHashKey key(center);
+    glm::vec3 trueCenter = position + halfExtents;
+    PositionHashKey key(trueCenter);
     VoxelObject* proxy = _voxels.find(key);
     if (!proxy) {
         // create a shape
@@ -40,11 +40,13 @@ bool PhysicsWorld::addVoxel(const glm::vec3& position, float scale) {
             object->setCollisionShape(shape);
             btTransform transform;
             transform.setIdentity();
-            transform.setOrigin(btVector3(center.x, center.y, center.z));
+            // we shift the center into the simulation's frame
+            glm::vec3 shiftedCenter = (position - _originOffset) + halfExtents;
+            transform.setOrigin(btVector3(shiftedCenter.x, shiftedCenter.y, shiftedCenter.z));
             object->setWorldTransform(transform);
 
             // add to map and world
-            _voxels.insert(key, VoxelObject(center, object));
+            _voxels.insert(key, VoxelObject(trueCenter, object));
             _dynamicsWorld->addCollisionObject(object);
             return true;
         }
@@ -54,8 +56,8 @@ bool PhysicsWorld::addVoxel(const glm::vec3& position, float scale) {
 
 bool PhysicsWorld::removeVoxel(const glm::vec3& position, float scale) {
     glm::vec3 halfExtents = glm::vec3(0.5f * scale);
-    glm::vec3 center = position + halfExtents;
-    PositionHashKey key(center);
+    glm::vec3 trueCenter = position + halfExtents;
+    PositionHashKey key(trueCenter);
     VoxelObject* proxy = _voxels.find(key);
     if (proxy) {
         // remove from world
@@ -85,8 +87,9 @@ bool PhysicsWorld::addEntity(CustomMotionState* motionState, float mass) {
         btVector3 inertia;
         shape->calculateLocalInertia(mass, inertia);
         btRigidBody* body = new btRigidBody(mass, motionState, shape, inertia);
-        _dynamicsWorld->addRigidBody(body);
         motionState->_body = body;
+        // TODO: set dynamic/kinematic/static property from data stored in motionState
+        _dynamicsWorld->addRigidBody(body);
     }
     return false;
 }

libraries/physics/src/PhysicsWorld.h

@@ -25,13 +25,22 @@
 class PhysicsWorld {
 public:
 
-    PhysicsWorld() : _collisionConfig(NULL), _collisionDispatcher(NULL), 
-        _broadphaseFilter(NULL), _constraintSolver(NULL), _dynamicsWorld(NULL) { }
+    PhysicsWorld(const glm::vec3& offset)
+        :   _collisionConfig(NULL), 
+            _collisionDispatcher(NULL), 
+            _broadphaseFilter(NULL), 
+            _constraintSolver(NULL), 
+            _dynamicsWorld(NULL),
+            _originOffset(offset),
+            _voxels() {
+    }
 
     ~PhysicsWorld();
 
     void init();
 
+    const glm::vec3& getOriginOffset() const { return _originOffset; }
+
     /// \return true if Voxel added
     /// \param position the minimum corner of the voxel
     /// \param scale the length of the voxel side
@@ -63,6 +72,7 @@ protected:
     ShapeManager _shapeManager;
 
 private:
+    glm::vec3 _originOffset;
     btHashMap<PositionHashKey, VoxelObject> _voxels;
 };