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