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 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
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:291
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:1065
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:294
CONETWIST_DEF_FIX_THRESH
#define CONETWIST_DEF_FIX_THRESH
Definition: btConeTwistConstraint.cpp:26
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:49
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:506
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:28
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:349
btConeTwistConstraint::GetPointForAngle
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
Definition: btConeTwistConstraint.cpp:870
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:286
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:133
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:479
btConeTwistConstraint::calcAngleInfo2
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
Definition: btConeTwistConstraint.cpp:603
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:477
btConeTwistConstraint::m_twistLimitSign
btScalar m_twistLimitSign
Definition: btConeTwistConstraint.h:86
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
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:25
btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
Definition: btConeTwistConstraint.cpp:928
btConeTwistConstraint::computeConeLimitInfo
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
Definition: btConeTwistConstraint.cpp:806
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:478
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:960
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:1026
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:54
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:261
btConeTwistConstraint::setMotorTargetInConstraintSpace
void setMotorTargetInConstraintSpace(const btQuaternion &q)
Definition: btConeTwistConstraint.cpp:973
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:133
btJacobianEntry::getDiagonal
btScalar getDiagonal() const
Definition: btJacobianEntry.h:104
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:524
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:66
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:903
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:552
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:83
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:34
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:103
btConeTwistConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btConeTwistConstraint.cpp:508
btConeTwistConstraint::setFrames
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btConeTwistConstraint.cpp:1109
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:120
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
btConeTwistConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btConeTwistConstraint.cpp:96
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:343
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:108
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:498
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:533
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:1029
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:245
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:401
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:514
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