1 /*
2 * PURPOSE:
3 * Class representing an articulated rigid body. Stores the body's
4 * current state, allows forces and torques to be set, handles
5 * timestepping and implements Featherstone's algorithm.
6 *
7 * COPYRIGHT:
8 * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9 * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10
11 This software is provided 'as-is', without any express or implied warranty.
12 In no event will the authors be held liable for any damages arising from the use of this software.
13 Permission is granted to anyone to use this software for any purpose,
14 including commercial applications, and to alter it and redistribute it freely,
15 subject to the following restrictions:
16
17 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
18 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20
21 */
22
23
24 #include "btMultiBody.h"
25 #include "btMultiBodyLink.h"
26 #include "btMultiBodyLinkCollider.h"
27 #include "btMultiBodyJointFeedback.h"
28 #include "LinearMath/btTransformUtil.h"
29 #include "LinearMath/btSerializer.h"
30 // #define INCLUDE_GYRO_TERM
31
32 ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
33 bool gJointFeedbackInWorldSpace = false;
34 bool gJointFeedbackInJointFrame = false;
35
36 namespace {
37 const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
38 const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
39 }
40
41 namespace {
SpatialTransform(const btMatrix3x3 & rotation_matrix,const btVector3 & displacement,const btVector3 & top_in,const btVector3 & bottom_in,btVector3 & top_out,btVector3 & bottom_out)42 void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
43 const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
44 const btVector3 &top_in, // top part of input vector
45 const btVector3 &bottom_in, // bottom part of input vector
46 btVector3 &top_out, // top part of output vector
47 btVector3 &bottom_out) // bottom part of output vector
48 {
49 top_out = rotation_matrix * top_in;
50 bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
51 }
52
InverseSpatialTransform(const btMatrix3x3 & rotation_matrix,const btVector3 & displacement,const btVector3 & top_in,const btVector3 & bottom_in,btVector3 & top_out,btVector3 & bottom_out)53 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
54 const btVector3 &displacement,
55 const btVector3 &top_in,
56 const btVector3 &bottom_in,
57 btVector3 &top_out,
58 btVector3 &bottom_out)
59 {
60 top_out = rotation_matrix.transpose() * top_in;
61 bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
62 }
63
SpatialDotProduct(const btVector3 & a_top,const btVector3 & a_bottom,const btVector3 & b_top,const btVector3 & b_bottom)64 btScalar SpatialDotProduct(const btVector3 &a_top,
65 const btVector3 &a_bottom,
66 const btVector3 &b_top,
67 const btVector3 &b_bottom)
68 {
69 return a_bottom.dot(b_top) + a_top.dot(b_bottom);
70 }
71
SpatialCrossProduct(const btVector3 & a_top,const btVector3 & a_bottom,const btVector3 & b_top,const btVector3 & b_bottom,btVector3 & top_out,btVector3 & bottom_out)72 void SpatialCrossProduct(const btVector3 &a_top,
73 const btVector3 &a_bottom,
74 const btVector3 &b_top,
75 const btVector3 &b_bottom,
76 btVector3 &top_out,
77 btVector3 &bottom_out)
78 {
79 top_out = a_top.cross(b_top);
80 bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
81 }
82 }
83
84
85 //
86 // Implementation of class btMultiBody
87 //
88
btMultiBody(int n_links,btScalar mass,const btVector3 & inertia,bool fixedBase,bool canSleep,bool multiDof)89 btMultiBody::btMultiBody(int n_links,
90 btScalar mass,
91 const btVector3 &inertia,
92 bool fixedBase,
93 bool canSleep,
94 bool multiDof)
95 :
96 m_baseCollider(0),
97 m_baseName(0),
98 m_basePos(0,0,0),
99 m_baseQuat(0, 0, 0, 1),
100 m_baseMass(mass),
101 m_baseInertia(inertia),
102
103 m_fixedBase(fixedBase),
104 m_awake(true),
105 m_canSleep(canSleep),
106 m_sleepTimer(0),
107
108 m_linearDamping(0.04f),
109 m_angularDamping(0.04f),
110 m_useGyroTerm(true),
111 m_maxAppliedImpulse(1000.f),
112 m_maxCoordinateVelocity(100.f),
113 m_hasSelfCollision(true),
114 m_isMultiDof(multiDof),
115 __posUpdated(false),
116 m_dofCount(0),
117 m_posVarCnt(0),
118 m_useRK4(false),
119 m_useGlobalVelocities(false),
120 m_internalNeedsJointFeedback(false)
121 {
122
123 if(!m_isMultiDof)
124 {
125 m_vectorBuf.resize(2*n_links);
126 m_realBuf.resize(6 + 2*n_links);
127 m_posVarCnt = n_links;
128 }
129
130 m_links.resize(n_links);
131 m_matrixBuf.resize(n_links + 1);
132
133
134 m_baseForce.setValue(0, 0, 0);
135 m_baseTorque.setValue(0, 0, 0);
136 }
137
~btMultiBody()138 btMultiBody::~btMultiBody()
139 {
140 }
141
setupFixed(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)142 void btMultiBody::setupFixed(int i,
143 btScalar mass,
144 const btVector3 &inertia,
145 int parent,
146 const btQuaternion &rotParentToThis,
147 const btVector3 &parentComToThisPivotOffset,
148 const btVector3 &thisPivotToThisComOffset,
149 bool disableParentCollision)
150 {
151 btAssert(m_isMultiDof);
152
153 m_links[i].m_mass = mass;
154 m_links[i].m_inertiaLocal = inertia;
155 m_links[i].m_parent = parent;
156 m_links[i].m_zeroRotParentToThis = rotParentToThis;
157 m_links[i].m_dVector = thisPivotToThisComOffset;
158 m_links[i].m_eVector = parentComToThisPivotOffset;
159
160 m_links[i].m_jointType = btMultibodyLink::eFixed;
161 m_links[i].m_dofCount = 0;
162 m_links[i].m_posVarCount = 0;
163
164 if (disableParentCollision)
165 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
166 //
167 m_links[i].updateCacheMultiDof();
168
169 //
170 //if(m_isMultiDof)
171 // resizeInternalMultiDofBuffers();
172 //
173 updateLinksDofOffsets();
174
175 }
176
177
setupPrismatic(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & jointAxis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)178 void btMultiBody::setupPrismatic(int i,
179 btScalar mass,
180 const btVector3 &inertia,
181 int parent,
182 const btQuaternion &rotParentToThis,
183 const btVector3 &jointAxis,
184 const btVector3 &parentComToThisPivotOffset,
185 const btVector3 &thisPivotToThisComOffset,
186 bool disableParentCollision)
187 {
188 if(m_isMultiDof)
189 {
190 m_dofCount += 1;
191 m_posVarCnt += 1;
192 }
193
194 m_links[i].m_mass = mass;
195 m_links[i].m_inertiaLocal = inertia;
196 m_links[i].m_parent = parent;
197 m_links[i].m_zeroRotParentToThis = rotParentToThis;
198 m_links[i].setAxisTop(0, 0., 0., 0.);
199 m_links[i].setAxisBottom(0, jointAxis);
200 m_links[i].m_eVector = parentComToThisPivotOffset;
201 m_links[i].m_dVector = thisPivotToThisComOffset;
202 m_links[i].m_cachedRotParentToThis = rotParentToThis;
203
204 m_links[i].m_jointType = btMultibodyLink::ePrismatic;
205 m_links[i].m_dofCount = 1;
206 m_links[i].m_posVarCount = 1;
207 m_links[i].m_jointPos[0] = 0.f;
208 m_links[i].m_jointTorque[0] = 0.f;
209
210 if (disableParentCollision)
211 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
212 //
213 if(m_isMultiDof)
214 m_links[i].updateCacheMultiDof();
215 else
216 m_links[i].updateCache();
217 //
218 if(m_isMultiDof)
219 updateLinksDofOffsets();
220 }
221
setupRevolute(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & jointAxis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)222 void btMultiBody::setupRevolute(int i,
223 btScalar mass,
224 const btVector3 &inertia,
225 int parent,
226 const btQuaternion &rotParentToThis,
227 const btVector3 &jointAxis,
228 const btVector3 &parentComToThisPivotOffset,
229 const btVector3 &thisPivotToThisComOffset,
230 bool disableParentCollision)
231 {
232 if(m_isMultiDof)
233 {
234 m_dofCount += 1;
235 m_posVarCnt += 1;
236 }
237
238 m_links[i].m_mass = mass;
239 m_links[i].m_inertiaLocal = inertia;
240 m_links[i].m_parent = parent;
241 m_links[i].m_zeroRotParentToThis = rotParentToThis;
242 m_links[i].setAxisTop(0, jointAxis);
243 m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
244 m_links[i].m_dVector = thisPivotToThisComOffset;
245 m_links[i].m_eVector = parentComToThisPivotOffset;
246
247 m_links[i].m_jointType = btMultibodyLink::eRevolute;
248 m_links[i].m_dofCount = 1;
249 m_links[i].m_posVarCount = 1;
250 m_links[i].m_jointPos[0] = 0.f;
251 m_links[i].m_jointTorque[0] = 0.f;
252
253 if (disableParentCollision)
254 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
255 //
256 if(m_isMultiDof)
257 m_links[i].updateCacheMultiDof();
258 else
259 m_links[i].updateCache();
260 //
261 if(m_isMultiDof)
262 updateLinksDofOffsets();
263 }
264
265
266
setupSpherical(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)267 void btMultiBody::setupSpherical(int i,
268 btScalar mass,
269 const btVector3 &inertia,
270 int parent,
271 const btQuaternion &rotParentToThis,
272 const btVector3 &parentComToThisPivotOffset,
273 const btVector3 &thisPivotToThisComOffset,
274 bool disableParentCollision)
275 {
276 btAssert(m_isMultiDof);
277 m_dofCount += 3;
278 m_posVarCnt += 4;
279
280 m_links[i].m_mass = mass;
281 m_links[i].m_inertiaLocal = inertia;
282 m_links[i].m_parent = parent;
283 m_links[i].m_zeroRotParentToThis = rotParentToThis;
284 m_links[i].m_dVector = thisPivotToThisComOffset;
285 m_links[i].m_eVector = parentComToThisPivotOffset;
286
287 m_links[i].m_jointType = btMultibodyLink::eSpherical;
288 m_links[i].m_dofCount = 3;
289 m_links[i].m_posVarCount = 4;
290 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
291 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
292 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
293 m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
294 m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
295 m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
296 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
297 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
298
299
300 if (disableParentCollision)
301 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
302 //
303 m_links[i].updateCacheMultiDof();
304 //
305 updateLinksDofOffsets();
306 }
307
setupPlanar(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & rotationAxis,const btVector3 & parentComToThisComOffset,bool disableParentCollision)308 void btMultiBody::setupPlanar(int i,
309 btScalar mass,
310 const btVector3 &inertia,
311 int parent,
312 const btQuaternion &rotParentToThis,
313 const btVector3 &rotationAxis,
314 const btVector3 &parentComToThisComOffset,
315 bool disableParentCollision)
316 {
317 btAssert(m_isMultiDof);
318 m_dofCount += 3;
319 m_posVarCnt += 3;
320
321 m_links[i].m_mass = mass;
322 m_links[i].m_inertiaLocal = inertia;
323 m_links[i].m_parent = parent;
324 m_links[i].m_zeroRotParentToThis = rotParentToThis;
325 m_links[i].m_dVector.setZero();
326 m_links[i].m_eVector = parentComToThisComOffset;
327
328 //
329 static btVector3 vecNonParallelToRotAxis(1, 0, 0);
330 if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
331 vecNonParallelToRotAxis.setValue(0, 1, 0);
332 //
333
334 m_links[i].m_jointType = btMultibodyLink::ePlanar;
335 m_links[i].m_dofCount = 3;
336 m_links[i].m_posVarCount = 3;
337 btVector3 n=rotationAxis.normalized();
338 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
339 m_links[i].setAxisTop(1,0,0,0);
340 m_links[i].setAxisTop(2,0,0,0);
341 m_links[i].setAxisBottom(0,0,0,0);
342 btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
343 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
344 cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
345 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
346 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
347 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
348
349 if (disableParentCollision)
350 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
351 //
352 m_links[i].updateCacheMultiDof();
353 //
354 updateLinksDofOffsets();
355 }
356
finalizeMultiDof()357 void btMultiBody::finalizeMultiDof()
358 {
359 btAssert(m_isMultiDof);
360 m_deltaV.resize(0);
361 m_deltaV.resize(6 + m_dofCount);
362 m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
363 m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
364
365 updateLinksDofOffsets();
366 }
367
getParent(int i) const368 int btMultiBody::getParent(int i) const
369 {
370 return m_links[i].m_parent;
371 }
372
getLinkMass(int i) const373 btScalar btMultiBody::getLinkMass(int i) const
374 {
375 return m_links[i].m_mass;
376 }
377
getLinkInertia(int i) const378 const btVector3 & btMultiBody::getLinkInertia(int i) const
379 {
380 return m_links[i].m_inertiaLocal;
381 }
382
getJointPos(int i) const383 btScalar btMultiBody::getJointPos(int i) const
384 {
385 return m_links[i].m_jointPos[0];
386 }
387
getJointVel(int i) const388 btScalar btMultiBody::getJointVel(int i) const
389 {
390 return m_realBuf[6 + i];
391 }
392
getJointPosMultiDof(int i)393 btScalar * btMultiBody::getJointPosMultiDof(int i)
394 {
395 return &m_links[i].m_jointPos[0];
396 }
397
getJointVelMultiDof(int i)398 btScalar * btMultiBody::getJointVelMultiDof(int i)
399 {
400 return &m_realBuf[6 + m_links[i].m_dofOffset];
401 }
402
getJointPosMultiDof(int i) const403 const btScalar * btMultiBody::getJointPosMultiDof(int i) const
404 {
405 return &m_links[i].m_jointPos[0];
406 }
407
getJointVelMultiDof(int i) const408 const btScalar * btMultiBody::getJointVelMultiDof(int i) const
409 {
410 return &m_realBuf[6 + m_links[i].m_dofOffset];
411 }
412
413
setJointPos(int i,btScalar q)414 void btMultiBody::setJointPos(int i, btScalar q)
415 {
416 m_links[i].m_jointPos[0] = q;
417 m_links[i].updateCache();
418 }
419
setJointPosMultiDof(int i,btScalar * q)420 void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
421 {
422 for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
423 m_links[i].m_jointPos[pos] = q[pos];
424
425 m_links[i].updateCacheMultiDof();
426 }
427
setJointVel(int i,btScalar qdot)428 void btMultiBody::setJointVel(int i, btScalar qdot)
429 {
430 m_realBuf[6 + i] = qdot;
431 }
432
setJointVelMultiDof(int i,btScalar * qdot)433 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
434 {
435 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
436 m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
437 }
438
getRVector(int i) const439 const btVector3 & btMultiBody::getRVector(int i) const
440 {
441 return m_links[i].m_cachedRVector;
442 }
443
getParentToLocalRot(int i) const444 const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
445 {
446 return m_links[i].m_cachedRotParentToThis;
447 }
448
localPosToWorld(int i,const btVector3 & local_pos) const449 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
450 {
451 btVector3 result = local_pos;
452 while (i != -1) {
453 // 'result' is in frame i. transform it to frame parent(i)
454 result += getRVector(i);
455 result = quatRotate(getParentToLocalRot(i).inverse(),result);
456 i = getParent(i);
457 }
458
459 // 'result' is now in the base frame. transform it to world frame
460 result = quatRotate(getWorldToBaseRot().inverse() ,result);
461 result += getBasePos();
462
463 return result;
464 }
465
worldPosToLocal(int i,const btVector3 & world_pos) const466 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
467 {
468 if (i == -1) {
469 // world to base
470 return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
471 } else {
472 // find position in parent frame, then transform to current frame
473 return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
474 }
475 }
476
localDirToWorld(int i,const btVector3 & local_dir) const477 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
478 {
479 btVector3 result = local_dir;
480 while (i != -1) {
481 result = quatRotate(getParentToLocalRot(i).inverse() , result);
482 i = getParent(i);
483 }
484 result = quatRotate(getWorldToBaseRot().inverse() , result);
485 return result;
486 }
487
worldDirToLocal(int i,const btVector3 & world_dir) const488 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
489 {
490 if (i == -1) {
491 return quatRotate(getWorldToBaseRot(), world_dir);
492 } else {
493 return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
494 }
495 }
496
compTreeLinkVelocities(btVector3 * omega,btVector3 * vel) const497 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
498 {
499 int num_links = getNumLinks();
500 // Calculates the velocities of each link (and the base) in its local frame
501 omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
502 vel[0] = quatRotate(m_baseQuat ,getBaseVel());
503
504 for (int i = 0; i < num_links; ++i) {
505 const int parent = m_links[i].m_parent;
506
507 // transform parent vel into this frame, store in omega[i+1], vel[i+1]
508 SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
509 omega[parent+1], vel[parent+1],
510 omega[i+1], vel[i+1]);
511
512 // now add qidot * shat_i
513 omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
514 vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
515 }
516 }
517
getKineticEnergy() const518 btScalar btMultiBody::getKineticEnergy() const
519 {
520 int num_links = getNumLinks();
521 // TODO: would be better not to allocate memory here
522 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
523 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
524 compTreeLinkVelocities(&omega[0], &vel[0]);
525
526 // we will do the factor of 0.5 at the end
527 btScalar result = m_baseMass * vel[0].dot(vel[0]);
528 result += omega[0].dot(m_baseInertia * omega[0]);
529
530 for (int i = 0; i < num_links; ++i) {
531 result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
532 result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
533 }
534
535 return 0.5f * result;
536 }
537
getAngularMomentum() const538 btVector3 btMultiBody::getAngularMomentum() const
539 {
540 int num_links = getNumLinks();
541 // TODO: would be better not to allocate memory here
542 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
543 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
544 btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
545 compTreeLinkVelocities(&omega[0], &vel[0]);
546
547 rot_from_world[0] = m_baseQuat;
548 btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
549
550 for (int i = 0; i < num_links; ++i) {
551 rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
552 result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
553 }
554
555 return result;
556 }
557
clearConstraintForces()558 void btMultiBody::clearConstraintForces()
559 {
560 m_baseConstraintForce.setValue(0, 0, 0);
561 m_baseConstraintTorque.setValue(0, 0, 0);
562
563
564 for (int i = 0; i < getNumLinks(); ++i) {
565 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
566 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
567 }
568 }
clearForcesAndTorques()569 void btMultiBody::clearForcesAndTorques()
570 {
571 m_baseForce.setValue(0, 0, 0);
572 m_baseTorque.setValue(0, 0, 0);
573
574
575 for (int i = 0; i < getNumLinks(); ++i) {
576 m_links[i].m_appliedForce.setValue(0, 0, 0);
577 m_links[i].m_appliedTorque.setValue(0, 0, 0);
578 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
579 }
580 }
581
clearVelocities()582 void btMultiBody::clearVelocities()
583 {
584 for (int i = 0; i < 6 + getNumLinks(); ++i)
585 {
586 m_realBuf[i] = 0.f;
587 }
588 }
addLinkForce(int i,const btVector3 & f)589 void btMultiBody::addLinkForce(int i, const btVector3 &f)
590 {
591 m_links[i].m_appliedForce += f;
592 }
593
addLinkTorque(int i,const btVector3 & t)594 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
595 {
596 m_links[i].m_appliedTorque += t;
597 }
598
addLinkConstraintForce(int i,const btVector3 & f)599 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
600 {
601 m_links[i].m_appliedConstraintForce += f;
602 }
603
addLinkConstraintTorque(int i,const btVector3 & t)604 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
605 {
606 m_links[i].m_appliedConstraintTorque += t;
607 }
608
609
610
addJointTorque(int i,btScalar Q)611 void btMultiBody::addJointTorque(int i, btScalar Q)
612 {
613 m_links[i].m_jointTorque[0] += Q;
614 }
615
addJointTorqueMultiDof(int i,int dof,btScalar Q)616 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
617 {
618 m_links[i].m_jointTorque[dof] += Q;
619 }
620
addJointTorqueMultiDof(int i,const btScalar * Q)621 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
622 {
623 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
624 m_links[i].m_jointTorque[dof] = Q[dof];
625 }
626
getLinkForce(int i) const627 const btVector3 & btMultiBody::getLinkForce(int i) const
628 {
629 return m_links[i].m_appliedForce;
630 }
631
getLinkTorque(int i) const632 const btVector3 & btMultiBody::getLinkTorque(int i) const
633 {
634 return m_links[i].m_appliedTorque;
635 }
636
getJointTorque(int i) const637 btScalar btMultiBody::getJointTorque(int i) const
638 {
639 return m_links[i].m_jointTorque[0];
640 }
641
getJointTorqueMultiDof(int i)642 btScalar * btMultiBody::getJointTorqueMultiDof(int i)
643 {
644 return &m_links[i].m_jointTorque[0];
645 }
646
outerProduct(const btVector3 & v0,const btVector3 & v1)647 inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
648 {
649 btVector3 row0 = btVector3(
650 v0.x() * v1.x(),
651 v0.x() * v1.y(),
652 v0.x() * v1.z());
653 btVector3 row1 = btVector3(
654 v0.y() * v1.x(),
655 v0.y() * v1.y(),
656 v0.y() * v1.z());
657 btVector3 row2 = btVector3(
658 v0.z() * v1.x(),
659 v0.z() * v1.y(),
660 v0.z() * v1.z());
661
662 btMatrix3x3 m(row0[0],row0[1],row0[2],
663 row1[0],row1[1],row1[2],
664 row2[0],row2[1],row2[2]);
665 return m;
666 }
667
668 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
669 //
670
stepVelocitiesMultiDof(btScalar dt,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m,bool isConstraintPass)671 void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
672 btAlignedObjectArray<btScalar> &scratch_r,
673 btAlignedObjectArray<btVector3> &scratch_v,
674 btAlignedObjectArray<btMatrix3x3> &scratch_m,
675 bool isConstraintPass)
676 {
677 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
678 // and the base linear & angular accelerations.
679
680 // We apply damping forces in this routine as well as any external forces specified by the
681 // caller (via addBaseForce etc).
682
683 // output should point to an array of 6 + num_links reals.
684 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
685 // num_links joint acceleration values.
686
687 m_internalNeedsJointFeedback = false;
688
689 int num_links = getNumLinks();
690
691 const btScalar DAMPING_K1_LINEAR = m_linearDamping;
692 const btScalar DAMPING_K2_LINEAR = m_linearDamping;
693
694 const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
695 const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
696
697 btVector3 base_vel = getBaseVel();
698 btVector3 base_omega = getBaseOmega();
699
700 // Temporary matrices/vectors -- use scratch space from caller
701 // so that we don't have to keep reallocating every frame
702
703 scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
704 scratch_v.resize(8*num_links + 6);
705 scratch_m.resize(4*num_links + 4);
706
707 //btScalar * r_ptr = &scratch_r[0];
708 btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
709 btVector3 * v_ptr = &scratch_v[0];
710
711 // vhat_i (top = angular, bottom = linear part)
712 btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
713 v_ptr += num_links * 2 + 2;
714 //
715 // zhat_i^A
716 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
717 v_ptr += num_links * 2 + 2;
718 //
719 // chat_i (note NOT defined for the base)
720 btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
721 v_ptr += num_links * 2;
722 //
723 // Ihat_i^A.
724 btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
725
726 // Cached 3x3 rotation matrices from parent frame to this frame.
727 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
728 btMatrix3x3 * rot_from_world = &scratch_m[0];
729
730 // hhat_i, ahat_i
731 // hhat is NOT stored for the base (but ahat is)
732 btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
733 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
734 v_ptr += num_links * 2 + 2;
735 //
736 // Y_i, invD_i
737 btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
738 btScalar * Y = &scratch_r[0];
739 //
740 //aux variables
741 static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
742 static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
743 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
744 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
745 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
746 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
747 static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
748 static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
749 static btSpatialTransformationMatrix fromWorld;
750 fromWorld.m_trnVec.setZero();
751 /////////////////
752
753 // ptr to the joint accel part of the output
754 btScalar * joint_accel = output + 6;
755
756 // Start of the algorithm proper.
757
758 // First 'upward' loop.
759 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
760
761 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
762
763 //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
764 spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
765
766 if (m_fixedBase)
767 {
768 zeroAccSpatFrc[0].setZero();
769 }
770 else
771 {
772 btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
773 btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
774 //external forces
775 zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
776
777 //adding damping terms (only)
778 btScalar linDampMult = 1., angDampMult = 1.;
779 zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
780 linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
781
782 //
783 //p += vhat x Ihat vhat - done in a simpler way
784 if (m_useGyroTerm)
785 zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
786 //
787 zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
788 }
789
790
791 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
792 spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
793 //
794 btMatrix3x3(m_baseMass, 0, 0,
795 0, m_baseMass, 0,
796 0, 0, m_baseMass),
797 //
798 btMatrix3x3(m_baseInertia[0], 0, 0,
799 0, m_baseInertia[1], 0,
800 0, 0, m_baseInertia[2])
801 );
802
803 rot_from_world[0] = rot_from_parent[0];
804
805 //
806 for (int i = 0; i < num_links; ++i) {
807 const int parent = m_links[i].m_parent;
808 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
809 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
810
811 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
812 fromWorld.m_rotMat = rot_from_world[i+1];
813 fromParent.transform(spatVel[parent+1], spatVel[i+1]);
814
815 // now set vhat_i to its true value by doing
816 // vhat_i += qidot * shat_i
817 if(!m_useGlobalVelocities)
818 {
819 spatJointVel.setZero();
820
821 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
822 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
823
824 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
825 spatVel[i+1] += spatJointVel;
826
827 //
828 // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
829 //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
830
831 }
832 else
833 {
834 fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
835 fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
836 }
837
838 // we can now calculate chat_i
839 spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
840
841 // calculate zhat_i^A
842 //
843 //external forces
844 btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
845 btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
846
847 zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
848
849 #if 0
850 {
851
852 b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
853 i+1,
854 zeroAccSpatFrc[i+1].m_topVec[0],
855 zeroAccSpatFrc[i+1].m_topVec[1],
856 zeroAccSpatFrc[i+1].m_topVec[2],
857
858 zeroAccSpatFrc[i+1].m_bottomVec[0],
859 zeroAccSpatFrc[i+1].m_bottomVec[1],
860 zeroAccSpatFrc[i+1].m_bottomVec[2]);
861 }
862 #endif
863 //
864 //adding damping terms (only)
865 btScalar linDampMult = 1., angDampMult = 1.;
866 zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
867 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
868
869 // calculate Ihat_i^A
870 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
871 spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
872 //
873 btMatrix3x3(m_links[i].m_mass, 0, 0,
874 0, m_links[i].m_mass, 0,
875 0, 0, m_links[i].m_mass),
876 //
877 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
878 0, m_links[i].m_inertiaLocal[1], 0,
879 0, 0, m_links[i].m_inertiaLocal[2])
880 );
881 //
882 //p += vhat x Ihat vhat - done in a simpler way
883 if(m_useGyroTerm)
884 zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
885 //
886 zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
887 //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
888 ////clamp parent's omega
889 //btScalar parOmegaMod = temp.length();
890 //btScalar parOmegaModMax = 1000;
891 //if(parOmegaMod > parOmegaModMax)
892 // temp *= parOmegaModMax / parOmegaMod;
893 //zeroAccSpatFrc[i+1].addLinear(temp);
894 //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
895 //temp = spatCoriolisAcc[i].getLinear();
896 //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
897
898
899
900 //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
901 //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
902 //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
903 }
904
905 // 'Downward' loop.
906 // (part of TreeForwardDynamics in Mirtich.)
907 for (int i = num_links - 1; i >= 0; --i)
908 {
909 const int parent = m_links[i].m_parent;
910 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
911
912 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
913 {
914 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
915 //
916 hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
917 //
918 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
919 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
920 - spatCoriolisAcc[i].dot(hDof)
921 ;
922 }
923
924 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
925 {
926 btScalar *D_row = &D[dof * m_links[i].m_dofCount];
927 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
928 {
929 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
930 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
931 }
932 }
933
934 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
935 switch(m_links[i].m_jointType)
936 {
937 case btMultibodyLink::ePrismatic:
938 case btMultibodyLink::eRevolute:
939 {
940 invDi[0] = 1.0f / D[0];
941 break;
942 }
943 case btMultibodyLink::eSpherical:
944 case btMultibodyLink::ePlanar:
945 {
946 static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
947 static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
948
949 //unroll the loop?
950 for(int row = 0; row < 3; ++row)
951 {
952 for(int col = 0; col < 3; ++col)
953 {
954 invDi[row * 3 + col] = invD3x3[row][col];
955 }
956 }
957
958 break;
959 }
960 default:
961 {
962
963 }
964 }
965
966 //determine h*D^{-1}
967 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
968 {
969 spatForceVecTemps[dof].setZero();
970
971 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
972 {
973 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
974 //
975 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
976 }
977 }
978
979 dyadTemp = spatInertia[i+1];
980
981 //determine (h*D^{-1}) * h^{T}
982 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
983 {
984 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
985 //
986 dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
987 }
988
989 fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
990
991 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
992 {
993 invD_times_Y[dof] = 0.f;
994
995 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
996 {
997 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
998 }
999 }
1000
1001 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1002
1003 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1004 {
1005 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1006 //
1007 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1008 }
1009
1010 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1011
1012 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1013 }
1014
1015
1016 // Second 'upward' loop
1017 // (part of TreeForwardDynamics in Mirtich)
1018
1019 if (m_fixedBase)
1020 {
1021 spatAcc[0].setZero();
1022 }
1023 else
1024 {
1025 if (num_links > 0)
1026 {
1027 m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1028 m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1029 m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1030 m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
1031
1032 }
1033
1034 solveImatrix(zeroAccSpatFrc[0], result);
1035 spatAcc[0] = -result;
1036 }
1037
1038
1039 // now do the loop over the m_links
1040 for (int i = 0; i < num_links; ++i)
1041 {
1042 // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1043 // a = apar + cor + Sqdd
1044 //or
1045 // qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1046 // a = apar + Sqdd
1047
1048 const int parent = m_links[i].m_parent;
1049 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1050
1051 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1052
1053 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1054 {
1055 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1056 //
1057 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1058 }
1059
1060 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1061 //D^{-1} * (Y - h^{T}*apar)
1062 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1063
1064 spatAcc[i+1] += spatCoriolisAcc[i];
1065
1066 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1067 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1068
1069 if (m_links[i].m_jointFeedback)
1070 {
1071 m_internalNeedsJointFeedback = true;
1072
1073 btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1074 btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1075
1076 if (gJointFeedbackInJointFrame)
1077 {
1078 //shift the reaction forces to the joint frame
1079 //linear (force) component is the same
1080 //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
1081 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1082 }
1083
1084
1085 if (gJointFeedbackInWorldSpace)
1086 {
1087 if (isConstraintPass)
1088 {
1089 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1090 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1091 } else
1092 {
1093 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1094 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1095 }
1096 } else
1097 {
1098 if (isConstraintPass)
1099 {
1100 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1101 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1102
1103 }
1104 else
1105 {
1106 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1107 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1108 }
1109 }
1110 }
1111
1112 }
1113
1114 // transform base accelerations back to the world frame.
1115 btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1116 output[0] = omegadot_out[0];
1117 output[1] = omegadot_out[1];
1118 output[2] = omegadot_out[2];
1119
1120 btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1121 output[3] = vdot_out[0];
1122 output[4] = vdot_out[1];
1123 output[5] = vdot_out[2];
1124
1125 /////////////////
1126 //printf("q = [");
1127 //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1128 //for(int link = 0; link < getNumLinks(); ++link)
1129 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1130 // printf("%.6f ", m_links[link].m_jointPos[dof]);
1131 //printf("]\n");
1132 ////
1133 //printf("qd = [");
1134 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1135 // printf("%.6f ", m_realBuf[dof]);
1136 //printf("]\n");
1137 //printf("qdd = [");
1138 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1139 // printf("%.6f ", output[dof]);
1140 //printf("]\n");
1141 /////////////////
1142
1143 // Final step: add the accelerations (times dt) to the velocities.
1144
1145 if (!isConstraintPass)
1146 {
1147 if(dt > 0.)
1148 applyDeltaVeeMultiDof(output, dt);
1149
1150 }
1151 /////
1152 //btScalar angularThres = 1;
1153 //btScalar maxAngVel = 0.;
1154 //bool scaleDown = 1.;
1155 //for(int link = 0; link < m_links.size(); ++link)
1156 //{
1157 // if(spatVel[link+1].getAngular().length() > maxAngVel)
1158 // {
1159 // maxAngVel = spatVel[link+1].getAngular().length();
1160 // scaleDown = angularThres / spatVel[link+1].getAngular().length();
1161 // break;
1162 // }
1163 //}
1164
1165 //if(scaleDown != 1.)
1166 //{
1167 // for(int link = 0; link < m_links.size(); ++link)
1168 // {
1169 // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1170 // {
1171 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1172 // getJointVelMultiDof(link)[dof] *= scaleDown;
1173 // }
1174 // }
1175 //}
1176 /////
1177
1178 /////////////////////
1179 if(m_useGlobalVelocities)
1180 {
1181 for (int i = 0; i < num_links; ++i)
1182 {
1183 const int parent = m_links[i].m_parent;
1184 //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
1185 //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
1186
1187 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1188 fromWorld.m_rotMat = rot_from_world[i+1];
1189
1190 // vhat_i = i_xhat_p(i) * vhat_p(i)
1191 fromParent.transform(spatVel[parent+1], spatVel[i+1]);
1192 //nice alternative below (using operator *) but it generates temps
1193 /////////////////////////////////////////////////////////////
1194
1195 // now set vhat_i to its true value by doing
1196 // vhat_i += qidot * shat_i
1197 spatJointVel.setZero();
1198
1199 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1200 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1201
1202 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1203 spatVel[i+1] += spatJointVel;
1204
1205
1206 fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
1207 fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1208 }
1209 }
1210
1211 }
1212
1213
stepVelocities(btScalar dt,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)1214 void btMultiBody::stepVelocities(btScalar dt,
1215 btAlignedObjectArray<btScalar> &scratch_r,
1216 btAlignedObjectArray<btVector3> &scratch_v,
1217 btAlignedObjectArray<btMatrix3x3> &scratch_m)
1218 {
1219 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
1220 // and the base linear & angular accelerations.
1221
1222 // We apply damping forces in this routine as well as any external forces specified by the
1223 // caller (via addBaseForce etc).
1224
1225 // output should point to an array of 6 + num_links reals.
1226 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
1227 // num_links joint acceleration values.
1228
1229 int num_links = getNumLinks();
1230
1231 const btScalar DAMPING_K1_LINEAR = m_linearDamping;
1232 const btScalar DAMPING_K2_LINEAR = m_linearDamping;
1233
1234 const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
1235 const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
1236
1237 btVector3 base_vel = getBaseVel();
1238 btVector3 base_omega = getBaseOmega();
1239
1240 // Temporary matrices/vectors -- use scratch space from caller
1241 // so that we don't have to keep reallocating every frame
1242
1243 scratch_r.resize(2*num_links + 6);
1244 scratch_v.resize(8*num_links + 6);
1245 scratch_m.resize(4*num_links + 4);
1246
1247 btScalar * r_ptr = &scratch_r[0];
1248 btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
1249 btVector3 * v_ptr = &scratch_v[0];
1250
1251 // vhat_i (top = angular, bottom = linear part)
1252 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
1253 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
1254
1255 // zhat_i^A
1256 btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1257 btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1258
1259 // chat_i (note NOT defined for the base)
1260 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
1261 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
1262
1263 // top left, top right and bottom left blocks of Ihat_i^A.
1264 // bottom right block = transpose of top left block and is not stored.
1265 // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
1266 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
1267 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
1268 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
1269
1270 // Cached 3x3 rotation matrices from parent frame to this frame.
1271 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1272 btMatrix3x3 * rot_from_world = &scratch_m[0];
1273
1274 // hhat_i, ahat_i
1275 // hhat is NOT stored for the base (but ahat is)
1276 btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
1277 btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
1278 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1279 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1280
1281 // Y_i, D_i
1282 btScalar * Y1 = r_ptr; r_ptr += num_links;
1283 btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
1284
1285 // ptr to the joint accel part of the output
1286 btScalar * joint_accel = output + 6;
1287
1288
1289 // Start of the algorithm proper.
1290
1291 // First 'upward' loop.
1292 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1293
1294 rot_from_parent[0] = btMatrix3x3(m_baseQuat);
1295
1296 vel_top_angular[0] = rot_from_parent[0] * base_omega;
1297 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
1298
1299 if (m_fixedBase) {
1300 f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
1301 } else {
1302 f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
1303 - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
1304
1305 f_zero_acc_bottom_linear[0] =
1306 - (rot_from_parent[0] * m_baseTorque);
1307
1308 if (m_useGyroTerm)
1309 f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
1310
1311 f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
1312
1313 }
1314
1315
1316
1317 inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1318
1319
1320 inertia_top_right[0].setValue(m_baseMass, 0, 0,
1321 0, m_baseMass, 0,
1322 0, 0, m_baseMass);
1323 inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
1324 0, m_baseInertia[1], 0,
1325 0, 0, m_baseInertia[2]);
1326
1327 rot_from_world[0] = rot_from_parent[0];
1328
1329 for (int i = 0; i < num_links; ++i) {
1330 const int parent = m_links[i].m_parent;
1331 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1332
1333
1334 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1335
1336 // vhat_i = i_xhat_p(i) * vhat_p(i)
1337 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1338 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
1339 vel_top_angular[i+1], vel_bottom_linear[i+1]);
1340
1341 // we can now calculate chat_i
1342 // remember vhat_i is really vhat_p(i) (but in current frame) at this point
1343 coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
1344 + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
1345 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
1346 coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
1347 coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
1348 } else {
1349 coriolis_top_angular[i] = btVector3(0,0,0);
1350 }
1351
1352 // now set vhat_i to its true value by doing
1353 // vhat_i += qidot * shat_i
1354 vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
1355 vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
1356
1357 // calculate zhat_i^A
1358 f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
1359 f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
1360
1361 f_zero_acc_bottom_linear[i+1] =
1362 - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
1363 if (m_useGyroTerm)
1364 {
1365 f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
1366 }
1367
1368 f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
1369
1370 // calculate Ihat_i^A
1371 inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1372 inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
1373 0, m_links[i].m_mass, 0,
1374 0, 0, m_links[i].m_mass);
1375 inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
1376 0, m_links[i].m_inertiaLocal[1], 0,
1377 0, 0, m_links[i].m_inertiaLocal[2]);
1378 }
1379
1380
1381 // 'Downward' loop.
1382 // (part of TreeForwardDynamics in Mirtich.)
1383 for (int i = num_links - 1; i >= 0; --i) {
1384
1385 h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
1386 h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
1387 btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
1388 D[i] = val;
1389 //Y1 = joint torque - zero acceleration force - coriolis force
1390 Y1[i] = m_links[i].m_jointTorque[0]
1391 - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
1392 - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
1393
1394 const int parent = m_links[i].m_parent;
1395
1396 btAssert(D[i]!=0.f);
1397 // Ip += pXi * (Ii - hi hi' / Di) * iXp
1398 const btScalar one_over_di = 1.0f / D[i];
1399
1400
1401
1402
1403 const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
1404 const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
1405 const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
1406
1407
1408 btMatrix3x3 r_cross;
1409 r_cross.setValue(
1410 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
1411 m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
1412 -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
1413
1414 inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
1415 inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
1416 inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
1417 (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
1418
1419
1420 // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
1421 btVector3 in_top, in_bottom, out_top, out_bottom;
1422 const btScalar Y_over_D = Y1[i] * one_over_di;
1423 in_top = f_zero_acc_top_angular[i+1]
1424 + inertia_top_left[i+1] * coriolis_top_angular[i]
1425 + inertia_top_right[i+1] * coriolis_bottom_linear[i]
1426 + Y_over_D * h_top[i];
1427 in_bottom = f_zero_acc_bottom_linear[i+1]
1428 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
1429 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
1430 + Y_over_D * h_bottom[i];
1431 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1432 in_top, in_bottom, out_top, out_bottom);
1433 f_zero_acc_top_angular[parent+1] += out_top;
1434 f_zero_acc_bottom_linear[parent+1] += out_bottom;
1435 }
1436
1437
1438 // Second 'upward' loop
1439 // (part of TreeForwardDynamics in Mirtich)
1440
1441 if (m_fixedBase)
1442 {
1443 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
1444 }
1445 else
1446 {
1447 if (num_links > 0)
1448 {
1449 //Matrix<btScalar, 6, 6> Imatrix;
1450 //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
1451 //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
1452 //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
1453 //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
1454 //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
1455
1456 m_cachedInertiaTopLeft = inertia_top_left[0];
1457 m_cachedInertiaTopRight = inertia_top_right[0];
1458 m_cachedInertiaLowerLeft = inertia_bottom_left[0];
1459 m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
1460
1461 }
1462 btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
1463 btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
1464 float result[6];
1465
1466 solveImatrix(rhs_top, rhs_bot, result);
1467 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
1468 for (int i = 0; i < 3; ++i) {
1469 accel_top[0][i] = -result[i];
1470 accel_bottom[0][i] = -result[i+3];
1471 }
1472
1473 }
1474
1475 // now do the loop over the m_links
1476 for (int i = 0; i < num_links; ++i)
1477 {
1478 //acceleration of the child link = acceleration of the parent transformed into child frame +
1479 // + coriolis acceleration
1480 // + joint acceleration
1481 const int parent = m_links[i].m_parent;
1482 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1483 accel_top[parent+1], accel_bottom[parent+1],
1484 accel_top[i+1], accel_bottom[i+1]);
1485
1486
1487 joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
1488 accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
1489 accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
1490 }
1491
1492 // transform base accelerations back to the world frame.
1493 btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1494 output[0] = omegadot_out[0];
1495 output[1] = omegadot_out[1];
1496 output[2] = omegadot_out[2];
1497
1498 btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1499 output[3] = vdot_out[0];
1500 output[4] = vdot_out[1];
1501 output[5] = vdot_out[2];
1502
1503 /////////////////
1504 //printf("q = [");
1505 //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1506 //for(int link = 0; link < getNumLinks(); ++link)
1507 // printf("%.2f ", m_links[link].m_jointPos[0]);
1508 //printf("]\n");
1509 ////
1510 //printf("qd = [");
1511 //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1512 // printf("%.2f ", m_realBuf[dof]);
1513 //printf("]\n");
1514 ////
1515 //printf("qdd = [");
1516 //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1517 // printf("%.2f ", output[dof]);
1518 //printf("]\n");
1519 /////////////////
1520
1521 // Final step: add the accelerations (times dt) to the velocities.
1522 //todo: do we already need to update the velocities, or can we move this into the constraint solver?
1523 //the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
1524 applyDeltaVee(output, dt);
1525
1526
1527 }
1528
solveImatrix(const btVector3 & rhs_top,const btVector3 & rhs_bot,float result[6]) const1529 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
1530 {
1531 int num_links = getNumLinks();
1532 ///solve I * x = rhs, so the result = invI * rhs
1533 if (num_links == 0)
1534 {
1535 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1536 result[0] = rhs_bot[0] / m_baseInertia[0];
1537 result[1] = rhs_bot[1] / m_baseInertia[1];
1538 result[2] = rhs_bot[2] / m_baseInertia[2];
1539 result[3] = rhs_top[0] / m_baseMass;
1540 result[4] = rhs_top[1] / m_baseMass;
1541 result[5] = rhs_top[2] / m_baseMass;
1542 } else
1543 {
1544 /// Special routine for calculating the inverse of a spatial inertia matrix
1545 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1546 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1547 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1548 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1549 tmp = invIupper_right * m_cachedInertiaLowerRight;
1550 btMatrix3x3 invI_upper_left = (tmp * Binv);
1551 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1552 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1553 tmp[0][0]-= 1.0;
1554 tmp[1][1]-= 1.0;
1555 tmp[2][2]-= 1.0;
1556 btMatrix3x3 invI_lower_left = (Binv * tmp);
1557
1558 //multiply result = invI * rhs
1559 {
1560 btVector3 vtop = invI_upper_left*rhs_top;
1561 btVector3 tmp;
1562 tmp = invIupper_right * rhs_bot;
1563 vtop += tmp;
1564 btVector3 vbot = invI_lower_left*rhs_top;
1565 tmp = invI_lower_right * rhs_bot;
1566 vbot += tmp;
1567 result[0] = vtop[0];
1568 result[1] = vtop[1];
1569 result[2] = vtop[2];
1570 result[3] = vbot[0];
1571 result[4] = vbot[1];
1572 result[5] = vbot[2];
1573 }
1574
1575 }
1576 }
solveImatrix(const btSpatialForceVector & rhs,btSpatialMotionVector & result) const1577 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
1578 {
1579 int num_links = getNumLinks();
1580 ///solve I * x = rhs, so the result = invI * rhs
1581 if (num_links == 0)
1582 {
1583 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1584 result.setAngular(rhs.getAngular() / m_baseInertia);
1585 result.setLinear(rhs.getLinear() / m_baseMass);
1586 } else
1587 {
1588 /// Special routine for calculating the inverse of a spatial inertia matrix
1589 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1590 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1591 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1592 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1593 tmp = invIupper_right * m_cachedInertiaLowerRight;
1594 btMatrix3x3 invI_upper_left = (tmp * Binv);
1595 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1596 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1597 tmp[0][0]-= 1.0;
1598 tmp[1][1]-= 1.0;
1599 tmp[2][2]-= 1.0;
1600 btMatrix3x3 invI_lower_left = (Binv * tmp);
1601
1602 //multiply result = invI * rhs
1603 {
1604 btVector3 vtop = invI_upper_left*rhs.getLinear();
1605 btVector3 tmp;
1606 tmp = invIupper_right * rhs.getAngular();
1607 vtop += tmp;
1608 btVector3 vbot = invI_lower_left*rhs.getLinear();
1609 tmp = invI_lower_right * rhs.getAngular();
1610 vbot += tmp;
1611 result.setVector(vtop, vbot);
1612 }
1613
1614 }
1615 }
1616
mulMatrix(btScalar * pA,btScalar * pB,int rowsA,int colsA,int rowsB,int colsB,btScalar * pC) const1617 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1618 {
1619 for (int row = 0; row < rowsA; row++)
1620 {
1621 for (int col = 0; col < colsB; col++)
1622 {
1623 pC[row * colsB + col] = 0.f;
1624 for (int inner = 0; inner < rowsB; inner++)
1625 {
1626 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1627 }
1628 }
1629 }
1630 }
1631
calcAccelerationDeltasMultiDof(const btScalar * force,btScalar * output,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v) const1632 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
1633 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
1634 {
1635 // Temporary matrices/vectors -- use scratch space from caller
1636 // so that we don't have to keep reallocating every frame
1637
1638
1639 int num_links = getNumLinks();
1640 scratch_r.resize(m_dofCount);
1641 scratch_v.resize(4*num_links + 4);
1642
1643 btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
1644 btVector3 * v_ptr = &scratch_v[0];
1645
1646 // zhat_i^A (scratch space)
1647 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1648 v_ptr += num_links * 2 + 2;
1649
1650 // rot_from_parent (cached from calcAccelerations)
1651 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1652
1653 // hhat (cached), accel (scratch)
1654 // hhat is NOT stored for the base (but ahat is)
1655 const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1656 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
1657 v_ptr += num_links * 2 + 2;
1658
1659 // Y_i (scratch), invD_i (cached)
1660 const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1661 btScalar * Y = r_ptr;
1662 ////////////////
1663 //aux variables
1664 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
1665 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1666 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
1667 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
1668 static btSpatialTransformationMatrix fromParent;
1669 /////////////////
1670
1671 // First 'upward' loop.
1672 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1673
1674 // Fill in zero_acc
1675 // -- set to force/torque on the base, zero otherwise
1676 if (m_fixedBase)
1677 {
1678 zeroAccSpatFrc[0].setZero();
1679 } else
1680 {
1681 //test forces
1682 fromParent.m_rotMat = rot_from_parent[0];
1683 fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
1684 }
1685 for (int i = 0; i < num_links; ++i)
1686 {
1687 zeroAccSpatFrc[i+1].setZero();
1688 }
1689
1690 // 'Downward' loop.
1691 // (part of TreeForwardDynamics in Mirtich.)
1692 for (int i = num_links - 1; i >= 0; --i)
1693 {
1694 const int parent = m_links[i].m_parent;
1695 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1696
1697 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1698 {
1699 Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
1700 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1701 ;
1702 }
1703
1704 btVector3 in_top, in_bottom, out_top, out_bottom;
1705 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1706
1707 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1708 {
1709 invD_times_Y[dof] = 0.f;
1710
1711 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1712 {
1713 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1714 }
1715 }
1716
1717 // Zp += pXi * (Zi + hi*Yi/Di)
1718 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1719
1720 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1721 {
1722 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1723 //
1724 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1725 }
1726
1727
1728 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1729
1730 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1731 }
1732
1733 // ptr to the joint accel part of the output
1734 btScalar * joint_accel = output + 6;
1735
1736
1737 // Second 'upward' loop
1738 // (part of TreeForwardDynamics in Mirtich)
1739
1740 if (m_fixedBase)
1741 {
1742 spatAcc[0].setZero();
1743 }
1744 else
1745 {
1746 solveImatrix(zeroAccSpatFrc[0], result);
1747 spatAcc[0] = -result;
1748
1749 }
1750
1751 // now do the loop over the m_links
1752 for (int i = 0; i < num_links; ++i)
1753 {
1754 const int parent = m_links[i].m_parent;
1755 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1756
1757 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1758
1759 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1760 {
1761 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1762 //
1763 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1764 }
1765
1766 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1767 mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1768
1769 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1770 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1771 }
1772
1773 // transform base accelerations back to the world frame.
1774 btVector3 omegadot_out;
1775 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1776 output[0] = omegadot_out[0];
1777 output[1] = omegadot_out[1];
1778 output[2] = omegadot_out[2];
1779
1780 btVector3 vdot_out;
1781 vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
1782 output[3] = vdot_out[0];
1783 output[4] = vdot_out[1];
1784 output[5] = vdot_out[2];
1785
1786 /////////////////
1787 //printf("delta = [");
1788 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1789 // printf("%.2f ", output[dof]);
1790 //printf("]\n");
1791 /////////////////
1792 }
1793
1794
calcAccelerationDeltas(const btScalar * force,btScalar * output,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v) const1795 void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
1796 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
1797 {
1798 // Temporary matrices/vectors -- use scratch space from caller
1799 // so that we don't have to keep reallocating every frame
1800 int num_links = getNumLinks();
1801 scratch_r.resize(num_links);
1802 scratch_v.resize(4*num_links + 4);
1803
1804 btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
1805 btVector3 * v_ptr = &scratch_v[0];
1806
1807 // zhat_i^A (scratch space)
1808 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1809 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1810
1811 // rot_from_parent (cached from calcAccelerations)
1812 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1813
1814 // hhat (cached), accel (scratch)
1815 const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
1816 const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
1817 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1818 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1819
1820 // Y_i (scratch), D_i (cached)
1821 btScalar * Y = r_ptr; r_ptr += num_links;
1822 const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
1823
1824 btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
1825 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
1826
1827
1828
1829 // First 'upward' loop.
1830 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1831
1832 btVector3 input_force(force[3],force[4],force[5]);
1833 btVector3 input_torque(force[0],force[1],force[2]);
1834
1835 // Fill in zero_acc
1836 // -- set to force/torque on the base, zero otherwise
1837 if (m_fixedBase)
1838 {
1839 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
1840 } else
1841 {
1842 zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
1843 zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
1844 }
1845 for (int i = 0; i < num_links; ++i)
1846 {
1847 zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
1848 }
1849
1850 // 'Downward' loop.
1851 for (int i = num_links - 1; i >= 0; --i)
1852 {
1853 // btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
1854 Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
1855 Y[i] += force[6 + i]; // add joint torque
1856
1857 const int parent = m_links[i].m_parent;
1858
1859 // Zp += pXi * (Zi + hi*Yi/Di)
1860 btVector3 in_top, in_bottom, out_top, out_bottom;
1861 const btScalar Y_over_D = Y[i] / D[i];
1862 in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
1863 in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
1864 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1865 in_top, in_bottom, out_top, out_bottom);
1866 zero_acc_top_angular[parent+1] += out_top;
1867 zero_acc_bottom_linear[parent+1] += out_bottom;
1868 }
1869
1870 // ptr to the joint accel part of the output
1871 btScalar * joint_accel = output + 6;
1872
1873 // Second 'upward' loop
1874 if (m_fixedBase)
1875 {
1876 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
1877 } else
1878 {
1879 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
1880 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
1881
1882 float result[6];
1883 solveImatrix(rhs_top,rhs_bot, result);
1884 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
1885
1886 for (int i = 0; i < 3; ++i) {
1887 accel_top[0][i] = -result[i];
1888 accel_bottom[0][i] = -result[i+3];
1889 }
1890
1891 }
1892
1893 // now do the loop over the m_links
1894 for (int i = 0; i < num_links; ++i) {
1895 const int parent = m_links[i].m_parent;
1896 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1897 accel_top[parent+1], accel_bottom[parent+1],
1898 accel_top[i+1], accel_bottom[i+1]);
1899 btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
1900 joint_accel[i] = Y_minus_hT_a / D[i];
1901 accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
1902 accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
1903 }
1904
1905 // transform base accelerations back to the world frame.
1906 btVector3 omegadot_out;
1907 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1908 output[0] = omegadot_out[0];
1909 output[1] = omegadot_out[1];
1910 output[2] = omegadot_out[2];
1911
1912 btVector3 vdot_out;
1913 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1914
1915 output[3] = vdot_out[0];
1916 output[4] = vdot_out[1];
1917 output[5] = vdot_out[2];
1918
1919
1920 /////////////////
1921 /*
1922 int ndof = getNumLinks() + 6;
1923 printf("test force(impulse) (%d) = [\n",ndof);
1924
1925 for (int i=0;i<ndof;i++)
1926 {
1927 printf("%.2f ", force[i]);
1928 printf("]\n");
1929 }
1930
1931 printf("delta(%d) = [",ndof);
1932 for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1933 printf("%.2f ", output[dof]);
1934 printf("]\n");
1935 /////////////////
1936 */
1937
1938 //int dummy = 0;
1939 }
1940
stepPositions(btScalar dt)1941 void btMultiBody::stepPositions(btScalar dt)
1942 {
1943 int num_links = getNumLinks();
1944 // step position by adding dt * velocity
1945 btVector3 v = getBaseVel();
1946 m_basePos += dt * v;
1947
1948 // "exponential map" method for the rotation
1949 btVector3 base_omega = getBaseOmega();
1950 const btScalar omega_norm = base_omega.norm();
1951 const btScalar omega_times_dt = omega_norm * dt;
1952 const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
1953 if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
1954 {
1955 const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
1956 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
1957 const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
1958 m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
1959 } else
1960 {
1961 m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
1962 }
1963
1964 // Make sure the quaternion represents a valid rotation.
1965 // (Not strictly necessary, but helps prevent any round-off errors from building up.)
1966 m_baseQuat.normalize();
1967
1968 // Finally we can update m_jointPos for each of the m_links
1969 for (int i = 0; i < num_links; ++i)
1970 {
1971 float jointVel = getJointVel(i);
1972 m_links[i].m_jointPos[0] += dt * jointVel;
1973 m_links[i].updateCache();
1974 }
1975 }
1976
stepPositionsMultiDof(btScalar dt,btScalar * pq,btScalar * pqd)1977 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
1978 {
1979 int num_links = getNumLinks();
1980 // step position by adding dt * velocity
1981 //btVector3 v = getBaseVel();
1982 //m_basePos += dt * v;
1983 //
1984 btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1985 btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
1986 //
1987 pBasePos[0] += dt * pBaseVel[0];
1988 pBasePos[1] += dt * pBaseVel[1];
1989 pBasePos[2] += dt * pBaseVel[2];
1990
1991 ///////////////////////////////
1992 //local functor for quaternion integration (to avoid error prone redundancy)
1993 struct
1994 {
1995 //"exponential map" based on btTransformUtil::integrateTransform(..)
1996 void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1997 {
1998 //baseBody => quat is alias and omega is global coor
1999 //!baseBody => quat is alibi and omega is local coor
2000
2001 btVector3 axis;
2002 btVector3 angvel;
2003
2004 if(!baseBody)
2005 angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
2006 else
2007 angvel = omega;
2008
2009 btScalar fAngle = angvel.length();
2010 //limit the angular motion
2011 if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
2012 {
2013 fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
2014 }
2015
2016 if ( fAngle < btScalar(0.001) )
2017 {
2018 // use Taylor's expansions of sync function
2019 axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
2020 }
2021 else
2022 {
2023 // sync(fAngle) = sin(c*fAngle)/t
2024 axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
2025 }
2026
2027 if(!baseBody)
2028 quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;
2029 else
2030 quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
2031 //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
2032
2033 quat.normalize();
2034 }
2035 } pQuatUpdateFun;
2036 ///////////////////////////////
2037
2038 //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
2039 //
2040 btScalar *pBaseQuat = pq ? pq : m_baseQuat;
2041 btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
2042 //
2043 static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
2044 static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
2045 pQuatUpdateFun(baseOmega, baseQuat, true, dt);
2046 pBaseQuat[0] = baseQuat.x();
2047 pBaseQuat[1] = baseQuat.y();
2048 pBaseQuat[2] = baseQuat.z();
2049 pBaseQuat[3] = baseQuat.w();
2050
2051
2052 //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
2053 //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
2054 //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
2055
2056 if(pq)
2057 pq += 7;
2058 if(pqd)
2059 pqd += 6;
2060
2061 // Finally we can update m_jointPos for each of the m_links
2062 for (int i = 0; i < num_links; ++i)
2063 {
2064 btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
2065 btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
2066
2067 switch(m_links[i].m_jointType)
2068 {
2069 case btMultibodyLink::ePrismatic:
2070 case btMultibodyLink::eRevolute:
2071 {
2072 btScalar jointVel = pJointVel[0];
2073 pJointPos[0] += dt * jointVel;
2074 break;
2075 }
2076 case btMultibodyLink::eSpherical:
2077 {
2078 static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
2079 static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
2080 pQuatUpdateFun(jointVel, jointOri, false, dt);
2081 pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
2082 break;
2083 }
2084 case btMultibodyLink::ePlanar:
2085 {
2086 pJointPos[0] += dt * getJointVelMultiDof(i)[0];
2087
2088 btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
2089 btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
2090 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
2091 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
2092
2093 break;
2094 }
2095 default:
2096 {
2097 }
2098
2099 }
2100
2101 m_links[i].updateCacheMultiDof(pq);
2102
2103 if(pq)
2104 pq += m_links[i].m_posVarCount;
2105 if(pqd)
2106 pqd += m_links[i].m_dofCount;
2107 }
2108 }
2109
filConstraintJacobianMultiDof(int link,const btVector3 & contact_point,const btVector3 & normal_ang,const btVector3 & normal_lin,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m) const2110 void btMultiBody::filConstraintJacobianMultiDof(int link,
2111 const btVector3 &contact_point,
2112 const btVector3 &normal_ang,
2113 const btVector3 &normal_lin,
2114 btScalar *jac,
2115 btAlignedObjectArray<btScalar> &scratch_r,
2116 btAlignedObjectArray<btVector3> &scratch_v,
2117 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2118 {
2119 // temporary space
2120 int num_links = getNumLinks();
2121 int m_dofCount = getNumDofs();
2122 scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
2123 scratch_m.resize(num_links + 1);
2124
2125 btVector3 * v_ptr = &scratch_v[0];
2126 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
2127 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
2128 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
2129 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2130
2131 scratch_r.resize(m_dofCount);
2132 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
2133
2134 btMatrix3x3 * rot_from_world = &scratch_m[0];
2135
2136 const btVector3 p_minus_com_world = contact_point - m_basePos;
2137 const btVector3 &normal_lin_world = normal_lin; //convenience
2138 const btVector3 &normal_ang_world = normal_ang;
2139
2140 rot_from_world[0] = btMatrix3x3(m_baseQuat);
2141
2142 // omega coeffients first.
2143 btVector3 omega_coeffs_world;
2144 omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
2145 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
2146 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
2147 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
2148 // then v coefficients
2149 jac[3] = normal_lin_world[0];
2150 jac[4] = normal_lin_world[1];
2151 jac[5] = normal_lin_world[2];
2152
2153 //create link-local versions of p_minus_com and normal
2154 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
2155 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
2156 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2157
2158 // Set remaining jac values to zero for now.
2159 for (int i = 6; i < 6 + m_dofCount; ++i)
2160 {
2161 jac[i] = 0;
2162 }
2163
2164 // Qdot coefficients, if necessary.
2165 if (num_links > 0 && link > -1) {
2166
2167 // TODO: speed this up -- don't calculate for m_links we don't need.
2168 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2169 // which is resulting in repeated work being done...)
2170
2171 // calculate required normals & positions in the local frames.
2172 for (int i = 0; i < num_links; ++i) {
2173
2174 // transform to local frame
2175 const int parent = m_links[i].m_parent;
2176 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2177 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2178
2179 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
2180 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
2181 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
2182
2183 // calculate the jacobian entry
2184 switch(m_links[i].m_jointType)
2185 {
2186 case btMultibodyLink::eRevolute:
2187 {
2188 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
2189 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2190 break;
2191 }
2192 case btMultibodyLink::ePrismatic:
2193 {
2194 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2195 break;
2196 }
2197 case btMultibodyLink::eSpherical:
2198 {
2199 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
2200 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
2201 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
2202
2203 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2204 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
2205 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
2206
2207 break;
2208 }
2209 case btMultibodyLink::ePlanar:
2210 {
2211 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
2212 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
2213 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
2214
2215 break;
2216 }
2217 default:
2218 {
2219 }
2220 }
2221
2222 }
2223
2224 // Now copy through to output.
2225 //printf("jac[%d] = ", link);
2226 while (link != -1)
2227 {
2228 for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
2229 {
2230 jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2231 //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
2232 }
2233
2234 link = m_links[link].m_parent;
2235 }
2236 //printf("]\n");
2237 }
2238 }
2239
fillContactJacobian(int link,const btVector3 & contact_point,const btVector3 & normal,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m) const2240 void btMultiBody::fillContactJacobian(int link,
2241 const btVector3 &contact_point,
2242 const btVector3 &normal,
2243 btScalar *jac,
2244 btAlignedObjectArray<btScalar> &scratch_r,
2245 btAlignedObjectArray<btVector3> &scratch_v,
2246 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2247 {
2248 // temporary space
2249 int num_links = getNumLinks();
2250 scratch_v.resize(2*num_links + 2);
2251 scratch_m.resize(num_links + 1);
2252
2253 btVector3 * v_ptr = &scratch_v[0];
2254 btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
2255 btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
2256 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2257
2258 scratch_r.resize(num_links);
2259 btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
2260
2261 btMatrix3x3 * rot_from_world = &scratch_m[0];
2262
2263 const btVector3 p_minus_com_world = contact_point - m_basePos;
2264
2265 rot_from_world[0] = btMatrix3x3(m_baseQuat);
2266
2267 p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
2268 n_local[0] = rot_from_world[0] * normal;
2269
2270 // omega coeffients first.
2271 if (this->m_fixedBase)
2272 {
2273 for (int i=0;i<6;i++)
2274 {
2275 jac[i]=0;
2276 }
2277 } else
2278 {
2279 btVector3 omega_coeffs;
2280
2281 omega_coeffs = p_minus_com_world.cross(normal);
2282 jac[0] = omega_coeffs[0];
2283 jac[1] = omega_coeffs[1];
2284 jac[2] = omega_coeffs[2];
2285 // then v coefficients
2286 jac[3] = normal[0];
2287 jac[4] = normal[1];
2288 jac[5] = normal[2];
2289 }
2290 // Set remaining jac values to zero for now.
2291 for (int i = 6; i < 6 + num_links; ++i) {
2292 jac[i] = 0;
2293 }
2294
2295 // Qdot coefficients, if necessary.
2296 if (num_links > 0 && link > -1) {
2297
2298 // TODO: speed this up -- don't calculate for m_links we don't need.
2299 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2300 // which is resulting in repeated work being done...)
2301
2302 // calculate required normals & positions in the local frames.
2303 for (int i = 0; i < num_links; ++i) {
2304
2305 // transform to local frame
2306 const int parent = m_links[i].m_parent;
2307 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2308 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2309
2310 n_local[i+1] = mtx * n_local[parent+1];
2311 p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
2312
2313 // calculate the jacobian entry
2314 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
2315 results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
2316 } else {
2317 results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
2318 }
2319 }
2320
2321 // Now copy through to output.
2322 //printf("jac[%d] = ", link);
2323 while (link != -1)
2324 {
2325 jac[6 + link] = results[link];
2326 //printf("%.2f\t", jac[6 + link]);
2327 link = m_links[link].m_parent;
2328 }
2329 //printf("]\n");
2330 }
2331 }
2332
wakeUp()2333 void btMultiBody::wakeUp()
2334 {
2335 m_awake = true;
2336 }
2337
goToSleep()2338 void btMultiBody::goToSleep()
2339 {
2340 m_awake = false;
2341 }
2342
checkMotionAndSleepIfRequired(btScalar timestep)2343 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
2344 {
2345 int num_links = getNumLinks();
2346 extern bool gDisableDeactivation;
2347 if (!m_canSleep || gDisableDeactivation)
2348 {
2349 m_awake = true;
2350 m_sleepTimer = 0;
2351 return;
2352 }
2353
2354 // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
2355 btScalar motion = 0;
2356 if(m_isMultiDof)
2357 {
2358 for (int i = 0; i < 6 + m_dofCount; ++i)
2359 motion += m_realBuf[i] * m_realBuf[i];
2360 }
2361 else
2362 {
2363 for (int i = 0; i < 6 + num_links; ++i)
2364 motion += m_realBuf[i] * m_realBuf[i];
2365 }
2366
2367
2368 if (motion < SLEEP_EPSILON) {
2369 m_sleepTimer += timestep;
2370 if (m_sleepTimer > SLEEP_TIMEOUT) {
2371 goToSleep();
2372 }
2373 } else {
2374 m_sleepTimer = 0;
2375 if (!m_awake)
2376 wakeUp();
2377 }
2378 }
2379
2380
forwardKinematics(btAlignedObjectArray<btQuaternion> & world_to_local,btAlignedObjectArray<btVector3> & local_origin)2381 void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
2382 {
2383
2384 int num_links = getNumLinks();
2385
2386 // Cached 3x3 rotation matrices from parent frame to this frame.
2387 btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
2388
2389 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
2390
2391 for (int i = 0; i < num_links; ++i)
2392 {
2393 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
2394 }
2395
2396 int nLinks = getNumLinks();
2397 ///base + num m_links
2398 world_to_local.resize(nLinks+1);
2399 local_origin.resize(nLinks+1);
2400
2401 world_to_local[0] = getWorldToBaseRot();
2402 local_origin[0] = getBasePos();
2403
2404 for (int k=0;k<getNumLinks();k++)
2405 {
2406 const int parent = getParent(k);
2407 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
2408 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
2409 }
2410
2411 for (int link=0;link<getNumLinks();link++)
2412 {
2413 int index = link+1;
2414
2415 btVector3 posr = local_origin[index];
2416 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2417 btTransform tr;
2418 tr.setIdentity();
2419 tr.setOrigin(posr);
2420 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2421 getLink(link).m_cachedWorldTransform = tr;
2422
2423 }
2424
2425 }
2426
updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local,btAlignedObjectArray<btVector3> & local_origin)2427 void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
2428 {
2429 world_to_local.resize(getNumLinks()+1);
2430 local_origin.resize(getNumLinks()+1);
2431
2432 world_to_local[0] = getWorldToBaseRot();
2433 local_origin[0] = getBasePos();
2434
2435 if (getBaseCollider())
2436 {
2437 btVector3 posr = local_origin[0];
2438 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2439 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
2440 btTransform tr;
2441 tr.setIdentity();
2442 tr.setOrigin(posr);
2443 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2444
2445 getBaseCollider()->setWorldTransform(tr);
2446
2447 }
2448
2449 for (int k=0;k<getNumLinks();k++)
2450 {
2451 const int parent = getParent(k);
2452 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
2453 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
2454 }
2455
2456
2457 for (int m=0;m<getNumLinks();m++)
2458 {
2459 btMultiBodyLinkCollider* col = getLink(m).m_collider;
2460 if (col)
2461 {
2462 int link = col->m_link;
2463 btAssert(link == m);
2464
2465 int index = link+1;
2466
2467 btVector3 posr = local_origin[index];
2468 // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2469 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2470 btTransform tr;
2471 tr.setIdentity();
2472 tr.setOrigin(posr);
2473 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2474
2475 col->setWorldTransform(tr);
2476 }
2477 }
2478 }
2479
calculateSerializeBufferSize() const2480 int btMultiBody::calculateSerializeBufferSize() const
2481 {
2482 int sz = sizeof(btMultiBodyData);
2483 return sz;
2484 }
2485
2486 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,class btSerializer * serializer) const2487 const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
2488 {
2489 btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
2490 getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
2491 mbd->m_baseMass = this->getBaseMass();
2492 getBaseInertia().serialize(mbd->m_baseInertia);
2493 {
2494 char* name = (char*) serializer->findNameForPointer(m_baseName);
2495 mbd->m_baseName = (char*)serializer->getUniquePointer(name);
2496 if (mbd->m_baseName)
2497 {
2498 serializer->serializeName(name);
2499 }
2500 }
2501 mbd->m_numLinks = this->getNumLinks();
2502 if (mbd->m_numLinks)
2503 {
2504 int sz = sizeof(btMultiBodyLinkData);
2505 int numElem = mbd->m_numLinks;
2506 btChunk* chunk = serializer->allocate(sz,numElem);
2507 btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
2508 for (int i=0;i<numElem;i++,memPtr++)
2509 {
2510
2511 memPtr->m_jointType = getLink(i).m_jointType;
2512 memPtr->m_dofCount = getLink(i).m_dofCount;
2513 memPtr->m_posVarCount = getLink(i).m_posVarCount;
2514
2515 getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
2516 memPtr->m_linkMass = getLink(i).m_mass;
2517 memPtr->m_parentIndex = getLink(i).m_parent;
2518 getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
2519 getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
2520 getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
2521 btAssert(memPtr->m_dofCount<=3);
2522 for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
2523 {
2524 getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
2525 getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
2526
2527 memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
2528 memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
2529
2530 }
2531 int numPosVar = getLink(i).m_posVarCount;
2532 for (int posvar = 0; posvar < numPosVar;posvar++)
2533 {
2534 memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
2535 }
2536
2537
2538 {
2539 char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
2540 memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
2541 if (memPtr->m_linkName)
2542 {
2543 serializer->serializeName(name);
2544 }
2545 }
2546 {
2547 char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
2548 memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
2549 if (memPtr->m_jointName)
2550 {
2551 serializer->serializeName(name);
2552 }
2553 }
2554 memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
2555
2556 }
2557 serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
2558 }
2559 mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
2560
2561 return btMultiBodyDataName;
2562 }
2563