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