Panda3D
 All Classes Functions Variables Enumerations
bullet_utils.cxx
00001 // Filename: bullet_utils.h
00002 // Created by:  enn0x (23Jan10)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "bullet_utils.h"
00016 
00017 #include "transformState.h"
00018 
00019 ////////////////////////////////////////////////////////////////////
00020 //     Function: LVecBase3_to_btVector3
00021 //  Description: 
00022 ////////////////////////////////////////////////////////////////////
00023 btVector3 LVecBase3_to_btVector3(const LVecBase3 &v) {
00024 
00025   return btVector3((btScalar)v.get_x(),
00026                    (btScalar)v.get_y(),
00027                    (btScalar)v.get_z());
00028 }
00029 
00030 ////////////////////////////////////////////////////////////////////
00031 //     Function: btVector3_to_LVecBase3
00032 //  Description: 
00033 ////////////////////////////////////////////////////////////////////
00034 LVecBase3 btVector3_to_LVecBase3(const btVector3 &v) {
00035 
00036   return LVecBase3((PN_stdfloat)v.getX(),
00037                    (PN_stdfloat)v.getY(),
00038                    (PN_stdfloat)v.getZ());
00039 }
00040 
00041 ////////////////////////////////////////////////////////////////////
00042 //     Function: btVector3_to_LVector3
00043 //  Description: 
00044 ////////////////////////////////////////////////////////////////////
00045 LVector3 btVector3_to_LVector3(const btVector3 &v) {
00046 
00047   return LVector3((PN_stdfloat)v.getX(),
00048                   (PN_stdfloat)v.getY(),
00049                   (PN_stdfloat)v.getZ());
00050 }
00051 
00052 ////////////////////////////////////////////////////////////////////
00053 //     Function: btVector3_to_LPoint3
00054 //  Description: 
00055 ////////////////////////////////////////////////////////////////////
00056 LPoint3 btVector3_to_LPoint3(const btVector3 &p) {
00057 
00058   return LPoint3((PN_stdfloat)p.getX(),
00059                  (PN_stdfloat)p.getY(),
00060                  (PN_stdfloat)p.getZ());
00061 }
00062 
00063 ////////////////////////////////////////////////////////////////////
00064 //     Function: LMatrix3_to_btMatrix3x3
00065 //  Description: 
00066 ////////////////////////////////////////////////////////////////////
00067 btMatrix3x3 LMatrix3_to_btMatrix3x3(const LMatrix3 &m) {
00068 
00069   btMatrix3x3 result;
00070   result.setFromOpenGLSubMatrix((const btScalar *)m.get_data());
00071   return result;
00072 }
00073 
00074 ////////////////////////////////////////////////////////////////////
00075 //     Function: btMatrix3x3_to_LMatrix3
00076 //  Description: 
00077 ////////////////////////////////////////////////////////////////////
00078 LMatrix3 btMatrix3x3_to_LMatrix3(const btMatrix3x3 &m) {
00079 
00080   btScalar cells[9];
00081   m.getOpenGLSubMatrix(cells);
00082   return LMatrix3((PN_stdfloat)cells[0], (PN_stdfloat)cells[1], (PN_stdfloat)cells[2],
00083                   (PN_stdfloat)cells[3], (PN_stdfloat)cells[4], (PN_stdfloat)cells[5],
00084                   (PN_stdfloat)cells[6], (PN_stdfloat)cells[7], (PN_stdfloat)cells[8]);
00085 }
00086 
00087 ////////////////////////////////////////////////////////////////////
00088 //     Function: LQuaternion_to_btQuat
00089 //  Description: 
00090 ////////////////////////////////////////////////////////////////////
00091 btQuaternion LQuaternion_to_btQuat(const LQuaternion &q) {
00092 
00093   return btQuaternion((btScalar)q.get_i(),
00094                       (btScalar)q.get_j(),
00095                       (btScalar)q.get_k(),
00096                       (btScalar)q.get_r());
00097 }
00098 
00099 ////////////////////////////////////////////////////////////////////
00100 //     Function: btQuat_to_LQuaternion
00101 //  Description: 
00102 ////////////////////////////////////////////////////////////////////
00103 LQuaternion btQuat_to_LQuaternion(const btQuaternion &q) {
00104 
00105   return LQuaternion((PN_stdfloat)q.getW(),
00106                      (PN_stdfloat)q.getX(),
00107                      (PN_stdfloat)q.getY(),
00108                      (PN_stdfloat)q.getZ());
00109 }
00110 
00111 ////////////////////////////////////////////////////////////////////
00112 //     Function: LMatrix4_to_btTrans
00113 //  Description: 
00114 ////////////////////////////////////////////////////////////////////
00115 btTransform LMatrix4_to_btTrans(const LMatrix4 &m) {
00116 
00117   LQuaternion quat;
00118   quat.set_from_matrix(m.get_upper_3());
00119 
00120   btQuaternion btq = LQuaternion_to_btQuat(quat);
00121   btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
00122 
00123   return btTransform(btq, btv);
00124 }
00125 
00126 ////////////////////////////////////////////////////////////////////
00127 //     Function: btTrans_to_LMatrix4
00128 //  Description: 
00129 ////////////////////////////////////////////////////////////////////
00130 LMatrix4 btTrans_to_LMatrix4(const btTransform &trans) {
00131 
00132   return TransformState::make_pos_quat_scale(
00133     btVector3_to_LVector3(trans.getOrigin()),
00134     btQuat_to_LQuaternion(trans.getRotation()),
00135     LVector3(1.0f, 1.0f, 1.0f))->get_mat();
00136 }
00137 
00138 ////////////////////////////////////////////////////////////////////
00139 //     Function: btTrans_to_TransformState
00140 //  Description: 
00141 ////////////////////////////////////////////////////////////////////
00142 CPT(TransformState) btTrans_to_TransformState(const btTransform &trans, const LVecBase3 &scale) {
00143 
00144   LVecBase3 pos = btVector3_to_LVector3(trans.getOrigin());
00145   LQuaternion quat = btQuat_to_LQuaternion(trans.getRotation());
00146 
00147   return TransformState::make_pos_quat_scale(pos, quat, scale);
00148 }
00149 
00150 ////////////////////////////////////////////////////////////////////
00151 //     Function: TransformState_to_btTrans
00152 //  Description: 
00153 ////////////////////////////////////////////////////////////////////
00154 btTransform TransformState_to_btTrans(CPT(TransformState) ts) {
00155 
00156   ts = ts->set_scale(1.0);
00157 
00158   LMatrix4 m = ts->get_mat();
00159 
00160   LQuaternion quat;
00161   quat.set_from_matrix(m.get_upper_3());
00162 
00163   btQuaternion btq = LQuaternion_to_btQuat(quat);
00164   btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
00165 
00166   return btTransform(btq, btv);
00167 }
00168 
00169 ////////////////////////////////////////////////////////////////////
00170 //     Function: get_default_up_axis
00171 //  Description: 
00172 ////////////////////////////////////////////////////////////////////
00173 BulletUpAxis get_default_up_axis() {
00174 
00175   switch (get_default_coordinate_system()) {
00176 
00177   case CS_yup_right:
00178   case CS_yup_left:
00179     return Y_up;
00180 
00181   case CS_zup_right:
00182   case CS_zup_left:
00183     return Z_up;
00184 
00185   default:
00186     return Z_up;
00187   }
00188 }
00189 
00190 ////////////////////////////////////////////////////////////////////
00191 //     Function: get_node_transform
00192 //  Description: 
00193 ////////////////////////////////////////////////////////////////////
00194 void get_node_transform(btTransform &trans, PandaNode *node) {
00195 
00196   // Get TS
00197   CPT(TransformState) ts;
00198   if (node->get_num_parents() == 0) {
00199     ts = node->get_transform();
00200   }
00201   else {
00202     NodePath np = NodePath::any_path(node);
00203     ts = np.get_net_transform();
00204   }
00205 
00206   // Remove scale from TS, since scale fudges the orientation
00207   ts = ts->set_scale(1.0);
00208 
00209   // Convert
00210   LMatrix4 m = ts->get_mat();
00211 
00212   LQuaternion quat;
00213   quat.set_from_matrix(m.get_upper_3());
00214 
00215   btQuaternion btq = LQuaternion_to_btQuat(quat);
00216   btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
00217 
00218   trans.setRotation(btq);
00219   trans.setOrigin(btv);
00220 }
00221 
 All Classes Functions Variables Enumerations