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