Handling on-floor target

Game.cpp

@@ -244,7 +244,7 @@ public:
 	float panel_width;
 	void initGui();
 };
-#define WATER
+#undef WATER
 class App : public OgreBites::ApplicationContext {
 	std::unique_ptr<Ogre::Bullet::DynamicsWorld> mDynWorld;
 	std::unique_ptr<Ogre::Bullet::DebugDrawer> mDbgDraw;
@@ -561,7 +561,7 @@ public:
 	}
 	void updateWorld(float delta)
 	{
-		mDynWorld->getBtWorld()->stepSimulation(delta, 60);
+		mDynWorld->getBtWorld()->stepSimulation(delta, 4);
 	}
 	void updateWater(float delta)
 	{

src/characters/controller.cpp

@@ -1,4 +1,5 @@
 #include <iostream>
+#include <OgreMath.h>
 #include "character.h"
 #include "controller.h"
 #if 0
@@ -554,10 +555,54 @@ struct EntityCollisionListener
     const Ogre::MovableObject* entity;
     Ogre::Bullet::CollisionListener* listener;
 };
+static bool is_on_floor = false;
+static bool penetration = false;
+static int is_on_floor_count = 0;
 void CharacterController::updateRootMotion(Ogre::Real delta)
 {
+	int i, j, k;
 	Ogre::Vector3 boneMotion = mRootBone->getPosition();
 	OgreAssert(delta > 0.0f, "Zero delta");
+	int maxPen = 0;
+	Ogre::Vector3 colNormal;
+	btVector3 currentPosition = mGhostObject->getWorldTransform().getOrigin();
+	for (i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) {
+		btManifoldArray	manifoldArray;
+		btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
+		if (collisionPair->m_algorithm)
+			collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+		for (j = 0; j < manifoldArray.size(); j++) {
+			btPersistentManifold * manifold = manifoldArray[j];
+			btScalar directionSign = manifold->getBody0() == mGhostObject ? btScalar(-1) : btScalar(1);
+			for (k = 0; k < manifold->getNumContacts(); k++) {
+				const btManifoldPoint &pt = manifold->getContactPoint(k);
+				btScalar dist = pt.getDistance();
+				if (dist < 0) {
+					maxPen = dist;
+					btVector3 touchingNormal = pt.m_normalWorldOnB;
+					colNormal = Ogre::Bullet::convert(touchingNormal);
+					if (touchingNormal.y() > 0 && (
+							touchingNormal.y() > Ogre::Math::Abs(touchingNormal.x()) ||
+							touchingNormal.y() > Ogre::Math::Abs(touchingNormal.z())))
+						is_on_floor = true;
+					penetration = true;
+				}
+				currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
+
+			}
+		}
+#if 0
+		std::cout << "overlapping: " << i << " "
+			<< manifoldArray.size()
+			<< " is_on_floor = " << is_on_floor
+			<< " position: " << Ogre::Bullet::convert(currentPosition)
+			<< " colNormal: " << colNormal
+			<< " penetration: " << penetration
+			<< " dist: " << maxPen << "\n";
+#endif
+	}
+	btTransform newTrans = mGhostObject->getWorldTransform();
+	newTrans.setOrigin(currentPosition);
 #if 0
 	Ogre::Vector3 motion = boneMotion - rootMotion;
 	if (motion.squaredLength() > 0.1f * 0.1f)
@@ -570,10 +615,21 @@ void CharacterController::updateRootMotion(Ogre::Real delta)
 	std::cout << "body mass: " << mass << "\n";
 #endif
 	/* Kinematic motion */
+	if (is_on_floor)
+		is_on_floor_count++;
+	else
+		is_on_floor_count--;
 	Ogre::Quaternion rot = mBodyNode->getOrientation();
 	Ogre::Vector3 gravity(0, -9.8, 0);
 	Ogre::Vector3 velocity = rot * boneMotion / delta;
-	velocity += gravity * 10.0f * delta;
+	if (is_on_floor_count < 0 && !penetration)
+		velocity += gravity;
+	if (is_on_floor_count < 0 && penetration)
+		velocity += gravity * delta;
+	if (is_on_floor_count < -1)
+		is_on_floor_count = -1;
+	if (is_on_floor_count > 1)
+		is_on_floor_count = 1;
 	Ogre::Vector3 rotMotion = velocity * delta;
 	btTransform from(convert(mBodyNode->getOrientation()),
 			 convert(mBodyNode->getPosition()));