Panda3D
|
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