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

Bullet Collision Detection & Physics Library

btMultiBody.cpp
Go to the documentation of this file.
1 /*
2 * PURPOSE:
3 * Class representing an articulated rigid body. Stores the body's
4 * current state, allows forces and torques to be set, handles
5 * timestepping and implements Featherstone's algorithm.
6 *
7 * COPYRIGHT:
8 * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9 * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10 * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
11
12 This software is provided 'as-is', without any express or implied warranty.
13 In no event will the authors be held liable for any damages arising from the use of this software.
14 Permission is granted to anyone to use this software for any purpose,
15 including commercial applications, and to alter it and redistribute it freely,
16 subject to the following restrictions:
17
18 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
19 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
20 3. This notice may not be removed or altered from any source distribution.
21
22 */
23
24
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 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
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
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btMultibodyLink::m_jointLowerLimit
btScalar m_jointLowerLimit
Definition: btMultiBodyLink.h:149
btMultibodyLink::m_zeroRotParentToThis
btQuaternion m_zeroRotParentToThis
Definition: btMultiBodyLink.h:58
btTransformUtil.h
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:597
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
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:433
btMultiBody::m_awake
bool m_awake
Definition: btMultiBody.h:694
btMultiBody::m_matrixBuf
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:682
btChunk
Definition: btSerializer.h:51
btMultiBody::m_useGyroTerm
bool m_useGyroTerm
Definition: btMultiBody.h:705
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
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:164
btMultiBody::~btMultiBody
virtual ~btMultiBody()
Definition: btMultiBody.cpp:141
btMultiBody::m_baseForce
btVector3 m_baseForce
Definition: btMultiBody.h:655
btMultibodyLink::m_jointFriction
btScalar m_jointFriction
Definition: btMultiBodyLink.h:148
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btSpatialForceVector::getLinear
const btVector3 & getLinear() const
Definition: btSpatialAlgebra.h:48
btMultibodyLink::getAxisTop
const btVector3 & getAxisTop(int dof) const
Definition: btMultiBodyLink.h:111
btMultiBody::m_baseMass
btScalar m_baseMass
Definition: btMultiBody.h:652
btMultiBody::finalizeMultiDof
void finalizeMultiDof()
Definition: btMultiBody.cpp:342
btAlignedObjectArray< btVector3 >
btMultiBody::m_fixedBase
bool m_fixedBase
Definition: btMultiBody.h:691
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
Definition: btMultiBodyLink.h:25
btSpatialForceVector
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
Definition: btSpatialAlgebra.h:24
btMultiBodyLinkDataName
#define btMultiBodyLinkDataName
Definition: btMultiBody.h:45
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:477
btMultiBodyLink.h
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:74
btSpatialTransformationMatrix::transformInverseRotationOnly
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:246
btMultiBody::m_dofCount
int m_dofCount
Definition: btMultiBody.h:711
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:1542
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:131
btMultiBody::compTreeLinkVelocities
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
Definition: btMultiBody.cpp:520
gDisableDeactivation
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
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
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btMultiBodyJointFeedback.h
gJointFeedbackInWorldSpace
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals ...
Definition: btMultiBody.cpp:35
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
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
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:638
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:191
btMultiBody::m_cachedInertiaLowerRight
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:688
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:382
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:628
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:135
btMultiBody::solveImatrix
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
Definition: btMultiBody.cpp:1259
btSpatialForceVector::addLinear
void addLinear(const btVector3 &linear)
Definition: btSpatialAlgebra.h:55
btMultiBody::worldPosToLocal
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:457
btMultiBody::wakeUp
void wakeUp()
Definition: btMultiBody.cpp:1806
gJointFeedbackInJointFrame
bool gJointFeedbackInJointFrame
Definition: btMultiBody.cpp:36
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1377
btMultiBody::setJointPosMultiDof
void setJointPosMultiDof(int i, btScalar *q)
Definition: btMultiBody.cpp:404
symmetricSpatialOuterProduct
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
Definition: btSpatialAlgebra.h:307
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1893
btMultiBody::m_baseConstraintTorque
btVector3 m_baseConstraintTorque
Definition: btMultiBody.h:659
btCollisionObjectData
#define btCollisionObjectData
Definition: btCollisionObject.h:41
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:900
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:506
btMultiBody::getLinkTorque
const btVector3 & getLinkTorque(int i) const
Definition: btMultiBody.cpp:671
btMultiBody::m_deltaV
btAlignedObjectArray< btScalar > m_deltaV
Definition: btMultiBody.h:679
btMultiBody::getBaseOmega
btVector3 getBaseOmega() const
Definition: btMultiBody.h:195
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:60
outerProduct
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
Definition: btMultiBody.cpp:686
btMultibodyLink::getAxisBottom
const btVector3 & getAxisBottom(int dof) const
Definition: btMultiBodyLink.h:112
btSerializer
Definition: btSerializer.h:68
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:608
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btMultibodyLink::eFixed
Definition: btMultiBodyLink.h:78
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btMultiBody::m_baseTorque
btVector3 m_baseTorque
Definition: btMultiBody.h:656
ANGULAR_MOTION_THRESHOLD
#define ANGULAR_MOTION_THRESHOLD
Definition: btTransformUtil.h:20
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btSpatialForceVector::addAngular
void addAngular(const btVector3 &angular)
Definition: btSpatialAlgebra.h:54
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:643
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
Definition: btMultiBody.cpp:510
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
Definition: btMultiBody.cpp:1847
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:1946
btMultiBody::m_cachedInertiaTopLeft
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:685
btSpatialForceVector::setZero
void setZero()
Definition: btSpatialAlgebra.h:57
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:522
btSpatialTransformationMatrix::transformRotationOnly
void transformRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:201
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
btMultibodyLink::m_jointTorque
btScalar m_jointTorque[6]
Definition: btMultiBodyLink.h:129
btMultiBody::getBaseInertia
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:168
btMultiBody::getParentToLocalRot
const btQuaternion & getParentToLocalRot(int i) const
Definition: btMultiBody.cpp:428
btMultibodyLink::m_jointUpperLimit
btScalar m_jointUpperLimit
Definition: btMultiBodyLink.h:150
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:209
btSpatialMotionVector::getLinear
const btVector3 & getLinear() const
Definition: btSpatialAlgebra.h:89
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btMultibodyLink::m_dofCount
int m_dofCount
Definition: btMultiBodyLink.h:135
btSpatialMotionVector::setVector
void setVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:75
btMultiBody::m_baseQuat
btQuaternion m_baseQuat
Definition: btMultiBody.h:650
btMultiBody::m_cachedInertiaLowerLeft
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:687
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:167
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:369
btMultiBody::getJointTorque
btScalar getJointTorque(int i) const
Definition: btMultiBody.cpp:676
btMultiBody::addLinkTorque
void addLinkTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:633
btSpatialMotionVector::getAngular
const btVector3 & getAngular() const
Definition: btSpatialAlgebra.h:88
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btMultibodyLink::m_jointMaxForce
btScalar m_jointMaxForce
Definition: btMultiBodyLink.h:151
output
#define output
btQuadWord::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:152
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:165
btSpatialMotionVector::setLinear
void setLinear(const btVector3 &linear)
Definition: btSpatialAlgebra.h:92
btMultiBody::getParent
int getParent(int link_num) const
Definition: btMultiBody.cpp:352
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btMultibodyLink::m_mass
btScalar m_mass
Definition: btMultiBodyLink.h:53
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btMultiBody::m_cachedInertiaValid
bool m_cachedInertiaValid
Definition: btMultiBody.h:689
btMultiBody::addJointTorque
void addJointTorque(int i, btScalar Q)
Definition: btMultiBody.cpp:650
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
btVector3::setZero
void setZero()
Definition: btVector3.h:683
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
btMultiBody::setJointVel
void setJointVel(int i, btScalar qdot)
Definition: btMultiBody.cpp:412
btSpatialTransformationMatrix::m_trnVec
btVector3 m_trnVec
Definition: btSpatialAlgebra.h:168
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btSpatialMotionVector
Definition: btSpatialAlgebra.h:68
btMultiBody::m_sleepTimer
btScalar m_sleepTimer
Definition: btMultiBody.h:696
btTransform::serialize
void serialize(struct btTransformData &dataOut) const
Definition: btTransform.h:267
btSpatialMotionVector::setAngular
void setAngular(const btVector3 &angular)
Definition: btSpatialAlgebra.h:91
btMultiBody::setJointPos
void setJointPos(int i, btScalar q)
Definition: btMultiBody.cpp:398
btMultibodyLink::m_eVector
btVector3 m_eVector
Definition: btMultiBodyLink.h:68
btCollisionObject::setWorldTransform
void setWorldTransform(const btTransform &worldTrans)
Definition: btCollisionObject.h:382
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:125
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:621
btMultiBody::m_basePos
btVector3 m_basePos
Definition: btMultiBody.h:649
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:1816
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:126
btMultiBody::m_baseConstraintForce
btVector3 m_baseConstraintForce
Definition: btMultiBody.h:658
btMultiBodyLinkCollider.h
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
btMultiBody::m_internalNeedsJointFeedback
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
Definition: btMultiBody.h:715
btMultibodyLink::m_jointDamping
btScalar m_jointDamping
Definition: btMultiBodyLink.h:147
btMultibodyLink::eSpherical
Definition: btMultiBodyLink.h:76
btMultiBody::btMultiBody
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
Definition: btMultiBody.cpp:94
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btMultiBody::m_baseInertia
btVector3 m_baseInertia
Definition: btMultiBody.h:653
btSpatialTransformationMatrix
Definition: btSpatialAlgebra.h:165
btMultiBody::m_baseName
const char * m_baseName
Definition: btMultiBody.h:647
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3::serialize
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1351
btMultiBody.h
btMultiBody::m_realBuf
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:680
btSpatialMotionVector::cross
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
Definition: btSpatialAlgebra.h:105
btSymmetricSpatialDyad::setMatrix
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
Definition: btSpatialAlgebra.h:135
btMultiBody::m_posVarCnt
int m_posVarCnt
Definition: btMultiBody.h:711
btSpatialMotionVector::dot
btScalar dot(const btSpatialForceVector &b) const
Definition: btSpatialAlgebra.h:99
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btSpatialTransformationMatrix::Add
Definition: btSpatialAlgebra.h:173
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBody::updateLinksDofOffsets
void updateLinksDofOffsets()
Definition: btMultiBody.h:631
btSpatialForceVector::getAngular
const btVector3 & getAngular() const
Definition: btSpatialAlgebra.h:49
btMultiBodyDataName
#define btMultiBodyDataName
Definition: btMultiBody.h:43
btMultiBodyData
#define btMultiBodyData
serialization data, don&#39;t change them if you are not familiar with the details of the serialization m...
Definition: btMultiBody.h:42
btMultiBody::m_useGlobalVelocities
bool m_useGlobalVelocities
Definition: btMultiBody.h:712
btMultiBody::localDirToWorld
btVector3 localDirToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:475
btSerializer::serializeName
virtual void serializeName(const char *ptr)=0
btMultiBody::m_linearDamping
btScalar m_linearDamping
Definition: btMultiBody.h:703
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btMultiBody::addJointTorqueMultiDof
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
Definition: btMultiBody.cpp:655
btMultiBody::goToSleep
void goToSleep()
Definition: btMultiBody.cpp:1811
btMultiBody::mulMatrix
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
Definition: btMultiBody.cpp:1362
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition: btMultiBody.cpp:367
btMultiBody::getAngularMomentum
btVector3 getAngularMomentum() const
Definition: btMultiBody.cpp:577
btMultiBody::getLinkForce
const btVector3 & getLinkForce(int i) const
Definition: btMultiBody.cpp:666
btSymmetricSpatialDyad::m_bottomLeftMat
btMatrix3x3 m_bottomLeftMat
Definition: btSpatialAlgebra.h:130
btSpatialForceVector::setVector
void setVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:35
btSpatialTransformationMatrix::transformInverse
void transformInverse(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:224
btMultiBody::m_vectorBuf
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:681
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:29
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
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
btMultiBody::setJointVelMultiDof
void setJointVelMultiDof(int i, btScalar *qdot)
Definition: btMultiBody.cpp:417
btSymmetricSpatialDyad::m_topLeftMat
btMatrix3x3 m_topLeftMat
Definition: btSpatialAlgebra.h:130
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:357
btMultiBody::getKineticEnergy
btScalar getKineticEnergy() const
Definition: btMultiBody.cpp:557
btSerializer::findNameForPointer
virtual const char * findNameForPointer(const void *ptr) const =0
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btSpatialForceVector::addVector
void addVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:41
btMultiBody::getJointVel
btScalar getJointVel(int i) const
Definition: btMultiBody.cpp:372
btMultibodyLink::m_inertiaLocal
btVector3 m_inertiaLocal
Definition: btMultiBodyLink.h:54
btSpatialMotionVector::setZero
void setZero()
Definition: btSpatialAlgebra.h:97
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
btSymmetricSpatialDyad
Definition: btSpatialAlgebra.h:128
btMultiBody::worldDirToLocal
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:494
btMultibodyLink::ePlanar
Definition: btMultiBodyLink.h:77
btMultiBody::m_links
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:661
btSpatialTransformationMatrix::m_rotMat
btMatrix3x3 m_rotMat
Definition: btSpatialAlgebra.h:167
btMultiBody::getJointTorqueMultiDof
btScalar * getJointTorqueMultiDof(int i)
Definition: btMultiBody.cpp:681
btMultiBodyLinkData
#define btMultiBodyLinkData
Definition: btMultiBody.h:44
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:75
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:56
btQuaternion::serialize
void serialize(struct btQuaternionData &dataOut) const
Definition: btQuaternion.h:999
btMultiBody::m_cachedInertiaTopRight
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:686
btMultibodyLink::m_parent
int m_parent
Definition: btMultiBodyLink.h:56
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:377
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btMultiBody::getLinkInertia
const btVector3 & getLinkInertia(int i) const
Definition: btMultiBody.cpp:362
btMultiBody::getRVector
const btVector3 & getRVector(int i) const
Definition: btMultiBody.cpp:423
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btMultiBody::m_angularDamping
btScalar m_angularDamping
Definition: btMultiBody.h:704
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:476
btSymmetricSpatialDyad::m_topRightMat
btMatrix3x3 m_topRightMat
Definition: btSpatialAlgebra.h:130
btMultibodyLink::m_jointMaxVelocity
btScalar m_jointMaxVelocity
Definition: btMultiBodyLink.h:152
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btMultiBody::m_canSleep
bool m_canSleep
Definition: btMultiBody.h:695
btSpatialTransformationMatrix::transform
void transform(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:178
btMultiBody::getBaseVel
const btVector3 getBaseVel() const
Definition: btMultiBody.h:187
btSerializer.h