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