00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "bullet_utils.h"
00016
00017 #include "transformState.h"
00018
00019
00020
00021
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
00032
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
00043
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
00054
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
00065
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
00076
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
00089
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
00101
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
00113
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
00128
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
00140
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
00152
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
00171
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
00192
00193
00194 void get_node_transform(btTransform &trans, PandaNode *node) {
00195
00196
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
00207 ts = ts->set_scale(1.0);
00208
00209
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