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