Panda3D
 All Classes Functions Variables Enumerations
bullet_utils.cxx
1 // Filename: bullet_utils.h
2 // Created by: enn0x (23Jan10)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "bullet_utils.h"
16 
17 #include "transformState.h"
18 
19 ////////////////////////////////////////////////////////////////////
20 // Function: LVecBase3_to_btVector3
21 // Description:
22 ////////////////////////////////////////////////////////////////////
23 btVector3 LVecBase3_to_btVector3(const LVecBase3 &v) {
24 
25  return btVector3((btScalar)v.get_x(),
26  (btScalar)v.get_y(),
27  (btScalar)v.get_z());
28 }
29 
30 ////////////////////////////////////////////////////////////////////
31 // Function: btVector3_to_LVecBase3
32 // Description:
33 ////////////////////////////////////////////////////////////////////
34 LVecBase3 btVector3_to_LVecBase3(const btVector3 &v) {
35 
36  return LVecBase3((PN_stdfloat)v.getX(),
37  (PN_stdfloat)v.getY(),
38  (PN_stdfloat)v.getZ());
39 }
40 
41 ////////////////////////////////////////////////////////////////////
42 // Function: btVector3_to_LVector3
43 // Description:
44 ////////////////////////////////////////////////////////////////////
45 LVector3 btVector3_to_LVector3(const btVector3 &v) {
46 
47  return LVector3((PN_stdfloat)v.getX(),
48  (PN_stdfloat)v.getY(),
49  (PN_stdfloat)v.getZ());
50 }
51 
52 ////////////////////////////////////////////////////////////////////
53 // Function: btVector3_to_LPoint3
54 // Description:
55 ////////////////////////////////////////////////////////////////////
56 LPoint3 btVector3_to_LPoint3(const btVector3 &p) {
57 
58  return LPoint3((PN_stdfloat)p.getX(),
59  (PN_stdfloat)p.getY(),
60  (PN_stdfloat)p.getZ());
61 }
62 
63 ////////////////////////////////////////////////////////////////////
64 // Function: LMatrix3_to_btMatrix3x3
65 // Description:
66 ////////////////////////////////////////////////////////////////////
67 btMatrix3x3 LMatrix3_to_btMatrix3x3(const LMatrix3 &m) {
68 
69  btMatrix3x3 result;
70  result.setFromOpenGLSubMatrix((const btScalar *)m.get_data());
71  return result;
72 }
73 
74 ////////////////////////////////////////////////////////////////////
75 // Function: btMatrix3x3_to_LMatrix3
76 // Description:
77 ////////////////////////////////////////////////////////////////////
78 LMatrix3 btMatrix3x3_to_LMatrix3(const btMatrix3x3 &m) {
79 
80  btScalar cells[9];
81  m.getOpenGLSubMatrix(cells);
82  return LMatrix3((PN_stdfloat)cells[0], (PN_stdfloat)cells[1], (PN_stdfloat)cells[2],
83  (PN_stdfloat)cells[3], (PN_stdfloat)cells[4], (PN_stdfloat)cells[5],
84  (PN_stdfloat)cells[6], (PN_stdfloat)cells[7], (PN_stdfloat)cells[8]);
85 }
86 
87 ////////////////////////////////////////////////////////////////////
88 // Function: LQuaternion_to_btQuat
89 // Description:
90 ////////////////////////////////////////////////////////////////////
91 btQuaternion LQuaternion_to_btQuat(const LQuaternion &q) {
92 
93  return btQuaternion((btScalar)q.get_i(),
94  (btScalar)q.get_j(),
95  (btScalar)q.get_k(),
96  (btScalar)q.get_r());
97 }
98 
99 ////////////////////////////////////////////////////////////////////
100 // Function: btQuat_to_LQuaternion
101 // Description:
102 ////////////////////////////////////////////////////////////////////
103 LQuaternion btQuat_to_LQuaternion(const btQuaternion &q) {
104 
105  return LQuaternion((PN_stdfloat)q.getW(),
106  (PN_stdfloat)q.getX(),
107  (PN_stdfloat)q.getY(),
108  (PN_stdfloat)q.getZ());
109 }
110 
111 ////////////////////////////////////////////////////////////////////
112 // Function: LMatrix4_to_btTrans
113 // Description:
114 ////////////////////////////////////////////////////////////////////
115 btTransform LMatrix4_to_btTrans(const LMatrix4 &m) {
116 
117  LQuaternion quat;
118  quat.set_from_matrix(m.get_upper_3());
119 
120  btQuaternion btq = LQuaternion_to_btQuat(quat);
121  btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
122 
123  return btTransform(btq, btv);
124 }
125 
126 ////////////////////////////////////////////////////////////////////
127 // Function: btTrans_to_LMatrix4
128 // Description:
129 ////////////////////////////////////////////////////////////////////
130 LMatrix4 btTrans_to_LMatrix4(const btTransform &trans) {
131 
132  return TransformState::make_pos_quat_scale(
133  btVector3_to_LVector3(trans.getOrigin()),
134  btQuat_to_LQuaternion(trans.getRotation()),
135  LVector3(1.0f, 1.0f, 1.0f))->get_mat();
136 }
137 
138 ////////////////////////////////////////////////////////////////////
139 // Function: btTrans_to_TransformState
140 // Description:
141 ////////////////////////////////////////////////////////////////////
142 CPT(TransformState) btTrans_to_TransformState(const btTransform &trans, const LVecBase3 &scale) {
143 
144  LVecBase3 pos = btVector3_to_LVector3(trans.getOrigin());
145  LQuaternion quat = btQuat_to_LQuaternion(trans.getRotation());
146 
147  return TransformState::make_pos_quat_scale(pos, quat, scale);
148 }
149 
150 ////////////////////////////////////////////////////////////////////
151 // Function: TransformState_to_btTrans
152 // Description:
153 ////////////////////////////////////////////////////////////////////
154 btTransform TransformState_to_btTrans(CPT(TransformState) ts) {
155 
156  ts = ts->set_scale(1.0);
157 
158  LMatrix4 m = ts->get_mat();
159 
160  LQuaternion quat;
161  quat.set_from_matrix(m.get_upper_3());
162 
163  btQuaternion btq = LQuaternion_to_btQuat(quat);
164  btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
165 
166  return btTransform(btq, btv);
167 }
168 
169 ////////////////////////////////////////////////////////////////////
170 // Function: get_default_up_axis
171 // Description:
172 ////////////////////////////////////////////////////////////////////
173 BulletUpAxis get_default_up_axis() {
174 
175  switch (get_default_coordinate_system()) {
176 
177  case CS_yup_right:
178  case CS_yup_left:
179  return Y_up;
180 
181  case CS_zup_right:
182  case CS_zup_left:
183  return Z_up;
184 
185  default:
186  return Z_up;
187  }
188 }
189 
190 ////////////////////////////////////////////////////////////////////
191 // Function: get_node_transform
192 // Description:
193 ////////////////////////////////////////////////////////////////////
194 void get_node_transform(btTransform &trans, PandaNode *node) {
195 
196  // Get TS
197  CPT(TransformState) ts;
198  if (node->get_num_parents() == 0) {
199  ts = node->get_transform();
200  }
201  else {
202  NodePath np = NodePath::any_path(node);
203  ts = np.get_net_transform();
204  }
205 
206  // Remove scale from TS, since scale fudges the orientation
207  ts = ts->set_scale(1.0);
208 
209  // Convert
210  LMatrix4 m = ts->get_mat();
211 
212  LQuaternion quat;
213  quat.set_from_matrix(m.get_upper_3());
214 
215  btQuaternion btq = LQuaternion_to_btQuat(quat);
216  btVector3 btv = LVecBase3_to_btVector3(m.get_row3(3));
217 
218  trans.setRotation(btq);
219  trans.setOrigin(btv);
220 }
221 
222 ////////////////////////////////////////////////////////////////////
223 // Function: get_bullet_version
224 // Description: Returns the version of the linked Bullet library.
225 ////////////////////////////////////////////////////////////////////
226 int get_bullet_version() {
227 
228  return BT_BULLET_VERSION;
229 }
230 
LMatrix3f get_upper_3() const
Retrieves the upper 3x3 submatrix.
Definition: lmatrix.h:1169
A basic node of the scene graph or data graph.
Definition: pandaNode.h:72
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
void set_scale(PN_stdfloat scale)
Sets the scale component of the transform, leaving translation and rotation untouched.
Definition: nodePath.I:848
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
static NodePath any_path(PandaNode *node, Thread *current_thread=Thread::get_current_thread())
Returns a new NodePath that represents any arbitrary path from the root to the indicated node...
Definition: nodePath.I:77
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
This is the base quaternion class.
Definition: lquaternion.h:96
const float * get_data() const
Returns the address of the first of the nine data elements in the matrix.
Definition: lmatrix.h:3196
void set_from_matrix(const LMatrix3f &m)
Sets the quaternion according to the rotation represented by the matrix.
LVecBase3f get_row3(int row) const
Retrieves the row column of the matrix as a 3-component vector, ignoring the last column...
Definition: lmatrix.h:1312
This is a 3-by-3 transform matrix.
Definition: lmatrix.h:110
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165