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