From c23a719b26d8fd2566a85983760a0a211112487f Mon Sep 17 00:00:00 2001
From: enn0x <ralf.pfrogner@gmx.net>
Date: Sun, 7 Aug 2011 22:09:42 +0000
Subject: [PATCH] Fix for conversion from TransformState (with scale) to
 btTransform.

---
 panda/src/bullet/bullet_utils.cxx | 36 +++++++++++++++++++++++++++----
 panda/src/bullet/bullet_utils.h   |  2 ++
 2 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/panda/src/bullet/bullet_utils.cxx b/panda/src/bullet/bullet_utils.cxx
index f043b6a195..3e6df584b6 100644
--- a/panda/src/bullet/bullet_utils.cxx
+++ b/panda/src/bullet/bullet_utils.cxx
@@ -133,6 +133,28 @@ CPT(TransformState) btTrans_to_TransformState(const btTransform &trans, const LV
   return TransformState::make_pos_quat_scale(pos, quat, scale);
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: TransformState_to_btTrans
+//  Description: 
+////////////////////////////////////////////////////////////////////
+btTransform TransformState_to_btTrans(CPT(TransformState) ts) {
+
+  ts = ts->set_scale(1.0);
+
+  LMatrix4f m = ts->get_mat();
+
+  LQuaternionf quat;
+  quat.set_from_matrix(m.get_upper_3());
+
+  btQuaternion btq = LQuaternionf_to_btQuat(quat);
+  btVector3 btv = LVecBase3f_to_btVector3(m.get_row3(3));
+
+  btTransform trans;
+  trans.setRotation(btq);
+  trans.setOrigin(btv);
+  return trans;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: get_default_up_axis
 //  Description: 
@@ -160,16 +182,22 @@ BulletUpAxis get_default_up_axis() {
 ////////////////////////////////////////////////////////////////////
 void get_node_transform(btTransform &trans, PandaNode *node) {
 
-  LMatrix4f m;
-
+  // Get TS
+  CPT(TransformState) ts;
   if (node->get_num_parents() == 0) {
-    m = node->get_transform()->get_mat();
+    ts = node->get_transform();
   }
   else {
     NodePath np = NodePath::any_path(node);
-    m = np.get_net_transform()->get_mat();
+    ts = np.get_net_transform();
   }
 
+  // Remove scale from TS, since scale fudges the orientation
+  ts = ts->set_scale(1.0);
+
+  // Convert
+  LMatrix4f m = ts->get_mat();
+
   LQuaternionf quat;
   quat.set_from_matrix(m.get_upper_3());
 
diff --git a/panda/src/bullet/bullet_utils.h b/panda/src/bullet/bullet_utils.h
index fc6d3e2cd4..9d9ab12b0f 100644
--- a/panda/src/bullet/bullet_utils.h
+++ b/panda/src/bullet/bullet_utils.h
@@ -44,6 +44,8 @@ EXPCL_PANDABULLET CPT(TransformState) btTrans_to_TransformState(
   const btTransform &tf, 
   const LVecBase3f &scale=LVecBase3f(1.0f, 1.0f, 1.0f));
 
+EXPCL_PANDABULLET btTransform TransformState_to_btTrans(CPT(TransformState) ts);
+
 // UpAxis
 BEGIN_PUBLISH