Bullet: better performance for rigid body scene graph sync

panda/src/bullet/bulletRigidBodyNode.I

@@ -21,8 +21,8 @@
 INLINE BulletRigidBodyNode::
 ~BulletRigidBodyNode() {
 
-  delete _rigid->getMotionState();
   delete _rigid;
+  delete _motion;
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/bullet/bulletRigidBodyNode.cxx

@@ -25,17 +25,16 @@ TypeHandle BulletRigidBodyNode::_type_handle;
 BulletRigidBodyNode::
 BulletRigidBodyNode(const char *name) : BulletBodyNode(name) {
 
-  // Synchronised transform
+  // Motion state
+  _motion = new MotionState();
   _sync = TransformState::make_identity();
-  _sync_disable = false;
 
   // Mass properties
   btScalar mass(0.0);
   btVector3 inertia(0, 0, 0);
 
-  // Motion state and construction info
-  MotionState *motion = new MotionState(_sync);
-  btRigidBody::btRigidBodyConstructionInfo ci(mass, motion, _shape, inertia);
+  // construction info
+  btRigidBody::btRigidBodyConstructionInfo ci(mass, _motion, _shape, inertia);
 
   // Additional damping
   if (bullet_additional_damping) {
@@ -268,37 +267,47 @@ apply_central_impulse(const LVector3 &impulse) {
 void BulletRigidBodyNode::
 transform_changed() {
 
-  if (_sync_disable) return;
+  if (_motion->sync_disabled()) return;
 
   NodePath np = NodePath::any_path((PandaNode *)this);
   CPT(TransformState) ts = np.get_net_transform();
 
-  LMatrix4 m_sync = _sync->get_mat();
-  LMatrix4 m_ts = ts->get_mat();
-
-  if (!m_sync.almost_equal(m_ts)) {
-    _sync = ts;
-
+  // For kinematic bodies Bullet with collect the transform
+  // via Motionstate::getWorldTransform. Therefor we need to
+  // store the new transform within the motion state.
+  // For dynamic bodies we need to store the net scale within
+  // the motion state, since Bullet might update the transform
+  // via MotionState::setWorldTransform.
+  _motion->set_net_transform(ts);
+
+  // For dynamic or static bodies we directly apply the
+  // new transform.
+  if (!is_kinematic()) {
     btTransform trans = TransformState_to_btTrans(ts);
     _rigid->setCenterOfMassTransform(trans);
+  }
 
-    if (ts->has_scale()) {
-      LVecBase3 scale = ts->get_scale();
-      if (!scale.almost_equal(LVecBase3(1.0f, 1.0f, 1.0f))) {
-        for (int i=0; i<get_num_shapes(); i++) {
-          PT(BulletShape) shape = _shapes[i];
-          shape->set_local_scale(scale);
-        }
-
-        shape_changed();
+  // Rescale all shapes, but only if the new transform state
+  // has a scale
+  if (ts->has_scale()) {
+    LVecBase3 scale = ts->get_scale();
+    if (!scale.almost_equal(LVecBase3(1.0f, 1.0f, 1.0f))) {
+      for (int i=0; i<get_num_shapes(); i++) {
+        PT(BulletShape) shape = _shapes[i];
+        shape->set_local_scale(scale);
       }
-    }
 
-    // Activate the body if it has been sleeping
-    if (!_rigid->isActive()) {
-      _rigid->activate(true);
+      shape_changed();
     }
   }
+
+  // Activate the body if it has been sleeping
+  if (!_rigid->isActive()) {
+    _rigid->activate(true);
+  }
+
+  // Rememeber current transform (bullet_full_sync)
+  _sync = ts;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -309,7 +318,22 @@ transform_changed() {
 void BulletRigidBodyNode::
 sync_p2b() {
 
-  transform_changed();
+  if (is_kinematic()) {
+    transform_changed();
+  }
+
+  // Check if net transform has changed (bullet_full_sync)
+  else if (bullet_full_sync) {
+    NodePath np = NodePath::any_path((PandaNode *)this);
+    CPT(TransformState) ts = np.get_net_transform();
+
+    LMatrix4 m_sync = _sync->get_mat();
+    LMatrix4 m_ts = ts->get_mat();
+
+    if (!m_sync.almost_equal(m_ts)) {
+      transform_changed();
+    }
+  }
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -320,20 +344,11 @@ sync_p2b() {
 void BulletRigidBodyNode::
 sync_b2p() {
 
-  NodePath np = NodePath::any_path((PandaNode *)this);
-  LVecBase3 scale = np.get_net_transform()->get_scale();
-
-  btTransform trans = _rigid->getWorldTransform();
-  CPT(TransformState) ts = btTrans_to_TransformState(trans, scale);
-
-  LMatrix4 m_sync = _sync->get_mat();
-  LMatrix4 m_ts = ts->get_mat();
+  _motion->sync_b2p((PandaNode *)this);
 
-  if (!m_sync.almost_equal(m_ts)) {
-    _sync = ts;
-    _sync_disable = true;
-    np.set_transform(NodePath(), ts);
-    _sync_disable = false;
+  // Store new transform (bullet_full_sync)
+  if (bullet_full_sync) {
+    _motion->get_net_transform(_sync);
   }
 }
 
@@ -486,6 +501,20 @@ set_angular_factor(const LVector3 &factor) {
   _rigid->setAngularFactor(LVecBase3_to_btVector3(factor));
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: BulletRigidBodyNode::MotionState::Constructor
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+BulletRigidBodyNode::MotionState::
+MotionState() {
+
+  _trans.setIdentity();
+  _scale = LVecBase3(1.0f, 1.0f, 1.0f);
+  _disabled = false;
+  _dirty = false;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: BulletRigidBodyNode::MotionState::getWorldTransform
 //       Access: Public
@@ -494,7 +523,7 @@ set_angular_factor(const LVector3 &factor) {
 void BulletRigidBodyNode::MotionState::
 getWorldTransform(btTransform &trans) const {
 
-  trans = TransformState_to_btTrans(_sync);
+  trans = _trans;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -505,5 +534,65 @@ getWorldTransform(btTransform &trans) const {
 void BulletRigidBodyNode::MotionState::
 setWorldTransform(const btTransform &trans) {
 
+  _trans = trans;
+  _dirty = true;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: BulletRigidBodyNode::MotionState::sync_b2p
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+void BulletRigidBodyNode::MotionState::
+sync_b2p(PandaNode *node) {
+
+  if (!_dirty) return;
+
+  NodePath np = NodePath::any_path(node);
+  CPT(TransformState) ts = btTrans_to_TransformState(_trans, _scale);
+
+  _disabled = true;
+  np.set_transform(NodePath(), ts);
+  _disabled = false;
+  _dirty = false;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: BulletRigidBodyNode::MotionState::set_net_transform
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+void BulletRigidBodyNode::MotionState::
+set_net_transform(CPT(TransformState) &ts) {
+
+  nassertv(ts);
+
+  _trans = TransformState_to_btTrans(ts);
+
+  if (ts->has_scale()) {
+    _scale = ts->get_scale();
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: BulletRigidBodyNode::MotionState::get_net_transform
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+void BulletRigidBodyNode::MotionState::
+get_net_transform(CPT(TransformState) &ts) const {
+
+  ts = btTrans_to_TransformState(_trans, _scale);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: BulletRigidBodyNode::MotionState::sync_disabled
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+bool BulletRigidBodyNode::MotionState::
+sync_disabled() const {
+
+  return _disabled;
 }
 

panda/src/bullet/bulletRigidBodyNode.h

@@ -91,27 +91,34 @@ protected:
 private:
   virtual void shape_changed();
 
-  // The motion state is required only for kinematic bodies.
-  // For kinematic nodies getWorldTransform is called each frame, and
-  // should return the current world transform of the node.
+  // The motion state is used for syncronisation between Bullet
+  // and the Panda3D scene graph.
   class MotionState : public btMotionState {
 
   public:
-    MotionState(CPT(TransformState) & sync) : _sync(sync) {};
-    ~MotionState() {};
+    MotionState();
 
     virtual void getWorldTransform(btTransform &trans) const;
     virtual void setWorldTransform(const btTransform &trans);
 
+    void set_net_transform(CPT(TransformState) &ts);
+    void get_net_transform(CPT(TransformState) &ts) const;
+
+    void sync_b2p(PandaNode *node);
+    bool sync_disabled() const;
+
   private:
-    CPT(TransformState) &_sync;
+    btTransform _trans;
+    LVecBase3 _scale;
+    bool _disabled;
+    bool _dirty;
   };
 
-  CPT(TransformState) _sync;
-  bool _sync_disable;
-
+  MotionState *_motion;
   btRigidBody *_rigid;
 
+  CPT(TransformState) _sync; // only used with bullet_full_sync
+
 ////////////////////////////////////////////////////////////////////
 public:
   static TypeHandle get_class_type() {

panda/src/bullet/bulletWorld.cxx

@@ -987,9 +987,12 @@ needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) co
   CollideMask mask0 = node0->get_into_collide_mask();
   CollideMask mask1 = node1->get_into_collide_mask();
 
+//cout << mask0 << "   " << mask1 << endl;
+
   for (int i=0; i<32; i++) {
     if (mask0.get_bit(i)) {
       if ((_collide[i] & mask1) != 0)
+//cout << "collide: i=" << i << " _collide[i]" << _collide[i] << endl;
         return true;
     }
   }

panda/src/bullet/config_bullet.cxx

@@ -129,6 +129,13 @@ ConfigVariableDouble bullet_additional_damping_angular_threshold
 PRC_DESC("Only used when bullet-additional-damping is set to TRUE. "
          "Default value is 0.01."));
 
+ConfigVariableBool bullet_full_sync
+("bullet-full-sync", true,
+PRC_DESC("Enables optional synchronisation features like checking "
+         "for changed in a nodes net transform before each simulation "
+         "step. Disable these features for better performance. "
+         "Default value is TRUE."));
+
 ////////////////////////////////////////////////////////////////////
 //     Function: init_libbullet
 //  Description: Initializes the library. This must be called at

panda/src/bullet/config_bullet.h

@@ -40,6 +40,7 @@ extern ConfigVariableDouble bullet_additional_damping_linear_factor;
 extern ConfigVariableDouble bullet_additional_damping_angular_factor;
 extern ConfigVariableDouble bullet_additional_damping_linear_threshold;
 extern ConfigVariableDouble bullet_additional_damping_angular_threshold;
+extern ConfigVariableBool bullet_full_sync;
 
 extern EXPCL_PANDABULLET void init_libbullet();