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