Bullet Collision Detection & Physics Library
btConeTwistConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 
15 Written by: Marcus Hennix
16 */
17 
18 
19 #include "btConeTwistConstraint.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "LinearMath/btTransformUtil.h"
22 #include "LinearMath/btMinMax.h"
23 #include <new>
24 
25 
26 
27 //#define CONETWIST_USE_OBSOLETE_SOLVER true
28 #define CONETWIST_USE_OBSOLETE_SOLVER false
29 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
30 
31 
32 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
33 {
34  btVector3 vec = axis * invInertiaWorld;
35  return axis.dot(vec);
36 }
37 
38 
39 
40 
41 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
42  const btTransform& rbAFrame,const btTransform& rbBFrame)
43  :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
44  m_angularOnly(false),
45  m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
46 {
47  init();
48 }
49 
50 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
51  :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
52  m_angularOnly(false),
53  m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
54 {
55  m_rbBFrame = m_rbAFrame;
56  init();
57 }
58 
59 
60 void btConeTwistConstraint::init()
61 {
62  m_angularOnly = false;
63  m_solveTwistLimit = false;
64  m_solveSwingLimit = false;
65  m_bMotorEnabled = false;
66  m_maxMotorImpulse = btScalar(-1);
67 
68  setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
69  m_damping = btScalar(0.01);
70  m_fixThresh = CONETWIST_DEF_FIX_THRESH;
71  m_flags = 0;
72  m_linCFM = btScalar(0.f);
73  m_linERP = btScalar(0.7f);
74  m_angCFM = btScalar(0.f);
75 }
76 
77 
78 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
79 {
80  if (m_useSolveConstraintObsolete)
81  {
82  info->m_numConstraintRows = 0;
83  info->nub = 0;
84  }
85  else
86  {
87  info->m_numConstraintRows = 3;
88  info->nub = 3;
89  calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
90  if(m_solveSwingLimit)
91  {
92  info->m_numConstraintRows++;
93  info->nub--;
94  if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
95  {
96  info->m_numConstraintRows++;
97  info->nub--;
98  }
99  }
100  if(m_solveTwistLimit)
101  {
102  info->m_numConstraintRows++;
103  info->nub--;
104  }
105  }
106 }
107 
108 void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
109 {
110  //always reserve 6 rows: object transform is not available on SPU
111  info->m_numConstraintRows = 6;
112  info->nub = 0;
113 
114 }
115 
116 
117 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
118 {
119  getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
120 }
121 
122 void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
123 {
124  calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
125 
126  btAssert(!m_useSolveConstraintObsolete);
127  // set jacobian
128  info->m_J1linearAxis[0] = 1;
129  info->m_J1linearAxis[info->rowskip+1] = 1;
130  info->m_J1linearAxis[2*info->rowskip+2] = 1;
131  btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
132  {
133  btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
134  btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
135  btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
136  btVector3 a1neg = -a1;
137  a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
138  }
139  btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
140  {
141  btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
142  btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
143  btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
144  a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
145  }
146  // set right hand side
147  btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
148  btScalar k = info->fps * linERP;
149  int j;
150  for (j=0; j<3; j++)
151  {
152  info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
153  info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
154  info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
155  if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
156  {
157  info->cfm[j*info->rowskip] = m_linCFM;
158  }
159  }
160  int row = 3;
161  int srow = row * info->rowskip;
162  btVector3 ax1;
163  // angular limits
164  if(m_solveSwingLimit)
165  {
166  btScalar *J1 = info->m_J1angularAxis;
167  btScalar *J2 = info->m_J2angularAxis;
168  if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
169  {
170  btTransform trA = transA*m_rbAFrame;
171  btVector3 p = trA.getBasis().getColumn(1);
172  btVector3 q = trA.getBasis().getColumn(2);
173  int srow1 = srow + info->rowskip;
174  J1[srow+0] = p[0];
175  J1[srow+1] = p[1];
176  J1[srow+2] = p[2];
177  J1[srow1+0] = q[0];
178  J1[srow1+1] = q[1];
179  J1[srow1+2] = q[2];
180  J2[srow+0] = -p[0];
181  J2[srow+1] = -p[1];
182  J2[srow+2] = -p[2];
183  J2[srow1+0] = -q[0];
184  J2[srow1+1] = -q[1];
185  J2[srow1+2] = -q[2];
186  btScalar fact = info->fps * m_relaxationFactor;
187  info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
188  info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
189  info->m_lowerLimit[srow] = -SIMD_INFINITY;
190  info->m_upperLimit[srow] = SIMD_INFINITY;
191  info->m_lowerLimit[srow1] = -SIMD_INFINITY;
192  info->m_upperLimit[srow1] = SIMD_INFINITY;
193  srow = srow1 + info->rowskip;
194  }
195  else
196  {
197  ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
198  J1[srow+0] = ax1[0];
199  J1[srow+1] = ax1[1];
200  J1[srow+2] = ax1[2];
201  J2[srow+0] = -ax1[0];
202  J2[srow+1] = -ax1[1];
203  J2[srow+2] = -ax1[2];
204  btScalar k = info->fps * m_biasFactor;
205 
206  info->m_constraintError[srow] = k * m_swingCorrection;
207  if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
208  {
209  info->cfm[srow] = m_angCFM;
210  }
211  // m_swingCorrection is always positive or 0
212  info->m_lowerLimit[srow] = 0;
213  info->m_upperLimit[srow] = SIMD_INFINITY;
214  srow += info->rowskip;
215  }
216  }
217  if(m_solveTwistLimit)
218  {
219  ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
220  btScalar *J1 = info->m_J1angularAxis;
221  btScalar *J2 = info->m_J2angularAxis;
222  J1[srow+0] = ax1[0];
223  J1[srow+1] = ax1[1];
224  J1[srow+2] = ax1[2];
225  J2[srow+0] = -ax1[0];
226  J2[srow+1] = -ax1[1];
227  J2[srow+2] = -ax1[2];
228  btScalar k = info->fps * m_biasFactor;
229  info->m_constraintError[srow] = k * m_twistCorrection;
230  if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
231  {
232  info->cfm[srow] = m_angCFM;
233  }
234  if(m_twistSpan > 0.0f)
235  {
236 
237  if(m_twistCorrection > 0.0f)
238  {
239  info->m_lowerLimit[srow] = 0;
240  info->m_upperLimit[srow] = SIMD_INFINITY;
241  }
242  else
243  {
244  info->m_lowerLimit[srow] = -SIMD_INFINITY;
245  info->m_upperLimit[srow] = 0;
246  }
247  }
248  else
249  {
250  info->m_lowerLimit[srow] = -SIMD_INFINITY;
251  info->m_upperLimit[srow] = SIMD_INFINITY;
252  }
253  srow += info->rowskip;
254  }
255 }
256 
257 
258 
259 void btConeTwistConstraint::buildJacobian()
260 {
261  if (m_useSolveConstraintObsolete)
262  {
263  m_appliedImpulse = btScalar(0.);
264  m_accTwistLimitImpulse = btScalar(0.);
265  m_accSwingLimitImpulse = btScalar(0.);
266  m_accMotorImpulse = btVector3(0.,0.,0.);
267 
268  if (!m_angularOnly)
269  {
270  btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
271  btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
272  btVector3 relPos = pivotBInW - pivotAInW;
273 
274  btVector3 normal[3];
275  if (relPos.length2() > SIMD_EPSILON)
276  {
277  normal[0] = relPos.normalized();
278  }
279  else
280  {
281  normal[0].setValue(btScalar(1.0),0,0);
282  }
283 
284  btPlaneSpace1(normal[0], normal[1], normal[2]);
285 
286  for (int i=0;i<3;i++)
287  {
288  new (&m_jac[i]) btJacobianEntry(
289  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
290  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
291  pivotAInW - m_rbA.getCenterOfMassPosition(),
292  pivotBInW - m_rbB.getCenterOfMassPosition(),
293  normal[i],
294  m_rbA.getInvInertiaDiagLocal(),
295  m_rbA.getInvMass(),
296  m_rbB.getInvInertiaDiagLocal(),
297  m_rbB.getInvMass());
298  }
299  }
300 
301  calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
302  }
303 }
304 
305 
306 
307 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
308 {
309  #ifndef __SPU__
310  if (m_useSolveConstraintObsolete)
311  {
312  btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
313  btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
314 
315  btScalar tau = btScalar(0.3);
316 
317  //linear part
318  if (!m_angularOnly)
319  {
320  btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
321  btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
322 
323  btVector3 vel1;
324  bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
325  btVector3 vel2;
326  bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
327  btVector3 vel = vel1 - vel2;
328 
329  for (int i=0;i<3;i++)
330  {
331  const btVector3& normal = m_jac[i].m_linearJointAxis;
332  btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
333 
334  btScalar rel_vel;
335  rel_vel = normal.dot(vel);
336  //positional error (zeroth order error)
337  btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
338  btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
339  m_appliedImpulse += impulse;
340 
341  btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
342  btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
343  bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
344  bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
345 
346  }
347  }
348 
349  // apply motor
350  if (m_bMotorEnabled)
351  {
352  // compute current and predicted transforms
353  btTransform trACur = m_rbA.getCenterOfMassTransform();
354  btTransform trBCur = m_rbB.getCenterOfMassTransform();
355  btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
356  btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
357  btTransform trAPred; trAPred.setIdentity();
358  btVector3 zerovec(0,0,0);
359  btTransformUtil::integrateTransform(
360  trACur, zerovec, omegaA, timeStep, trAPred);
361  btTransform trBPred; trBPred.setIdentity();
362  btTransformUtil::integrateTransform(
363  trBCur, zerovec, omegaB, timeStep, trBPred);
364 
365  // compute desired transforms in world
366  btTransform trPose(m_qTarget);
367  btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
368  btTransform trADes = trBPred * trABDes;
369  btTransform trBDes = trAPred * trABDes.inverse();
370 
371  // compute desired omegas in world
372  btVector3 omegaADes, omegaBDes;
373 
374  btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
375  btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
376 
377  // compute delta omegas
378  btVector3 dOmegaA = omegaADes - omegaA;
379  btVector3 dOmegaB = omegaBDes - omegaB;
380 
381  // compute weighted avg axis of dOmega (weighting based on inertias)
382  btVector3 axisA, axisB;
383  btScalar kAxisAInv = 0, kAxisBInv = 0;
384 
385  if (dOmegaA.length2() > SIMD_EPSILON)
386  {
387  axisA = dOmegaA.normalized();
388  kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
389  }
390 
391  if (dOmegaB.length2() > SIMD_EPSILON)
392  {
393  axisB = dOmegaB.normalized();
394  kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
395  }
396 
397  btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
398 
399  static bool bDoTorque = true;
400  if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
401  {
402  avgAxis.normalize();
403  kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
404  kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
405  btScalar kInvCombined = kAxisAInv + kAxisBInv;
406 
407  btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
408  (kInvCombined * kInvCombined);
409 
410  if (m_maxMotorImpulse >= 0)
411  {
412  btScalar fMaxImpulse = m_maxMotorImpulse;
413  if (m_bNormalizedMotorStrength)
414  fMaxImpulse = fMaxImpulse/kAxisAInv;
415 
416  btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
417  btScalar newUnclampedMag = newUnclampedAccImpulse.length();
418  if (newUnclampedMag > fMaxImpulse)
419  {
420  newUnclampedAccImpulse.normalize();
421  newUnclampedAccImpulse *= fMaxImpulse;
422  impulse = newUnclampedAccImpulse - m_accMotorImpulse;
423  }
424  m_accMotorImpulse += impulse;
425  }
426 
427  btScalar impulseMag = impulse.length();
428  btVector3 impulseAxis = impulse / impulseMag;
429 
430  bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
431  bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
432 
433  }
434  }
435  else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
436  {
437  btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
438  btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
439  btVector3 relVel = angVelB - angVelA;
440  if (relVel.length2() > SIMD_EPSILON)
441  {
442  btVector3 relVelAxis = relVel.normalized();
443  btScalar m_kDamping = btScalar(1.) /
444  (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
445  getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
446  btVector3 impulse = m_damping * m_kDamping * relVel;
447 
448  btScalar impulseMag = impulse.length();
449  btVector3 impulseAxis = impulse / impulseMag;
450  bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
451  bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
452  }
453  }
454 
455  // joint limits
456  {
458  btVector3 angVelA;
459  bodyA.internalGetAngularVelocity(angVelA);
460  btVector3 angVelB;
461  bodyB.internalGetAngularVelocity(angVelB);
462 
463  // solve swing limit
464  if (m_solveSwingLimit)
465  {
466  btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
467  btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
468  if (relSwingVel > 0)
469  amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
470  btScalar impulseMag = amplitude * m_kSwing;
471 
472  // Clamp the accumulated impulse
473  btScalar temp = m_accSwingLimitImpulse;
474  m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
475  impulseMag = m_accSwingLimitImpulse - temp;
476 
477  btVector3 impulse = m_swingAxis * impulseMag;
478 
479  // don't let cone response affect twist
480  // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
481  {
482  btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
483  btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
484  impulse = impulseNoTwistCouple;
485  }
486 
487  impulseMag = impulse.length();
488  btVector3 noTwistSwingAxis = impulse / impulseMag;
489 
490  bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
491  bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
492  }
493 
494 
495  // solve twist limit
496  if (m_solveTwistLimit)
497  {
498  btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
499  btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
500  if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
501  amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
502  btScalar impulseMag = amplitude * m_kTwist;
503 
504  // Clamp the accumulated impulse
505  btScalar temp = m_accTwistLimitImpulse;
506  m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
507  impulseMag = m_accTwistLimitImpulse - temp;
508 
509  // btVector3 impulse = m_twistAxis * impulseMag;
510 
511  bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
512  bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
513  }
514  }
515  }
516 #else
517 btAssert(0);
518 #endif //__SPU__
519 }
520 
521 
522 
523 
524 void btConeTwistConstraint::updateRHS(btScalar timeStep)
525 {
526  (void)timeStep;
527 
528 }
529 
530 
531 #ifndef __SPU__
532 void btConeTwistConstraint::calcAngleInfo()
533 {
534  m_swingCorrection = btScalar(0.);
535  m_twistLimitSign = btScalar(0.);
536  m_solveTwistLimit = false;
537  m_solveSwingLimit = false;
538 
539  btVector3 b1Axis1,b1Axis2,b1Axis3;
540  btVector3 b2Axis1,b2Axis2;
541 
542  b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
543  b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
544 
545  btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
546 
547  btScalar swx=btScalar(0.),swy = btScalar(0.);
548  btScalar thresh = btScalar(10.);
549  btScalar fact;
550 
551  // Get Frame into world space
552  if (m_swingSpan1 >= btScalar(0.05f))
553  {
554  b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
555  swx = b2Axis1.dot(b1Axis1);
556  swy = b2Axis1.dot(b1Axis2);
557  swing1 = btAtan2Fast(swy, swx);
558  fact = (swy*swy + swx*swx) * thresh * thresh;
559  fact = fact / (fact + btScalar(1.0));
560  swing1 *= fact;
561  }
562 
563  if (m_swingSpan2 >= btScalar(0.05f))
564  {
565  b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
566  swx = b2Axis1.dot(b1Axis1);
567  swy = b2Axis1.dot(b1Axis3);
568  swing2 = btAtan2Fast(swy, swx);
569  fact = (swy*swy + swx*swx) * thresh * thresh;
570  fact = fact / (fact + btScalar(1.0));
571  swing2 *= fact;
572  }
573 
574  btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
575  btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
576  btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
577 
578  if (EllipseAngle > 1.0f)
579  {
580  m_swingCorrection = EllipseAngle-1.0f;
581  m_solveSwingLimit = true;
582  // Calculate necessary axis & factors
583  m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
584  m_swingAxis.normalize();
585  btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
586  m_swingAxis *= swingAxisSign;
587  }
588 
589  // Twist limits
590  if (m_twistSpan >= btScalar(0.))
591  {
592  btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
593  btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
594  btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
595  btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
596  m_twistAngle = twist;
597 
598 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
599  btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
600  if (twist <= -m_twistSpan*lockedFreeFactor)
601  {
602  m_twistCorrection = -(twist + m_twistSpan);
603  m_solveTwistLimit = true;
604  m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
605  m_twistAxis.normalize();
606  m_twistAxis *= -1.0f;
607  }
608  else if (twist > m_twistSpan*lockedFreeFactor)
609  {
610  m_twistCorrection = (twist - m_twistSpan);
611  m_solveTwistLimit = true;
612  m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
613  m_twistAxis.normalize();
614  }
615  }
616 }
617 #endif //__SPU__
618 
619 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
620 
621 
622 
623 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
624 {
625  m_swingCorrection = btScalar(0.);
626  m_twistLimitSign = btScalar(0.);
627  m_solveTwistLimit = false;
628  m_solveSwingLimit = false;
629  // compute rotation of A wrt B (in constraint space)
630  if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
631  { // it is assumed that setMotorTarget() was alredy called
632  // and motor target m_qTarget is within constraint limits
633  // TODO : split rotation to pure swing and pure twist
634  // compute desired transforms in world
635  btTransform trPose(m_qTarget);
636  btTransform trA = transA * m_rbAFrame;
637  btTransform trB = transB * m_rbBFrame;
638  btTransform trDeltaAB = trB * trPose * trA.inverse();
639  btQuaternion qDeltaAB = trDeltaAB.getRotation();
640  btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
641  float swingAxisLen2 = swingAxis.length2();
642  if(btFuzzyZero(swingAxisLen2))
643  {
644  return;
645  }
646  m_swingAxis = swingAxis;
647  m_swingAxis.normalize();
648  m_swingCorrection = qDeltaAB.getAngle();
649  if(!btFuzzyZero(m_swingCorrection))
650  {
651  m_solveSwingLimit = true;
652  }
653  return;
654  }
655 
656 
657  {
658  // compute rotation of A wrt B (in constraint space)
659  btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
660  btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
661  btQuaternion qAB = qB.inverse() * qA;
662  // split rotation into cone and twist
663  // (all this is done from B's perspective. Maybe I should be averaging axes...)
664  btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
665  btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
666  btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
667 
668  if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
669  {
670  btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
671  computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
672 
673  if (swingAngle > swingLimit * m_limitSoftness)
674  {
675  m_solveSwingLimit = true;
676 
677  // compute limit ratio: 0->1, where
678  // 0 == beginning of soft limit
679  // 1 == hard/real limit
680  m_swingLimitRatio = 1.f;
681  if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
682  {
683  m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
684  (swingLimit - swingLimit * m_limitSoftness);
685  }
686 
687  // swing correction tries to get back to soft limit
688  m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
689 
690  // adjustment of swing axis (based on ellipse normal)
691  adjustSwingAxisToUseEllipseNormal(swingAxis);
692 
693  // Calculate necessary axis & factors
694  m_swingAxis = quatRotate(qB, -swingAxis);
695 
696  m_twistAxisA.setValue(0,0,0);
697 
698  m_kSwing = btScalar(1.) /
699  (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
700  computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
701  }
702  }
703  else
704  {
705  // you haven't set any limits;
706  // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
707  // anyway, we have either hinge or fixed joint
708  btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
709  btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
710  btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
711  btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
712  btVector3 target;
713  btScalar x = ivB.dot(ivA);
714  btScalar y = ivB.dot(jvA);
715  btScalar z = ivB.dot(kvA);
716  if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
717  { // fixed. We'll need to add one more row to constraint
718  if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
719  {
720  m_solveSwingLimit = true;
721  m_swingAxis = -ivB.cross(ivA);
722  }
723  }
724  else
725  {
726  if(m_swingSpan1 < m_fixThresh)
727  { // hinge around Y axis
728  if(!(btFuzzyZero(y)))
729  {
730  m_solveSwingLimit = true;
731  if(m_swingSpan2 >= m_fixThresh)
732  {
733  y = btScalar(0.f);
734  btScalar span2 = btAtan2(z, x);
735  if(span2 > m_swingSpan2)
736  {
737  x = btCos(m_swingSpan2);
738  z = btSin(m_swingSpan2);
739  }
740  else if(span2 < -m_swingSpan2)
741  {
742  x = btCos(m_swingSpan2);
743  z = -btSin(m_swingSpan2);
744  }
745  }
746  }
747  }
748  else
749  { // hinge around Z axis
750  if(!btFuzzyZero(z))
751  {
752  m_solveSwingLimit = true;
753  if(m_swingSpan1 >= m_fixThresh)
754  {
755  z = btScalar(0.f);
756  btScalar span1 = btAtan2(y, x);
757  if(span1 > m_swingSpan1)
758  {
759  x = btCos(m_swingSpan1);
760  y = btSin(m_swingSpan1);
761  }
762  else if(span1 < -m_swingSpan1)
763  {
764  x = btCos(m_swingSpan1);
765  y = -btSin(m_swingSpan1);
766  }
767  }
768  }
769  }
770  target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
771  target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
772  target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
773  target.normalize();
774  m_swingAxis = -ivB.cross(target);
775  m_swingCorrection = m_swingAxis.length();
776  m_swingAxis.normalize();
777  }
778  }
779 
780  if (m_twistSpan >= btScalar(0.f))
781  {
782  btVector3 twistAxis;
783  computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
784 
785  if (m_twistAngle > m_twistSpan*m_limitSoftness)
786  {
787  m_solveTwistLimit = true;
788 
789  m_twistLimitRatio = 1.f;
790  if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
791  {
792  m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
793  (m_twistSpan - m_twistSpan * m_limitSoftness);
794  }
795 
796  // twist correction tries to get back to soft limit
797  m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
798 
799  m_twistAxis = quatRotate(qB, -twistAxis);
800 
801  m_kTwist = btScalar(1.) /
802  (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
803  computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
804  }
805 
806  if (m_solveSwingLimit)
807  m_twistAxisA = quatRotate(qA, -twistAxis);
808  }
809  else
810  {
811  m_twistAngle = btScalar(0.f);
812  }
813  }
814 }
815 
816 
817 
818 // given a cone rotation in constraint space, (pre: twist must already be removed)
819 // this method computes its corresponding swing angle and axis.
820 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
821 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
822  btScalar& swingAngle, // out
823  btVector3& vSwingAxis, // out
824  btScalar& swingLimit) // out
825 {
826  swingAngle = qCone.getAngle();
827  if (swingAngle > SIMD_EPSILON)
828  {
829  vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
830  vSwingAxis.normalize();
831 #if 0
832  // non-zero twist?! this should never happen.
833  btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
834 #endif
835 
836  // Compute limit for given swing. tricky:
837  // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
838  // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
839 
840  // For starters, compute the direction from center to surface of ellipse.
841  // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
842  // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
843  btScalar xEllipse = vSwingAxis.y();
844  btScalar yEllipse = -vSwingAxis.z();
845 
846  // Now, we use the slope of the vector (using x/yEllipse) and find the length
847  // of the line that intersects the ellipse:
848  // x^2 y^2
849  // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
850  // a^2 b^2
851  // Do the math and it should be clear.
852 
853  swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
854  if (fabs(xEllipse) > SIMD_EPSILON)
855  {
856  btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
857  btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
858  norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
859  btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
860  swingLimit = sqrt(swingLimit2);
861  }
862 
863  // test!
864  /*swingLimit = m_swingSpan2;
865  if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
866  {
867  btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
868  btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
869  btScalar phi = asin(sinphi);
870  btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
871  btScalar alpha = 3.14159f - theta - phi;
872  btScalar sinalpha = sin(alpha);
873  swingLimit = m_swingSpan1 * sinphi/sinalpha;
874  }*/
875  }
876  else if (swingAngle < 0)
877  {
878  // this should never happen!
879 #if 0
880  btAssert(0);
881 #endif
882  }
883 }
884 
885 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
886 {
887  // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
888  btScalar xEllipse = btCos(fAngleInRadians);
889  btScalar yEllipse = btSin(fAngleInRadians);
890 
891  // Use the slope of the vector (using x/yEllipse) and find the length
892  // of the line that intersects the ellipse:
893  // x^2 y^2
894  // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
895  // a^2 b^2
896  // Do the math and it should be clear.
897 
898  float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
899  if (fabs(xEllipse) > SIMD_EPSILON)
900  {
901  btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
902  btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
903  norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
904  btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
905  swingLimit = sqrt(swingLimit2);
906  }
907 
908  // convert into point in constraint space:
909  // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
910  btVector3 vSwingAxis(0, xEllipse, -yEllipse);
911  btQuaternion qSwing(vSwingAxis, swingLimit);
912  btVector3 vPointInConstraintSpace(fLength,0,0);
913  return quatRotate(qSwing, vPointInConstraintSpace);
914 }
915 
916 // given a twist rotation in constraint space, (pre: cone must already be removed)
917 // this method computes its corresponding angle and axis.
918 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
919  btScalar& twistAngle, // out
920  btVector3& vTwistAxis) // out
921 {
922  btQuaternion qMinTwist = qTwist;
923  twistAngle = qTwist.getAngle();
924 
925  if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
926  {
927  qMinTwist = -(qTwist);
928  twistAngle = qMinTwist.getAngle();
929  }
930  if (twistAngle < 0)
931  {
932  // this should never happen
933 #if 0
934  btAssert(0);
935 #endif
936  }
937 
938  vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
939  if (twistAngle > SIMD_EPSILON)
940  vTwistAxis.normalize();
941 }
942 
943 
944 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
945 {
946  // the swing axis is computed as the "twist-free" cone rotation,
947  // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
948  // so, if we're outside the limits, the closest way back inside the cone isn't
949  // along the vector back to the center. better (and more stable) to use the ellipse normal.
950 
951  // convert swing axis to direction from center to surface of ellipse
952  // (ie. rotate 2D vector by PI/2)
953  btScalar y = -vSwingAxis.z();
954  btScalar z = vSwingAxis.y();
955 
956  // do the math...
957  if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
958  {
959  // compute gradient/normal of ellipse surface at current "point"
960  btScalar grad = y/z;
961  grad *= m_swingSpan2 / m_swingSpan1;
962 
963  // adjust y/z to represent normal at point (instead of vector to point)
964  if (y > 0)
965  y = fabs(grad * z);
966  else
967  y = -fabs(grad * z);
968 
969  // convert ellipse direction back to swing axis
970  vSwingAxis.setZ(-y);
971  vSwingAxis.setY( z);
972  vSwingAxis.normalize();
973  }
974 }
975 
976 
977 
978 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
979 {
980  btTransform trACur = m_rbA.getCenterOfMassTransform();
981  btTransform trBCur = m_rbB.getCenterOfMassTransform();
982 // btTransform trABCur = trBCur.inverse() * trACur;
983 // btQuaternion qABCur = trABCur.getRotation();
984 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
985  //btQuaternion qConstraintCur = trConstraintCur.getRotation();
986 
987  btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
988  setMotorTargetInConstraintSpace(qConstraint);
989 }
990 
991 
992 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
993 {
994  m_qTarget = q;
995 
996  // clamp motor target to within limits
997  {
998  btScalar softness = 1.f;//m_limitSoftness;
999 
1000  // split into twist and cone
1001  btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
1002  btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
1003  btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
1004 
1005  // clamp cone
1006  if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
1007  {
1008  btScalar swingAngle, swingLimit; btVector3 swingAxis;
1009  computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1010 
1011  if (fabs(swingAngle) > SIMD_EPSILON)
1012  {
1013  if (swingAngle > swingLimit*softness)
1014  swingAngle = swingLimit*softness;
1015  else if (swingAngle < -swingLimit*softness)
1016  swingAngle = -swingLimit*softness;
1017  qTargetCone = btQuaternion(swingAxis, swingAngle);
1018  }
1019  }
1020 
1021  // clamp twist
1022  if (m_twistSpan >= btScalar(0.05f))
1023  {
1024  btScalar twistAngle; btVector3 twistAxis;
1025  computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1026 
1027  if (fabs(twistAngle) > SIMD_EPSILON)
1028  {
1029  // eddy todo: limitSoftness used here???
1030  if (twistAngle > m_twistSpan*softness)
1031  twistAngle = m_twistSpan*softness;
1032  else if (twistAngle < -m_twistSpan*softness)
1033  twistAngle = -m_twistSpan*softness;
1034  qTargetTwist = btQuaternion(twistAxis, twistAngle);
1035  }
1036  }
1037 
1038  m_qTarget = qTargetCone * qTargetTwist;
1039  }
1040 }
1041 
1044 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1045 {
1046  switch(num)
1047  {
1048  case BT_CONSTRAINT_ERP :
1049  case BT_CONSTRAINT_STOP_ERP :
1050  if((axis >= 0) && (axis < 3))
1051  {
1052  m_linERP = value;
1053  m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1054  }
1055  else
1056  {
1057  m_biasFactor = value;
1058  }
1059  break;
1060  case BT_CONSTRAINT_CFM :
1061  case BT_CONSTRAINT_STOP_CFM :
1062  if((axis >= 0) && (axis < 3))
1063  {
1064  m_linCFM = value;
1065  m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1066  }
1067  else
1068  {
1069  m_angCFM = value;
1070  m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1071  }
1072  break;
1073  default:
1074  btAssertConstrParams(0);
1075  break;
1076  }
1077 }
1078 
1080 btScalar btConeTwistConstraint::getParam(int num, int axis) const
1081 {
1082  btScalar retVal = 0;
1083  switch(num)
1084  {
1085  case BT_CONSTRAINT_ERP :
1086  case BT_CONSTRAINT_STOP_ERP :
1087  if((axis >= 0) && (axis < 3))
1088  {
1089  btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1090  retVal = m_linERP;
1091  }
1092  else if((axis >= 3) && (axis < 6))
1093  {
1094  retVal = m_biasFactor;
1095  }
1096  else
1097  {
1098  btAssertConstrParams(0);
1099  }
1100  break;
1101  case BT_CONSTRAINT_CFM :
1102  case BT_CONSTRAINT_STOP_CFM :
1103  if((axis >= 0) && (axis < 3))
1104  {
1105  btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1106  retVal = m_linCFM;
1107  }
1108  else if((axis >= 3) && (axis < 6))
1109  {
1110  btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1111  retVal = m_angCFM;
1112  }
1113  else
1114  {
1115  btAssertConstrParams(0);
1116  }
1117  break;
1118  default :
1119  btAssertConstrParams(0);
1120  }
1121  return retVal;
1122 }
1123 
1124 
1125 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1126 {
1127  m_rbAFrame = frameA;
1128  m_rbBFrame = frameB;
1129  buildJacobian();
1130  //calculateTransforms();
1131 }
1132 
1133 
1134 
1135 
Generated on Thu Oct 3 2013 18:25:05 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4