From 868047e65d5c134b8952447e2aab6046dd9520a9 Mon Sep 17 00:00:00 2001
From: enn0x <ralf.pfrogner@gmx.net>
Date: Sat, 29 Oct 2011 21:45:35 +0000
Subject: [PATCH] Bullet: fix walk velocity of character controller.

---
 .../bullet/bulletCharacterControllerNode.cxx  |  9 ++++---
 .../bullet/bulletCharacterControllerNode.h    |  2 +-
 panda/src/bullet/bulletWorld.cxx              | 26 +++++++++----------
 panda/src/bullet/bulletWorld.h                |  4 +--
 4 files changed, 20 insertions(+), 21 deletions(-)

diff --git a/panda/src/bullet/bulletCharacterControllerNode.cxx b/panda/src/bullet/bulletCharacterControllerNode.cxx
index 7a67b38663..dfc361ff8f 100644
--- a/panda/src/bullet/bulletCharacterControllerNode.cxx
+++ b/panda/src/bullet/bulletCharacterControllerNode.cxx
@@ -202,7 +202,7 @@ set_angular_velocity(PN_stdfloat omega) {
 //  Description:
 ////////////////////////////////////////////////////////////////////
 void BulletCharacterControllerNode::
-sync_p2b(PN_stdfloat dt) {
+sync_p2b(PN_stdfloat dt, int num_substeps) {
 
   // Synchronise global transform
   transform_changed();
@@ -218,19 +218,20 @@ sync_p2b(PN_stdfloat dt) {
   _ghost->getWorldTransform().setBasis(m);
 
   // Linear movement
+  LVector3 vp = _linear_velocity / (btScalar)num_substeps;
+
   btVector3 v;
   if (_linear_velocity_is_local) {
     btTransform xform = _ghost->getWorldTransform();
     xform.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
-    v = xform(LVecBase3_to_btVector3(_linear_velocity));
+    v = xform(LVecBase3_to_btVector3(vp));
   }
   else {
-    v = LVecBase3_to_btVector3(_linear_velocity);
+    v = LVecBase3_to_btVector3(vp);
   }
 
   //_character->setVelocityForTimeInterval(v, dt);
   _character->setWalkDirection(v * dt);
-  //_character->setWalkDirection(v);
   _angular_velocity = 0.0f;
 }
 
diff --git a/panda/src/bullet/bulletCharacterControllerNode.h b/panda/src/bullet/bulletCharacterControllerNode.h
index c2c41c34a3..1ac8021182 100644
--- a/panda/src/bullet/bulletCharacterControllerNode.h
+++ b/panda/src/bullet/bulletCharacterControllerNode.h
@@ -69,7 +69,7 @@ public:
   INLINE btPairCachingGhostObject *get_ghost() const;
   INLINE btKinematicCharacterController *get_character() const;
 
-  void sync_p2b(PN_stdfloat dt);
+  void sync_p2b(PN_stdfloat dt, int num_substeps);
   void sync_b2p();
 
 protected:
diff --git a/panda/src/bullet/bulletWorld.cxx b/panda/src/bullet/bulletWorld.cxx
index ff7f2939bb..e9ad292bf6 100644
--- a/panda/src/bullet/bulletWorld.cxx
+++ b/panda/src/bullet/bulletWorld.cxx
@@ -19,6 +19,8 @@
 
 #include "collideMask.h"
 
+#define clamp(x, x_min, x_max) max(min(x, x_max), x_min)
+
 TypeHandle BulletWorld::_type_handle;
 
 PStatCollector BulletWorld::_pstat_physics("App:Bullet:DoPhysics");
@@ -136,29 +138,23 @@ get_gravity() const {
 //       Access: Published
 //  Description:
 ////////////////////////////////////////////////////////////////////
-void BulletWorld::
-do_physics(PN_stdfloat dt, int substeps, PN_stdfloat stepsize) {
+int BulletWorld::
+do_physics(PN_stdfloat dt, int max_substeps, PN_stdfloat stepsize) {
 
   _pstat_physics.start();
 
+  int num_substeps = clamp(int(dt / stepsize), 1, max_substeps);
+
   // Synchronize Panda to Bullet
   _pstat_p2b.start();
-  sync_p2b(dt);
+  sync_p2b(dt, num_substeps);
   _pstat_p2b.stop();
 
   // Simulation
   _pstat_simulation.start();
-  int n = _world->stepSimulation((btScalar)dt, substeps, (btScalar)stepsize);
+  int n = _world->stepSimulation((btScalar)dt, max_substeps, (btScalar)stepsize);
   _pstat_simulation.stop();
 
-  if (!n) {
-    bullet_cat.debug() << "interpolated transforms!" << endl;
-  }
-
-  if (dt > substeps * stepsize) {
-    bullet_cat.debug() << "lost simulation time!" << endl;
-  }
-
   // Synchronize Bullet to Panda
   _pstat_b2p.start();
   sync_b2p();
@@ -173,6 +169,8 @@ do_physics(PN_stdfloat dt, int substeps, PN_stdfloat stepsize) {
   }
 
   _pstat_physics.stop();
+
+  return n;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -181,7 +179,7 @@ do_physics(PN_stdfloat dt, int substeps, PN_stdfloat stepsize) {
 //  Description: 
 ////////////////////////////////////////////////////////////////////
 void BulletWorld::
-sync_p2b(PN_stdfloat dt) {
+sync_p2b(PN_stdfloat dt, int num_substeps) {
 
   for (int i=0; i < get_num_rigid_bodies(); i++) {
     get_rigid_body(i)->sync_p2b();
@@ -196,7 +194,7 @@ sync_p2b(PN_stdfloat dt) {
   }
 
   for (int i=0; i < get_num_characters(); i++) {
-    get_character(i)->sync_p2b(dt);
+    get_character(i)->sync_p2b(dt, num_substeps);
   }
 }
 
diff --git a/panda/src/bullet/bulletWorld.h b/panda/src/bullet/bulletWorld.h
index 9902dd42f3..d23caa6900 100644
--- a/panda/src/bullet/bulletWorld.h
+++ b/panda/src/bullet/bulletWorld.h
@@ -56,7 +56,7 @@ PUBLISHED:
   void set_gravity(PN_stdfloat gx, PN_stdfloat gy, PN_stdfloat gz);
   const LVector3 get_gravity() const;
 
-  void do_physics(PN_stdfloat dt, int substeps=1, PN_stdfloat stepsize=1.0f/60.0f);
+  int do_physics(PN_stdfloat dt, int max_substeps=1, PN_stdfloat stepsize=1.0f/60.0f);
 
   void set_debug_node(BulletDebugNode *node);
   void clear_debug_node();
@@ -151,7 +151,7 @@ public:
   INLINE btDispatcher *get_dispatcher() const;
 
 private:
-  void sync_p2b(PN_stdfloat dt);
+  void sync_p2b(PN_stdfloat dt, int num_substeps);
   void sync_b2p();
 
   typedef PTA(PT(BulletRigidBodyNode)) BulletRigidBodies;