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