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