From 82d7b70ec979ee9003f3eb2b1e837f755e5f1d33 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Nov 2014 10:56:31 -0800
Subject: [PATCH] add offset to physics simulation

---
 interface/CMakeLists.txt             |  1 +
 interface/src/Application.cpp        | 25 ++++++++++++++++++++++++-
 interface/src/Application.h          |  6 ++++++
 libraries/physics/src/PhysicsWorld.h | 14 ++++++++++----
 4 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt
index eb788ac49a..f9b6fedfe1 100644
--- a/interface/CMakeLists.txt
+++ b/interface/CMakeLists.txt
@@ -38,6 +38,7 @@ endif ()
 
 # set up the external glm library
 include_glm()
+include_bullet()
 
 # create the InterfaceConfig.h file based on GL_HEADERS above
 configure_file(InterfaceConfig.h.in "${PROJECT_BINARY_DIR}/includes/InterfaceConfig.h")
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index ff4a23ceaf..df13e26eae 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -153,6 +153,9 @@ Application::Application(int& argc, char** argv, QElapsedTimer &startup_time) :
         _entityClipboardRenderer(false),
         _entityClipboard(),
         _wantToKillLocalVoxels(false),
+#ifdef USE_BULLET_PHYSICS
+        _physicsWorld(glm::vec3(0.0f)),
+#endif // USE_BULLET_PHYSICS
         _viewFrustum(),
         _lastQueriedViewFrustum(),
         _lastQueriedTime(usecTimestampNow()),
@@ -1987,7 +1990,7 @@ void Application::init() {
         &AudioDeviceScriptingInterface::muteToggled, Qt::DirectConnection);
 
     // save settings when avatar changes
-    connect(_myAvatar, &MyAvatar::transformChanged, this, &Application::bumpSettings);
+    connect(_myAvatar, &MyAvatar::transformChanged, this, &Application::updateMyAvatarTransform);
 }
 
 void Application::closeMirrorView() {
@@ -4059,6 +4062,26 @@ void Application::openUrl(const QUrl& url) {
     }
 }
 
+void Application::updateMyAvatarTransform() {
+    bumpSettings();
+#ifdef USE_BULLET_PHYSICS
+    const float SIMULATION_OFFSET_QUANTIZATION = 8.0f; // meters
+    glm::vec3 avatarPosition = _myAvatar->getPosition();
+    glm::vec3 physicsWorldOffset = _physicsWorld.getOriginOffset();
+    if (glm::distance(avatarPosition, physicsWorldOffset) > HALF_SIMULATION_EXTENT) {
+        //_entityCollisionSystem.forgetAllPhysics();
+        glm::vec3 newOriginOffset = avatarPosition;
+        int halfExtent = (int)HALF_SIMULATION_EXENT;
+        for (int i = 0; i < 3; ++i) {
+            newOriginOffset[i] = (float)(glm::max(halfExtent, 
+                    ((int)(avatarPosition[i] / SIMULATION_OFFSET_QUANTIZATION)) * (int)SIMULATION_OFFSET_QUANTIZATION));
+        }
+        _physicsWorld.setOriginOffset(newOrigin);
+        //_entityCollisionSystem.rememberAllPhysics();
+    }
+#endif // USE_BULLET_PHYSICS
+}
+
 void Application::domainSettingsReceived(const QJsonObject& domainSettingsObject) {
     
     // from the domain-handler, figure out the satoshi cost per voxel and per meter cubed
diff --git a/interface/src/Application.h b/interface/src/Application.h
index d92333058f..e9361a577a 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -35,6 +35,7 @@
 #include <NetworkPacket.h>
 #include <NodeList.h>
 #include <PacketHeaders.h>
+#include <PhysicsWorld.h>
 #include <ScriptEngine.h>
 #include <OctreeQuery.h>
 #include <ViewFrustum.h>
@@ -368,6 +369,7 @@ public slots:
     
     void openUrl(const QUrl& url);
 
+    void updateMyAvatarTransform();
     void bumpSettings() { ++_numChangedSettings; }
     
     void domainSettingsReceived(const QJsonObject& domainSettingsObject);
@@ -498,6 +500,10 @@ private:
 
     MetavoxelSystem _metavoxels;
 
+#ifdef USE_BULLET_PHYSICS
+    PhysicsWorld _physicsWorld;
+#endif // USE_BULLET_PHYSICS
+
     ViewFrustum _viewFrustum; // current state of view frustum, perspective, orientation, etc.
     ViewFrustum _lastQueriedViewFrustum; /// last view frustum used to query octree servers (voxels)
     ViewFrustum _displayViewFrustum;
diff --git a/libraries/physics/src/PhysicsWorld.h b/libraries/physics/src/PhysicsWorld.h
index 0e9fe50791..e599b4f8a8 100644
--- a/libraries/physics/src/PhysicsWorld.h
+++ b/libraries/physics/src/PhysicsWorld.h
@@ -22,6 +22,8 @@
 #include "ShapeManager.h"
 #include "VoxelObject.h"
 
+const float HALF_SIMULATION_EXTENT = 512.0f; // meters
+
 class PhysicsWorld {
 public:
 
@@ -39,24 +41,28 @@ public:
 
     void init();
 
+    /// \param offset position of simulation origin in domain-frame
+    void setOriginOffset(const glm::vec3& offset) { _originOffset = offset; }
+
+    /// \return position of simulation origin in domain-frame
     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
+    /// \return true if Voxel added
     bool addVoxel(const glm::vec3& position, float scale);
 
-    /// \return true if Voxel removed
     /// \param position the minimum corner of the voxel
     /// \param scale the length of the voxel side
+    /// \return true if Voxel removed
     bool removeVoxel(const glm::vec3& position, float scale);
 
-    /// \return true if Entity added
     /// \param info information about collision shapes to create
+    /// \return true if Entity added
     bool addEntity(CustomMotionState* motionState, float mass);
 
-    /// \return true if Entity removed
     /// \param id UUID of the entity
+    /// \return true if Entity removed
     bool removeEntity(CustomMotionState* motionState);
 
     bool updateEntityMotionType(CustomMotionState* motionState, MotionType type);