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

Bullet Collision Detection & Physics Library

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