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

Bullet Collision Detection & Physics Library

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