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