Blender  V3.3
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)
38 {
39  init();
40 }
41 
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  {
257  btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
258  btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
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(
276  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
277  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
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  {
297  btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
298  btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
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 
499  bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag);
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 
525  b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
526  b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
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  {
537  b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
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  {
548  b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
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));
567  m_swingAxis.normalize();
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  {
575  btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
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;
588  m_twistAxis.normalize();
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;
596  m_twistAxis.normalize();
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;
628  m_swingAxis.normalize();
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)
639  btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
640  btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
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);
761  m_swingCorrection = m_swingAxis.length();
762 
764  m_swingAxis.normalize();
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);
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);
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;
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 
970  btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
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 
1066 btScalar btConeTwistConstraint::getParam(int num, int axis) const
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 }
sqrt(x)+1/max(0
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
static btVector3 vTwist(1, 0, 0)
#define CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_DEF_FIX_THRESH
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
bool m_solveTwistLimit
btScalar m_twistSpan
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
btTransform m_rbBFrame
virtual void setParam(int num, btScalar value, int axis=-1)
bool m_bMotorEnabled
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
btScalar m_twistLimitSign
btScalar m_accSwingLimitImpulse
btScalar m_accTwistLimitImpulse
btScalar m_swingSpan2
void updateRHS(btScalar timeStep)
btScalar m_swingLimitRatio
bool m_bNormalizedMotorStrength
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
bool m_useSolveConstraintObsolete
btScalar m_maxMotorImpulse
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
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)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
const btRigidBody & getRigidBodyA() const
btScalar m_twistCorrection
btScalar m_twistAngle
bool m_solveSwingLimit
btScalar m_twistLimitRatio
btScalar m_kSwing
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btVector3 m_accMotorImpulse
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
btScalar m_swingCorrection
btQuaternion m_qTarget
btScalar m_limitSoftness
btScalar m_biasFactor
btScalar m_angCFM
void calcAngleInfo()
btTransform m_rbAFrame
const btRigidBody & getRigidBodyB() const
btScalar m_linCFM
btScalar m_linERP
btVector3 m_twistAxis
btScalar m_fixThresh
btScalar m_swingSpan1
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
btScalar m_kTwist
bool m_angularOnly
int m_flags
void setLimit(int limitIndex, btScalar limitValue)
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
btVector3 m_swingAxis
btVector3 m_twistAxisA
btScalar m_damping
btScalar m_relaxationFactor
void getInfo1NonVirtual(btConstraintInfo1 *info)
void setMotorTarget(const btQuaternion &q)
btFixedConstraint btRigidBody & rbB
btJacobianEntry
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
SIMD_FORCE_INLINE btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:940
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
SIMD_FORCE_INLINE btScalar btCos(btScalar x)
Definition: btScalar.h:498
#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
SIMD_FORCE_INLINE btScalar btSin(btScalar x)
Definition: btScalar.h:499
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
Definition: btScalar.h:553
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_FORCE_INLINE
Definition: btScalar.h:280
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btSimdScalar m_appliedImpulse
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btRigidBody & m_rbA
#define btAssertConstrParams(_par)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btRigidBody & m_rbB
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ CONETWIST_CONSTRAINT_TYPE
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
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
SIMD_FORCE_INLINE 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)
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
BLI_INLINE float grad(int hash_val, float x, float y, float z)
Definition: noise.c:270
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError