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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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