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