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 }
static btVector3 vTwist(1, 0, 0)
#define CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_DEF_FIX_THRESH
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:940
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
#define SIMD_PI
Definition: btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
btScalar btSin(btScalar x)
Definition: btScalar.h:499
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btScalar btCos(btScalar x)
Definition: btScalar.h:498
btScalar btAtan2Fast(btScalar y, btScalar x)
Definition: btScalar.h:553
#define SIMD_EPSILON
Definition: btScalar.h:543
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
#define btAssert(x)
Definition: btScalar.h:153
#define btAssertConstrParams(_par)
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ CONETWIST_CONSTRAINT_TYPE
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
void setLimit(int limitIndex, btScalar limitValue)
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
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...
const btRigidBody & getRigidBodyB() const
void getInfo1NonVirtual(btConstraintInfo1 *info)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
const btRigidBody & getRigidBodyA() const
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
btJacobianEntry m_jac[3]
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
void updateRHS(btScalar timeStep)
void setMotorTarget(const btQuaternion &q)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
btScalar getDiagonal() const
btVector3 m_linearJointAxis
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:468
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:493
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:423
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btRigidBody & m_rbA
btRigidBody & m_rbB
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
void internalGetAngularVelocity(btVector3 &angVel) const
Definition: btSolverBody.h:237
void internalGetVelocityInLocalPointObsolete(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:232