Bullet Collision Detection & Physics Library: btMultiBody.cpp Source File

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