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"
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 
33 {
34  btVector3 vec = axis * invInertiaWorld;
35  return axis.dot(vec);
36 }
37 
38 
39 
40 
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 
51  :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
52  m_angularOnly(false),
53  m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
54 {
56  m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
57  init();
58 }
59 
60 
62 {
63  m_angularOnly = false;
64  m_solveTwistLimit = false;
65  m_solveSwingLimit = false;
66  m_bMotorEnabled = false;
68 
70  m_damping = btScalar(0.01);
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 
80 {
82  {
83  info->m_numConstraintRows = 0;
84  info->nub = 0;
85  }
86  else
87  {
88  info->m_numConstraintRows = 3;
89  info->nub = 3;
92  {
93  info->m_numConstraintRows++;
94  info->nub--;
96  {
97  info->m_numConstraintRows++;
98  info->nub--;
99  }
100  }
102  {
103  info->m_numConstraintRows++;
104  info->nub--;
105  }
106  }
107 }
108 
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 
119 {
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 
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;
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
169  {
170  btScalar *J1 = info->m_J1angularAxis;
171  btScalar *J2 = info->m_J2angularAxis;
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  {
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;
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  }
222  {
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;
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 
264 {
266  {
270  m_accMotorImpulse = btVector3(0.,0.,0.);
271 
272  if (!m_angularOnly)
273  {
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(
295  pivotAInW - m_rbA.getCenterOfMassPosition(),
296  pivotBInW - m_rbB.getCenterOfMassPosition(),
297  normal[i],
299  m_rbA.getInvMass(),
301  m_rbB.getInvMass());
302  }
303  }
304 
306  }
307 }
308 
309 
310 
312 {
313  #ifndef __SPU__
315  {
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
359  btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
360  btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
361  btTransform trAPred; trAPred.setIdentity();
362  btVector3 zerovec(0,0,0);
364  trACur, zerovec, omegaA, timeStep, trAPred);
365  btTransform trBPred; trBPred.setIdentity();
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;
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.) /
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  {
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
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  {
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
511  impulseMag = m_accTwistLimitImpulse - temp;
512 
513  // btVector3 impulse = m_twistAxis * impulseMag;
514 
517  }
518  }
519  }
520 #else
521 btAssert(0);
522 #endif //__SPU__
523 }
524 
525 
526 
527 
529 {
530  (void)timeStep;
531 
532 }
533 
534 
535 #ifndef __SPU__
537 {
540  m_solveTwistLimit = false;
541  m_solveSwingLimit = false;
542 
543  btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
544  btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
545 
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  {
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  {
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));
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  {
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;
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;
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 {
631  m_solveTwistLimit = false;
632  m_solveSwingLimit = false;
633  // compute rotation of A wrt B (in constraint space)
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;
652  m_swingCorrection = qDeltaAB.getAngle();
654  {
655  m_solveSwingLimit = true;
656  }
657  return;
658  }
659 
660 
661  {
662  // compute rotation of A wrt B (in constraint space)
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 
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)
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) +
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);
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  {
731  { // hinge around Y axis
732 // if(!(btFuzzyZero(y)))
733  if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
734  {
735  m_solveSwingLimit = true;
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;
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);
782 
785  }
786  }
787 
788  if (m_twistSpan >= btScalar(0.f))
789  {
790  btVector3 twistAxis;
791  computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
792 
794  {
795  m_solveTwistLimit = true;
796 
797  m_twistLimitRatio = 1.f;
798  if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
799  {
801  (m_twistSpan - m_twistSpan * m_limitSoftness);
802  }
803 
804  // twist correction tries to get back to soft limit
806 
807  m_twistAxis = quatRotate(qB, -twistAxis);
808 
809  m_kTwist = btScalar(1.) /
810  (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
811  computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
812  }
813 
814  if (m_solveSwingLimit)
815  m_twistAxisA = quatRotate(qA, -twistAxis);
816  }
817  else
818  {
819  m_twistAngle = btScalar(0.f);
820  }
821  }
822 }
823 
824 
825 
826 // given a cone rotation in constraint space, (pre: twist must already be removed)
827 // this method computes its corresponding swing angle and axis.
828 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
830  btScalar& swingAngle, // out
831  btVector3& vSwingAxis, // out
832  btScalar& swingLimit) // out
833 {
834  swingAngle = qCone.getAngle();
835  if (swingAngle > SIMD_EPSILON)
836  {
837  vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
838  vSwingAxis.normalize();
839 #if 0
840  // non-zero twist?! this should never happen.
841  btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
842 #endif
843 
844  // Compute limit for given swing. tricky:
845  // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
846  // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
847 
848  // For starters, compute the direction from center to surface of ellipse.
849  // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
850  // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
851  btScalar xEllipse = vSwingAxis.y();
852  btScalar yEllipse = -vSwingAxis.z();
853 
854  // Now, we use the slope of the vector (using x/yEllipse) and find the length
855  // of the line that intersects the ellipse:
856  // x^2 y^2
857  // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
858  // a^2 b^2
859  // Do the math and it should be clear.
860 
861  swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
862  if (fabs(xEllipse) > SIMD_EPSILON)
863  {
864  btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
865  btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
866  norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
867  btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
868  swingLimit = sqrt(swingLimit2);
869  }
870 
871  // test!
872  /*swingLimit = m_swingSpan2;
873  if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
874  {
875  btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
876  btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
877  btScalar phi = asin(sinphi);
878  btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
879  btScalar alpha = 3.14159f - theta - phi;
880  btScalar sinalpha = sin(alpha);
881  swingLimit = m_swingSpan1 * sinphi/sinalpha;
882  }*/
883  }
884  else if (swingAngle < 0)
885  {
886  // this should never happen!
887 #if 0
888  btAssert(0);
889 #endif
890  }
891 }
892 
894 {
895  // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
896  btScalar xEllipse = btCos(fAngleInRadians);
897  btScalar yEllipse = btSin(fAngleInRadians);
898 
899  // Use the slope of the vector (using x/yEllipse) and find the length
900  // of the line that intersects the ellipse:
901  // x^2 y^2
902  // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
903  // a^2 b^2
904  // Do the math and it should be clear.
905 
906  float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
907  if (fabs(xEllipse) > SIMD_EPSILON)
908  {
909  btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
910  btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
911  norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
912  btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
913  swingLimit = sqrt(swingLimit2);
914  }
915 
916  // convert into point in constraint space:
917  // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
918  btVector3 vSwingAxis(0, xEllipse, -yEllipse);
919  btQuaternion qSwing(vSwingAxis, swingLimit);
920  btVector3 vPointInConstraintSpace(fLength,0,0);
921  return quatRotate(qSwing, vPointInConstraintSpace);
922 }
923 
924 // given a twist rotation in constraint space, (pre: cone must already be removed)
925 // this method computes its corresponding angle and axis.
927  btScalar& twistAngle, // out
928  btVector3& vTwistAxis) // out
929 {
930  btQuaternion qMinTwist = qTwist;
931  twistAngle = qTwist.getAngle();
932 
933  if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
934  {
935  qMinTwist = -(qTwist);
936  twistAngle = qMinTwist.getAngle();
937  }
938  if (twistAngle < 0)
939  {
940  // this should never happen
941 #if 0
942  btAssert(0);
943 #endif
944  }
945 
946  vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
947  if (twistAngle > SIMD_EPSILON)
948  vTwistAxis.normalize();
949 }
950 
951 
953 {
954  // the swing axis is computed as the "twist-free" cone rotation,
955  // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
956  // so, if we're outside the limits, the closest way back inside the cone isn't
957  // along the vector back to the center. better (and more stable) to use the ellipse normal.
958 
959  // convert swing axis to direction from center to surface of ellipse
960  // (ie. rotate 2D vector by PI/2)
961  btScalar y = -vSwingAxis.z();
962  btScalar z = vSwingAxis.y();
963 
964  // do the math...
965  if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
966  {
967  // compute gradient/normal of ellipse surface at current "point"
968  btScalar grad = y/z;
969  grad *= m_swingSpan2 / m_swingSpan1;
970 
971  // adjust y/z to represent normal at point (instead of vector to point)
972  if (y > 0)
973  y = fabs(grad * z);
974  else
975  y = -fabs(grad * z);
976 
977  // convert ellipse direction back to swing axis
978  vSwingAxis.setZ(-y);
979  vSwingAxis.setY( z);
980  vSwingAxis.normalize();
981  }
982 }
983 
984 
985 
987 {
988  //btTransform trACur = m_rbA.getCenterOfMassTransform();
989  //btTransform trBCur = m_rbB.getCenterOfMassTransform();
990 // btTransform trABCur = trBCur.inverse() * trACur;
991 // btQuaternion qABCur = trABCur.getRotation();
992 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
993  //btQuaternion qConstraintCur = trConstraintCur.getRotation();
994 
996  setMotorTargetInConstraintSpace(qConstraint);
997 }
998 
999 
1001 {
1002  m_qTarget = q;
1003 
1004  // clamp motor target to within limits
1005  {
1006  btScalar softness = 1.f;//m_limitSoftness;
1007 
1008  // split into twist and cone
1009  btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
1010  btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
1011  btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
1012 
1013  // clamp cone
1014  if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
1015  {
1016  btScalar swingAngle, swingLimit; btVector3 swingAxis;
1017  computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1018 
1019  if (fabs(swingAngle) > SIMD_EPSILON)
1020  {
1021  if (swingAngle > swingLimit*softness)
1022  swingAngle = swingLimit*softness;
1023  else if (swingAngle < -swingLimit*softness)
1024  swingAngle = -swingLimit*softness;
1025  qTargetCone = btQuaternion(swingAxis, swingAngle);
1026  }
1027  }
1028 
1029  // clamp twist
1030  if (m_twistSpan >= btScalar(0.05f))
1031  {
1032  btScalar twistAngle; btVector3 twistAxis;
1033  computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1034 
1035  if (fabs(twistAngle) > SIMD_EPSILON)
1036  {
1037  // eddy todo: limitSoftness used here???
1038  if (twistAngle > m_twistSpan*softness)
1039  twistAngle = m_twistSpan*softness;
1040  else if (twistAngle < -m_twistSpan*softness)
1041  twistAngle = -m_twistSpan*softness;
1042  qTargetTwist = btQuaternion(twistAxis, twistAngle);
1043  }
1044  }
1045 
1046  m_qTarget = qTargetCone * qTargetTwist;
1047  }
1048 }
1049 
1052 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1053 {
1054  switch(num)
1055  {
1056  case BT_CONSTRAINT_ERP :
1057  case BT_CONSTRAINT_STOP_ERP :
1058  if((axis >= 0) && (axis < 3))
1059  {
1060  m_linERP = value;
1062  }
1063  else
1064  {
1065  m_biasFactor = value;
1066  }
1067  break;
1068  case BT_CONSTRAINT_CFM :
1069  case BT_CONSTRAINT_STOP_CFM :
1070  if((axis >= 0) && (axis < 3))
1071  {
1072  m_linCFM = value;
1074  }
1075  else
1076  {
1077  m_angCFM = value;
1079  }
1080  break;
1081  default:
1083  break;
1084  }
1085 }
1086 
1089 {
1090  btScalar retVal = 0;
1091  switch(num)
1092  {
1093  case BT_CONSTRAINT_ERP :
1094  case BT_CONSTRAINT_STOP_ERP :
1095  if((axis >= 0) && (axis < 3))
1096  {
1098  retVal = m_linERP;
1099  }
1100  else if((axis >= 3) && (axis < 6))
1101  {
1102  retVal = m_biasFactor;
1103  }
1104  else
1105  {
1107  }
1108  break;
1109  case BT_CONSTRAINT_CFM :
1110  case BT_CONSTRAINT_STOP_CFM :
1111  if((axis >= 0) && (axis < 3))
1112  {
1114  retVal = m_linCFM;
1115  }
1116  else if((axis >= 3) && (axis < 6))
1117  {
1119  retVal = m_angCFM;
1120  }
1121  else
1122  {
1124  }
1125  break;
1126  default :
1128  }
1129  return retVal;
1130 }
1131 
1132 
1133 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1134 {
1135  m_rbAFrame = frameA;
1136  m_rbBFrame = frameB;
1137  buildJacobian();
1138  //calculateTransforms();
1139 }
1140 
1141 
1142 
1143 
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btScalar getInvMass() const
Definition: btRigidBody.h:270
void setLimit(int limitIndex, btScalar limitValue)
#define SIMD_EPSILON
Definition: btScalar.h:494
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
btScalar getAngle() const
Return the angle of rotation represented by this quaternion.
Definition: btQuaternion.h:422
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
void setMotorTarget(const btQuaternion &q)
static btVector3 vTwist(1, 0, 0)
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:412
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
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:255
btScalar btSin(btScalar x)
Definition: btScalar.h:451
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyB() const
#define CONETWIST_USE_OBSOLETE_SOLVER
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
void internalGetVelocityInLocalPointObsolete(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:243
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:1272
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:113
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
void internalGetAngularVelocity(btVector3 &angVel) const
Definition: btSolverBody.h:248
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
void updateRHS(btScalar timeStep)
#define SIMD_PI
Definition: btScalar.h:476
#define SIMD_INFINITY
Definition: btScalar.h:495
btVector3 m_linearJointAxis
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:898
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:339
btScalar getDiagonal() const
btScalar btAtan2Fast(btScalar y, btScalar x)
Definition: btScalar.h:502
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:351
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:453
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
btJacobianEntry m_jac[3]
TypedConstraint is the baseclass for Bullet constraints and vehicles.
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:518
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
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:29
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
void getInfo1NonVirtual(btConstraintInfo1 *info)
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:294
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:845
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyA() const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
#define btAssertConstrParams(_par)
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
btScalar btCos(btScalar x)
Definition: btScalar.h:450
btScalar btFabs(btScalar x)
Definition: btScalar.h:449
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579