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