From 1aa92067c2f42809c3025a513a2072abf311a4ec Mon Sep 17 00:00:00 2001
From: Chris Robinson <chris.kcat@gmail.com>
Date: Mon, 19 Aug 2013 04:56:02 -0700
Subject: [PATCH] Fix tracing down

---
 apps/openmw/mwworld/physicssystem.cpp | 14 ++++-----
 libs/openengine/bullet/trace.cpp      | 42 +++++++++++++++++++++++++--
 libs/openengine/bullet/trace.h        |  2 ++
 3 files changed, 46 insertions(+), 12 deletions(-)

diff --git a/apps/openmw/mwworld/physicssystem.cpp b/apps/openmw/mwworld/physicssystem.cpp
index a4d2aa321a..c7294b4f96 100644
--- a/apps/openmw/mwworld/physicssystem.cpp
+++ b/apps/openmw/mwworld/physicssystem.cpp
@@ -92,22 +92,18 @@ namespace MWWorld
             if (!physicActor)
                 return position;
 
-            const Ogre::Vector3 halfExtents = physicActor->getHalfExtents();
-            Ogre::Vector3 newPosition = position+Ogre::Vector3(0.0f, 0.0f, halfExtents.z);
-
             const int maxHeight = 200.f;
             OEngine::Physic::ActorTracer tracer;
-            tracer.doTrace(physicActor->getCollisionBody(), newPosition, newPosition-Ogre::Vector3(0,0,maxHeight), engine);
+            tracer.findGround(physicActor->getCollisionBody(), position, position-Ogre::Vector3(0,0,maxHeight), engine);
             if(tracer.mFraction >= 1.0f)
+            {
+                physicActor->setOnGround(false);
                 return position;
+            }
 
             physicActor->setOnGround(getSlope(tracer.mPlaneNormal) <= sMaxSlope);
 
-            newPosition = tracer.mEndPos;
-            newPosition.z -= halfExtents.z;
-            newPosition.z += 2.0f;
-
-            return newPosition;
+            return tracer.mEndPos;
         }
 
         static Ogre::Vector3 move(const MWWorld::Ptr &ptr, const Ogre::Vector3 &movement, float time,
diff --git a/libs/openengine/bullet/trace.cpp b/libs/openengine/bullet/trace.cpp
index 5edf536200..afda52448e 100644
--- a/libs/openengine/bullet/trace.cpp
+++ b/libs/openengine/bullet/trace.cpp
@@ -65,9 +65,8 @@ void ActorTracer::doTrace(btCollisionObject *actor, const Ogre::Vector3 &start,
     to.setOrigin(btend);
 
     ClosestNotMeConvexResultCallback newTraceCallback(actor, btstart-btend, btScalar(0.0));
-    newTraceCallback.m_collisionFilterMask = OEngine::Physic::CollisionType_World |
-                                             OEngine::Physic::CollisionType_HeightMap |
-                                             OEngine::Physic::CollisionType_Actor;
+    newTraceCallback.m_collisionFilterMask = CollisionType_World | CollisionType_HeightMap |
+                                             CollisionType_Actor;
 
     btCollisionShape *shape = actor->getCollisionShape();
     assert(shape->isConvex());
@@ -90,5 +89,42 @@ void ActorTracer::doTrace(btCollisionObject *actor, const Ogre::Vector3 &start,
     }
 }
 
+void ActorTracer::findGround(btCollisionObject *actor, const Ogre::Vector3 &start, const Ogre::Vector3 &end, const PhysicEngine *enginePass)
+{
+    const btVector3 btstart(start.x, start.y, start.z+1.0f);
+    const btVector3 btend(end.x, end.y, end.z+1.0f);
+
+    const btTransform &trans = actor->getWorldTransform();
+    btTransform from(trans.getBasis(), btstart);
+    btTransform to(trans.getBasis(), btend);
+
+    ClosestNotMeConvexResultCallback newTraceCallback(actor, btstart-btend, btScalar(0.0));
+    newTraceCallback.m_collisionFilterMask = CollisionType_World | CollisionType_HeightMap |
+                                             CollisionType_Actor;
+
+    const btBoxShape *shape = dynamic_cast<btBoxShape*>(actor->getCollisionShape());
+    assert(shape);
+
+    btVector3 halfExtents = shape->getHalfExtentsWithMargin();
+    halfExtents[2] = 1.0f;
+    btBoxShape box(halfExtents);
+
+    enginePass->dynamicsWorld->convexSweepTest(&box, from, to, newTraceCallback);
+    if(newTraceCallback.hasHit())
+    {
+        const btVector3& tracehitnormal = newTraceCallback.m_hitNormalWorld;
+        mFraction = newTraceCallback.m_closestHitFraction;
+        mPlaneNormal = Ogre::Vector3(tracehitnormal.x(), tracehitnormal.y(), tracehitnormal.z());
+        mEndPos = (end-start)*mFraction + start;
+        mEndPos[2] -= 1.0f;
+    }
+    else
+    {
+        mEndPos = end;
+        mPlaneNormal = Ogre::Vector3(0.0f, 0.0f, 1.0f);
+        mFraction = 1.0f;
+    }
+}
+
 }
 }
diff --git a/libs/openengine/bullet/trace.h b/libs/openengine/bullet/trace.h
index f81579c2e3..92795c87fa 100644
--- a/libs/openengine/bullet/trace.h
+++ b/libs/openengine/bullet/trace.h
@@ -22,6 +22,8 @@ namespace Physic
 
         void doTrace(btCollisionObject *actor, const Ogre::Vector3 &start, const Ogre::Vector3 &end,
                      const PhysicEngine *enginePass);
+        void findGround(btCollisionObject *actor, const Ogre::Vector3 &start, const Ogre::Vector3 &end,
+                        const PhysicEngine *enginePass);
     };
 }
 }