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