From 82828f0b9300256345cc28538d304bb19eab635b Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Tue, 5 May 2015 13:52:10 -0700
Subject: [PATCH] print when bullet and local octree don't agree on position of
 an entity

---
 .../src/RenderableDebugableEntityItem.cpp      |  9 +++++++++
 libraries/physics/src/PhysicsEngine.cpp        | 18 ++++++++++++++++++
 libraries/physics/src/PhysicsEngine.h          |  1 +
 3 files changed, 28 insertions(+)

diff --git a/libraries/entities-renderer/src/RenderableDebugableEntityItem.cpp b/libraries/entities-renderer/src/RenderableDebugableEntityItem.cpp
index 0410d78e80..196d3c2898 100644
--- a/libraries/entities-renderer/src/RenderableDebugableEntityItem.cpp
+++ b/libraries/entities-renderer/src/RenderableDebugableEntityItem.cpp
@@ -83,4 +83,13 @@ void RenderableDebugableEntityItem::render(EntityItem* entity, RenderArgs* args)
     if (PhysicsEngine::physicsInfoIsActive(entity->getPhysicsInfo())) {
         renderHoverDot(entity, args);
     }
+
+    glm::vec3 position;
+    glm::quat rotation;
+    if (PhysicsEngine::getBodyLocation(entity->getPhysicsInfo(), position, rotation)) {
+        glm::vec3 positionOffset = glm::abs(position - entity->getPosition());
+        if (positionOffset[0] > 0.001 || positionOffset[1] > 0.001 || positionOffset[2] > 0.001) {
+            qDebug() << positionOffset[0] << positionOffset[1] << positionOffset[2];
+        }
+    }
 }
diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 3f699692c6..75cba104d2 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -418,3 +418,21 @@ bool PhysicsEngine::physicsInfoIsActive(void* physicsInfo) {
 
     return body->isActive();
 }
+
+bool PhysicsEngine::getBodyLocation(void* physicsInfo, glm::vec3& positionReturn, glm::quat& rotationReturn) {
+    if (!physicsInfo) {
+        return false;
+    }
+
+    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+    btRigidBody* body = motionState->getRigidBody();
+    if (!body) {
+        return false;
+    }
+
+    const btTransform& worldTrans = body->getCenterOfMassTransform();
+    positionReturn = bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset();
+    rotationReturn = bulletToGLM(worldTrans.getRotation());
+
+    return true;
+}
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index 9341bfb855..8b947d2510 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -93,6 +93,7 @@ public:
     void dumpNextStats() { _dumpNextStats = true; }
 
     static bool physicsInfoIsActive(void* physicsInfo);
+    static bool getBodyLocation(void* physicsInfo, glm::vec3& positionReturn, glm::quat& rotationReturn);
 
 private:
     void removeContacts(ObjectMotionState* motionState);