Bullet Collision Detection & Physics Library
btHingeConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 
16 
17 #include "btHingeConstraint.h"
20 #include "LinearMath/btMinMax.h"
21 #include <new>
22 #include "btSolverBody.h"
23 
24 
25 
26 //#define HINGE_USE_OBSOLETE_SOLVER false
27 #define HINGE_USE_OBSOLETE_SOLVER false
28 
29 #define HINGE_USE_FRAME_OFFSET true
30 
31 #ifndef __SPU__
32 
33 
34 
35 
36 
38  const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
41  m_limit(),
42 #endif
43  m_angularOnly(false),
44  m_enableAngularMotor(false),
45  m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
46  m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
47  m_useReferenceFrameA(useReferenceFrameA),
48  m_flags(0),
49  m_normalCFM(0),
50  m_normalERP(0),
51  m_stopCFM(0),
52  m_stopERP(0)
53 {
54  m_rbAFrame.getOrigin() = pivotInA;
55 
56  // since no frame is given, assume this to be zero angle and just pick rb transform axis
58 
59  btVector3 rbAxisA2;
60  btScalar projection = axisInA.dot(rbAxisA1);
61  if (projection >= 1.0f - SIMD_EPSILON) {
62  rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
63  rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
64  } else if (projection <= -1.0f + SIMD_EPSILON) {
65  rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
66  rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
67  } else {
68  rbAxisA2 = axisInA.cross(rbAxisA1);
69  rbAxisA1 = rbAxisA2.cross(axisInA);
70  }
71 
72  m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
73  rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
74  rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
75 
76  btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
77  btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
78  btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
79 
80  m_rbBFrame.getOrigin() = pivotInB;
81  m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
82  rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
83  rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
84 
85 #ifndef _BT_USE_CENTER_LIMIT_
86  //start with free
87  m_lowerLimit = btScalar(1.0f);
88  m_upperLimit = btScalar(-1.0f);
89  m_biasFactor = 0.3f;
90  m_relaxationFactor = 1.0f;
91  m_limitSoftness = 0.9f;
92  m_solveLimit = false;
93 #endif
95 }
96 
97 
98 
99 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
102 m_limit(),
103 #endif
104 m_angularOnly(false), m_enableAngularMotor(false),
107 m_useReferenceFrameA(useReferenceFrameA),
108 m_flags(0),
109 m_normalCFM(0),
110 m_normalERP(0),
111 m_stopCFM(0),
112 m_stopERP(0)
113 {
114 
115  // since no frame is given, assume this to be zero angle and just pick rb transform axis
116  // fixed axis in worldspace
117  btVector3 rbAxisA1, rbAxisA2;
118  btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
119 
120  m_rbAFrame.getOrigin() = pivotInA;
121  m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
122  rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
123  rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
124 
125  btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
126 
127  btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
128  btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
129  btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
130 
131 
132  m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
133  m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
134  rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
135  rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
136 
137 #ifndef _BT_USE_CENTER_LIMIT_
138  //start with free
139  m_lowerLimit = btScalar(1.0f);
140  m_upperLimit = btScalar(-1.0f);
141  m_biasFactor = 0.3f;
142  m_relaxationFactor = 1.0f;
143  m_limitSoftness = 0.9f;
144  m_solveLimit = false;
145 #endif
147 }
148 
149 
150 
152  const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
153 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
155 m_limit(),
156 #endif
157 m_angularOnly(false),
158 m_enableAngularMotor(false),
161 m_useReferenceFrameA(useReferenceFrameA),
162 m_flags(0),
163 m_normalCFM(0),
164 m_normalERP(0),
165 m_stopCFM(0),
166 m_stopERP(0)
167 {
168 #ifndef _BT_USE_CENTER_LIMIT_
169  //start with free
170  m_lowerLimit = btScalar(1.0f);
171  m_upperLimit = btScalar(-1.0f);
172  m_biasFactor = 0.3f;
173  m_relaxationFactor = 1.0f;
174  m_limitSoftness = 0.9f;
175  m_solveLimit = false;
176 #endif
178 }
179 
180 
181 
182 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
185 m_limit(),
186 #endif
187 m_angularOnly(false),
188 m_enableAngularMotor(false),
191 m_useReferenceFrameA(useReferenceFrameA),
192 m_flags(0),
193 m_normalCFM(0),
194 m_normalERP(0),
195 m_stopCFM(0),
196 m_stopERP(0)
197 {
199 
201 #ifndef _BT_USE_CENTER_LIMIT_
202  //start with free
203  m_lowerLimit = btScalar(1.0f);
204  m_upperLimit = btScalar(-1.0f);
205  m_biasFactor = 0.3f;
206  m_relaxationFactor = 1.0f;
207  m_limitSoftness = 0.9f;
208  m_solveLimit = false;
209 #endif
211 }
212 
213 
214 
216 {
218  {
221 
222  if (!m_angularOnly)
223  {
226  btVector3 relPos = pivotBInW - pivotAInW;
227 
228  btVector3 normal[3];
229  if (relPos.length2() > SIMD_EPSILON)
230  {
231  normal[0] = relPos.normalized();
232  }
233  else
234  {
235  normal[0].setValue(btScalar(1.0),0,0);
236  }
237 
238  btPlaneSpace1(normal[0], normal[1], normal[2]);
239 
240  for (int i=0;i<3;i++)
241  {
242  new (&m_jac[i]) btJacobianEntry(
245  pivotAInW - m_rbA.getCenterOfMassPosition(),
246  pivotBInW - m_rbB.getCenterOfMassPosition(),
247  normal[i],
249  m_rbA.getInvMass(),
251  m_rbB.getInvMass());
252  }
253  }
254 
255  //calculate two perpendicular jointAxis, orthogonal to hingeAxis
256  //these two jointAxis require equal angular velocities for both bodies
257 
258  //this is unused for now, it's a todo
259  btVector3 jointAxis0local;
260  btVector3 jointAxis1local;
261 
262  btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
263 
264  btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
265  btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
267 
268  new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
273 
274  new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
279 
280  new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
285 
286  // clear accumulator
288 
289  // test angular limit
291 
292  //Compute K = J*W*J' for hinge axis
296 
297  }
298 }
299 
300 
301 #endif //__SPU__
302 
303 
305 {
306  return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
307 }
308 
309 
310 
312 {
314  btNormalizeAnglePositive(accAngle)));
315  return result;
316 }
317 
319 {
320  btScalar tol(0.3);
321  btScalar result = btShortestAngularDistance(accAngle, curAngle);
322 
323  if (btFabs(result) > tol)
324  return curAngle;
325  else
326  return accAngle + result;
327 
328  return curAngle;
329 }
330 
331 
333 {
334  btScalar hingeAngle = getHingeAngle();
335  m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
336  return m_accumulatedAngle;
337 }
339 {
340  m_accumulatedAngle = accAngle;
341 }
342 
344 {
345  //update m_accumulatedAngle
346  btScalar curHingeAngle = getHingeAngle();
347  m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
348 
350 
351 }
352 
353 
355 {
356 
357 
359  {
360  info->m_numConstraintRows = 0;
361  info->nub = 0;
362  }
363  else
364  {
365  info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
366  info->nub = 1;
367  //always add the row, to avoid computation (data is not available yet)
368  //prepare constraint
371  {
372  info->m_numConstraintRows++; // limit 3rd anguar as well
373  info->nub--;
374  }
375 
376  }
377 }
378 
380 {
382  {
383  info->m_numConstraintRows = 0;
384  info->nub = 0;
385  }
386  else
387  {
388  //always add the 'limit' row, to avoid computation (data is not available yet)
389  info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
390  info->nub = 0;
391  }
392 }
393 
395 {
397  {
399  }
400  else
401  {
403  }
404 }
405 
406 
407 void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
408 {
410  testLimit(transA,transB);
411 
412  getInfo2Internal(info,transA,transB,angVelA,angVelB);
413 }
414 
415 
416 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
417 {
418 
420  int i, skip = info->rowskip;
421  // transforms in world space
422  btTransform trA = transA*m_rbAFrame;
423  btTransform trB = transB*m_rbBFrame;
424  // pivot point
425  btVector3 pivotAInW = trA.getOrigin();
426  btVector3 pivotBInW = trB.getOrigin();
427 #if 0
428  if (0)
429  {
430  for (i=0;i<6;i++)
431  {
432  info->m_J1linearAxis[i*skip]=0;
433  info->m_J1linearAxis[i*skip+1]=0;
434  info->m_J1linearAxis[i*skip+2]=0;
435 
436  info->m_J1angularAxis[i*skip]=0;
437  info->m_J1angularAxis[i*skip+1]=0;
438  info->m_J1angularAxis[i*skip+2]=0;
439 
440  info->m_J2linearAxis[i*skip]=0;
441  info->m_J2linearAxis[i*skip+1]=0;
442  info->m_J2linearAxis[i*skip+2]=0;
443 
444  info->m_J2angularAxis[i*skip]=0;
445  info->m_J2angularAxis[i*skip+1]=0;
446  info->m_J2angularAxis[i*skip+2]=0;
447 
448  info->m_constraintError[i*skip]=0.f;
449  }
450  }
451 #endif //#if 0
452  // linear (all fixed)
453 
454  if (!m_angularOnly)
455  {
456  info->m_J1linearAxis[0] = 1;
457  info->m_J1linearAxis[skip + 1] = 1;
458  info->m_J1linearAxis[2 * skip + 2] = 1;
459 
460  info->m_J2linearAxis[0] = -1;
461  info->m_J2linearAxis[skip + 1] = -1;
462  info->m_J2linearAxis[2 * skip + 2] = -1;
463  }
464 
465 
466 
467 
468  btVector3 a1 = pivotAInW - transA.getOrigin();
469  {
470  btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
471  btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
472  btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
473  btVector3 a1neg = -a1;
474  a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
475  }
476  btVector3 a2 = pivotBInW - transB.getOrigin();
477  {
478  btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
479  btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
480  btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
481  a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
482  }
483  // linear RHS
484  btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
485 
486  btScalar k = info->fps * normalErp;
487  if (!m_angularOnly)
488  {
489  for(i = 0; i < 3; i++)
490  {
491  info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
492  }
493  }
494  // make rotations around X and Y equal
495  // the hinge axis should be the only unconstrained
496  // rotational axis, the angular velocity of the two bodies perpendicular to
497  // the hinge axis should be equal. thus the constraint equations are
498  // p*w1 - p*w2 = 0
499  // q*w1 - q*w2 = 0
500  // where p and q are unit vectors normal to the hinge axis, and w1 and w2
501  // are the angular velocity vectors of the two bodies.
502  // get hinge axis (Z)
503  btVector3 ax1 = trA.getBasis().getColumn(2);
504  // get 2 orthos to hinge axis (X, Y)
505  btVector3 p = trA.getBasis().getColumn(0);
506  btVector3 q = trA.getBasis().getColumn(1);
507  // set the two hinge angular rows
508  int s3 = 3 * info->rowskip;
509  int s4 = 4 * info->rowskip;
510 
511  info->m_J1angularAxis[s3 + 0] = p[0];
512  info->m_J1angularAxis[s3 + 1] = p[1];
513  info->m_J1angularAxis[s3 + 2] = p[2];
514  info->m_J1angularAxis[s4 + 0] = q[0];
515  info->m_J1angularAxis[s4 + 1] = q[1];
516  info->m_J1angularAxis[s4 + 2] = q[2];
517 
518  info->m_J2angularAxis[s3 + 0] = -p[0];
519  info->m_J2angularAxis[s3 + 1] = -p[1];
520  info->m_J2angularAxis[s3 + 2] = -p[2];
521  info->m_J2angularAxis[s4 + 0] = -q[0];
522  info->m_J2angularAxis[s4 + 1] = -q[1];
523  info->m_J2angularAxis[s4 + 2] = -q[2];
524  // compute the right hand side of the constraint equation. set relative
525  // body velocities along p and q to bring the hinge back into alignment.
526  // if ax1,ax2 are the unit length hinge axes as computed from body1 and
527  // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
528  // if `theta' is the angle between ax1 and ax2, we need an angular velocity
529  // along u to cover angle erp*theta in one step :
530  // |angular_velocity| = angle/time = erp*theta / stepsize
531  // = (erp*fps) * theta
532  // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
533  // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
534  // ...as ax1 and ax2 are unit length. if theta is smallish,
535  // theta ~= sin(theta), so
536  // angular_velocity = (erp*fps) * (ax1 x ax2)
537  // ax1 x ax2 is in the plane space of ax1, so we project the angular
538  // velocity to p and q to find the right hand side.
539  btVector3 ax2 = trB.getBasis().getColumn(2);
540  btVector3 u = ax1.cross(ax2);
541  info->m_constraintError[s3] = k * u.dot(p);
542  info->m_constraintError[s4] = k * u.dot(q);
543  // check angular limits
544  int nrow = 4; // last filled row
545  int srow;
546  btScalar limit_err = btScalar(0.0);
547  int limit = 0;
548  if(getSolveLimit())
549  {
550 #ifdef _BT_USE_CENTER_LIMIT_
551  limit_err = m_limit.getCorrection() * m_referenceSign;
552 #else
553  limit_err = m_correction * m_referenceSign;
554 #endif
555  limit = (limit_err > btScalar(0.0)) ? 1 : 2;
556 
557  }
558  // if the hinge has joint limits or motor, add in the extra row
559  int powered = 0;
561  {
562  powered = 1;
563  }
564  if(limit || powered)
565  {
566  nrow++;
567  srow = nrow * info->rowskip;
568  info->m_J1angularAxis[srow+0] = ax1[0];
569  info->m_J1angularAxis[srow+1] = ax1[1];
570  info->m_J1angularAxis[srow+2] = ax1[2];
571 
572  info->m_J2angularAxis[srow+0] = -ax1[0];
573  info->m_J2angularAxis[srow+1] = -ax1[1];
574  info->m_J2angularAxis[srow+2] = -ax1[2];
575 
576  btScalar lostop = getLowerLimit();
577  btScalar histop = getUpperLimit();
578  if(limit && (lostop == histop))
579  { // the joint motor is ineffective
580  powered = 0;
581  }
582  info->m_constraintError[srow] = btScalar(0.0f);
583  btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
584  if(powered)
585  {
587  {
588  info->cfm[srow] = m_normalCFM;
589  }
590  btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
591  info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
592  info->m_lowerLimit[srow] = - m_maxMotorImpulse;
593  info->m_upperLimit[srow] = m_maxMotorImpulse;
594  }
595  if(limit)
596  {
597  k = info->fps * currERP;
598  info->m_constraintError[srow] += k * limit_err;
600  {
601  info->cfm[srow] = m_stopCFM;
602  }
603  if(lostop == histop)
604  {
605  // limited low and high simultaneously
606  info->m_lowerLimit[srow] = -SIMD_INFINITY;
607  info->m_upperLimit[srow] = SIMD_INFINITY;
608  }
609  else if(limit == 1)
610  { // low limit
611  info->m_lowerLimit[srow] = 0;
612  info->m_upperLimit[srow] = SIMD_INFINITY;
613  }
614  else
615  { // high limit
616  info->m_lowerLimit[srow] = -SIMD_INFINITY;
617  info->m_upperLimit[srow] = 0;
618  }
619  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
620 #ifdef _BT_USE_CENTER_LIMIT_
622 #else
623  btScalar bounce = m_relaxationFactor;
624 #endif
625  if(bounce > btScalar(0.0))
626  {
627  btScalar vel = angVelA.dot(ax1);
628  vel -= angVelB.dot(ax1);
629  // only apply bounce if the velocity is incoming, and if the
630  // resulting c[] exceeds what we already have.
631  if(limit == 1)
632  { // low limit
633  if(vel < 0)
634  {
635  btScalar newc = -bounce * vel;
636  if(newc > info->m_constraintError[srow])
637  {
638  info->m_constraintError[srow] = newc;
639  }
640  }
641  }
642  else
643  { // high limit - all those computations are reversed
644  if(vel > 0)
645  {
646  btScalar newc = -bounce * vel;
647  if(newc < info->m_constraintError[srow])
648  {
649  info->m_constraintError[srow] = newc;
650  }
651  }
652  }
653  }
654 #ifdef _BT_USE_CENTER_LIMIT_
655  info->m_constraintError[srow] *= m_limit.getBiasFactor();
656 #else
657  info->m_constraintError[srow] *= m_biasFactor;
658 #endif
659  } // if(limit)
660  } // if angular limit or powered
661 }
662 
663 
664 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
665 {
666  m_rbAFrame = frameA;
667  m_rbBFrame = frameB;
668  buildJacobian();
669 }
670 
671 
673 {
674  (void)timeStep;
675 
676 }
677 
678 
679 
680 
682 {
684 }
685 
687 {
688  const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
689  const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
690  const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
691 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
692  btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
693  return m_referenceSign * angle;
694 }
695 
696 
697 
698 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
699 {
700  // Compute limit information
701  m_hingeAngle = getHingeAngle(transA,transB);
702 #ifdef _BT_USE_CENTER_LIMIT_
704 #else
705  m_correction = btScalar(0.);
706  m_limitSign = btScalar(0.);
707  m_solveLimit = false;
708  if (m_lowerLimit <= m_upperLimit)
709  {
710  m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
711  if (m_hingeAngle <= m_lowerLimit)
712  {
713  m_correction = (m_lowerLimit - m_hingeAngle);
714  m_limitSign = 1.0f;
715  m_solveLimit = true;
716  }
717  else if (m_hingeAngle >= m_upperLimit)
718  {
719  m_correction = m_upperLimit - m_hingeAngle;
720  m_limitSign = -1.0f;
721  m_solveLimit = true;
722  }
723  }
724 #endif
725  return;
726 }
727 
728 
729 static btVector3 vHinge(0, 0, btScalar(1));
730 
732 {
733  // convert target from body to constraint space
734  btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
735  qConstraint.normalize();
736 
737  // extract "pure" hinge component
738  btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
739  btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
740  btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
741  qHinge.normalize();
742 
743  // compute angular target, clamped to limits
744  btScalar targetAngle = qHinge.getAngle();
745  if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
746  {
747  qHinge = -(qHinge);
748  targetAngle = qHinge.getAngle();
749  }
750  if (qHinge.getZ() < 0)
751  targetAngle = -targetAngle;
752 
753  setMotorTarget(targetAngle, dt);
754 }
755 
757 {
758 #ifdef _BT_USE_CENTER_LIMIT_
759  m_limit.fit(targetAngle);
760 #else
761  if (m_lowerLimit < m_upperLimit)
762  {
763  if (targetAngle < m_lowerLimit)
764  targetAngle = m_lowerLimit;
765  else if (targetAngle > m_upperLimit)
766  targetAngle = m_upperLimit;
767  }
768 #endif
769  // compute angular velocity
771  btScalar dAngle = targetAngle - curAngle;
772  m_motorTargetVelocity = dAngle / dt;
773 }
774 
775 
776 
777 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
778 {
780  int i, s = info->rowskip;
781  // transforms in world space
782  btTransform trA = transA*m_rbAFrame;
783  btTransform trB = transB*m_rbBFrame;
784  // pivot point
785 // btVector3 pivotAInW = trA.getOrigin();
786 // btVector3 pivotBInW = trB.getOrigin();
787 #if 1
788  // difference between frames in WCS
789  btVector3 ofs = trB.getOrigin() - trA.getOrigin();
790  // now get weight factors depending on masses
793  bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
794  btScalar miS = miA + miB;
795  btScalar factA, factB;
796  if(miS > btScalar(0.f))
797  {
798  factA = miB / miS;
799  }
800  else
801  {
802  factA = btScalar(0.5f);
803  }
804  factB = btScalar(1.0f) - factA;
805  // get the desired direction of hinge axis
806  // as weighted sum of Z-orthos of frameA and frameB in WCS
807  btVector3 ax1A = trA.getBasis().getColumn(2);
808  btVector3 ax1B = trB.getBasis().getColumn(2);
809  btVector3 ax1 = ax1A * factA + ax1B * factB;
810  ax1.normalize();
811  // fill first 3 rows
812  // we want: velA + wA x relA == velB + wB x relB
813  btTransform bodyA_trans = transA;
814  btTransform bodyB_trans = transB;
815  int s0 = 0;
816  int s1 = s;
817  int s2 = s * 2;
818  int nrow = 2; // last filled row
819  btVector3 tmpA, tmpB, relA, relB, p, q;
820  // get vector from bodyB to frameB in WCS
821  relB = trB.getOrigin() - bodyB_trans.getOrigin();
822  // get its projection to hinge axis
823  btVector3 projB = ax1 * relB.dot(ax1);
824  // get vector directed from bodyB to hinge axis (and orthogonal to it)
825  btVector3 orthoB = relB - projB;
826  // same for bodyA
827  relA = trA.getOrigin() - bodyA_trans.getOrigin();
828  btVector3 projA = ax1 * relA.dot(ax1);
829  btVector3 orthoA = relA - projA;
830  btVector3 totalDist = projA - projB;
831  // get offset vectors relA and relB
832  relA = orthoA + totalDist * factA;
833  relB = orthoB - totalDist * factB;
834  // now choose average ortho to hinge axis
835  p = orthoB * factA + orthoA * factB;
836  btScalar len2 = p.length2();
837  if(len2 > SIMD_EPSILON)
838  {
839  p /= btSqrt(len2);
840  }
841  else
842  {
843  p = trA.getBasis().getColumn(1);
844  }
845  // make one more ortho
846  q = ax1.cross(p);
847  // fill three rows
848  tmpA = relA.cross(p);
849  tmpB = relB.cross(p);
850  for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
851  for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
852  tmpA = relA.cross(q);
853  tmpB = relB.cross(q);
854  if(hasStaticBody && getSolveLimit())
855  { // to make constraint between static and dynamic objects more rigid
856  // remove wA (or wB) from equation if angular limit is hit
857  tmpB *= factB;
858  tmpA *= factA;
859  }
860  for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
861  for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
862  tmpA = relA.cross(ax1);
863  tmpB = relB.cross(ax1);
864  if(hasStaticBody)
865  { // to make constraint between static and dynamic objects more rigid
866  // remove wA (or wB) from equation
867  tmpB *= factB;
868  tmpA *= factA;
869  }
870  for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
871  for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
872 
873  btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
874  btScalar k = info->fps * normalErp;
875 
876  if (!m_angularOnly)
877  {
878  for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
879  for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
880  for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
881 
882  for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
883  for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
884  for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
885 
886  // compute three elements of right hand side
887 
888  btScalar rhs = k * p.dot(ofs);
889  info->m_constraintError[s0] = rhs;
890  rhs = k * q.dot(ofs);
891  info->m_constraintError[s1] = rhs;
892  rhs = k * ax1.dot(ofs);
893  info->m_constraintError[s2] = rhs;
894  }
895  // the hinge axis should be the only unconstrained
896  // rotational axis, the angular velocity of the two bodies perpendicular to
897  // the hinge axis should be equal. thus the constraint equations are
898  // p*w1 - p*w2 = 0
899  // q*w1 - q*w2 = 0
900  // where p and q are unit vectors normal to the hinge axis, and w1 and w2
901  // are the angular velocity vectors of the two bodies.
902  int s3 = 3 * s;
903  int s4 = 4 * s;
904  info->m_J1angularAxis[s3 + 0] = p[0];
905  info->m_J1angularAxis[s3 + 1] = p[1];
906  info->m_J1angularAxis[s3 + 2] = p[2];
907  info->m_J1angularAxis[s4 + 0] = q[0];
908  info->m_J1angularAxis[s4 + 1] = q[1];
909  info->m_J1angularAxis[s4 + 2] = q[2];
910 
911  info->m_J2angularAxis[s3 + 0] = -p[0];
912  info->m_J2angularAxis[s3 + 1] = -p[1];
913  info->m_J2angularAxis[s3 + 2] = -p[2];
914  info->m_J2angularAxis[s4 + 0] = -q[0];
915  info->m_J2angularAxis[s4 + 1] = -q[1];
916  info->m_J2angularAxis[s4 + 2] = -q[2];
917  // compute the right hand side of the constraint equation. set relative
918  // body velocities along p and q to bring the hinge back into alignment.
919  // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
920  // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
921  // if "theta" is the angle between ax1 and ax2, we need an angular velocity
922  // along u to cover angle erp*theta in one step :
923  // |angular_velocity| = angle/time = erp*theta / stepsize
924  // = (erp*fps) * theta
925  // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
926  // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
927  // ...as ax1 and ax2 are unit length. if theta is smallish,
928  // theta ~= sin(theta), so
929  // angular_velocity = (erp*fps) * (ax1 x ax2)
930  // ax1 x ax2 is in the plane space of ax1, so we project the angular
931  // velocity to p and q to find the right hand side.
932  k = info->fps * normalErp;//??
933 
934  btVector3 u = ax1A.cross(ax1B);
935  info->m_constraintError[s3] = k * u.dot(p);
936  info->m_constraintError[s4] = k * u.dot(q);
937 #endif
938  // check angular limits
939  nrow = 4; // last filled row
940  int srow;
941  btScalar limit_err = btScalar(0.0);
942  int limit = 0;
943  if(getSolveLimit())
944  {
945 #ifdef _BT_USE_CENTER_LIMIT_
946  limit_err = m_limit.getCorrection() * m_referenceSign;
947 #else
948  limit_err = m_correction * m_referenceSign;
949 #endif
950  limit = (limit_err > btScalar(0.0)) ? 1 : 2;
951 
952  }
953  // if the hinge has joint limits or motor, add in the extra row
954  int powered = 0;
956  {
957  powered = 1;
958  }
959  if(limit || powered)
960  {
961  nrow++;
962  srow = nrow * info->rowskip;
963  info->m_J1angularAxis[srow+0] = ax1[0];
964  info->m_J1angularAxis[srow+1] = ax1[1];
965  info->m_J1angularAxis[srow+2] = ax1[2];
966 
967  info->m_J2angularAxis[srow+0] = -ax1[0];
968  info->m_J2angularAxis[srow+1] = -ax1[1];
969  info->m_J2angularAxis[srow+2] = -ax1[2];
970 
971  btScalar lostop = getLowerLimit();
972  btScalar histop = getUpperLimit();
973  if(limit && (lostop == histop))
974  { // the joint motor is ineffective
975  powered = 0;
976  }
977  info->m_constraintError[srow] = btScalar(0.0f);
978  btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
979  if(powered)
980  {
982  {
983  info->cfm[srow] = m_normalCFM;
984  }
985  btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
986  info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
987  info->m_lowerLimit[srow] = - m_maxMotorImpulse;
988  info->m_upperLimit[srow] = m_maxMotorImpulse;
989  }
990  if(limit)
991  {
992  k = info->fps * currERP;
993  info->m_constraintError[srow] += k * limit_err;
995  {
996  info->cfm[srow] = m_stopCFM;
997  }
998  if(lostop == histop)
999  {
1000  // limited low and high simultaneously
1001  info->m_lowerLimit[srow] = -SIMD_INFINITY;
1002  info->m_upperLimit[srow] = SIMD_INFINITY;
1003  }
1004  else if(limit == 1)
1005  { // low limit
1006  info->m_lowerLimit[srow] = 0;
1007  info->m_upperLimit[srow] = SIMD_INFINITY;
1008  }
1009  else
1010  { // high limit
1011  info->m_lowerLimit[srow] = -SIMD_INFINITY;
1012  info->m_upperLimit[srow] = 0;
1013  }
1014  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
1015 #ifdef _BT_USE_CENTER_LIMIT_
1017 #else
1018  btScalar bounce = m_relaxationFactor;
1019 #endif
1020  if(bounce > btScalar(0.0))
1021  {
1022  btScalar vel = angVelA.dot(ax1);
1023  vel -= angVelB.dot(ax1);
1024  // only apply bounce if the velocity is incoming, and if the
1025  // resulting c[] exceeds what we already have.
1026  if(limit == 1)
1027  { // low limit
1028  if(vel < 0)
1029  {
1030  btScalar newc = -bounce * vel;
1031  if(newc > info->m_constraintError[srow])
1032  {
1033  info->m_constraintError[srow] = newc;
1034  }
1035  }
1036  }
1037  else
1038  { // high limit - all those computations are reversed
1039  if(vel > 0)
1040  {
1041  btScalar newc = -bounce * vel;
1042  if(newc < info->m_constraintError[srow])
1043  {
1044  info->m_constraintError[srow] = newc;
1045  }
1046  }
1047  }
1048  }
1049 #ifdef _BT_USE_CENTER_LIMIT_
1050  info->m_constraintError[srow] *= m_limit.getBiasFactor();
1051 #else
1052  info->m_constraintError[srow] *= m_biasFactor;
1053 #endif
1054  } // if(limit)
1055  } // if angular limit or powered
1056 }
1057 
1058 
1061 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1062 {
1063  if((axis == -1) || (axis == 5))
1064  {
1065  switch(num)
1066  {
1067  case BT_CONSTRAINT_STOP_ERP :
1068  m_stopERP = value;
1070  break;
1071  case BT_CONSTRAINT_STOP_CFM :
1072  m_stopCFM = value;
1074  break;
1075  case BT_CONSTRAINT_CFM :
1076  m_normalCFM = value;
1078  break;
1079  case BT_CONSTRAINT_ERP:
1080  m_normalERP = value;
1082  break;
1083  default :
1085  }
1086  }
1087  else
1088  {
1090  }
1091 }
1092 
1094 btScalar btHingeConstraint::getParam(int num, int axis) const
1095 {
1096  btScalar retVal = 0;
1097  if((axis == -1) || (axis == 5))
1098  {
1099  switch(num)
1100  {
1101  case BT_CONSTRAINT_STOP_ERP :
1103  retVal = m_stopERP;
1104  break;
1105  case BT_CONSTRAINT_STOP_CFM :
1107  retVal = m_stopCFM;
1108  break;
1109  case BT_CONSTRAINT_CFM :
1111  retVal = m_normalCFM;
1112  break;
1113  case BT_CONSTRAINT_ERP:
1115  retVal = m_normalERP;
1116  break;
1117  default :
1119  }
1120  }
1121  else
1122  {
1124  }
1125  return retVal;
1126 }
1127 
1128 
btScalar getInvMass() const
Definition: btRigidBody.h:270
#define SIMD_EPSILON
Definition: btScalar.h:494
#define HINGE_USE_OBSOLETE_SOLVER
void getInfo2Internal(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
btScalar getAngle() const
Return the angle of rotation represented by this quaternion.
Definition: btQuaternion.h:415
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...
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:412
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
void setAccumulatedHingeAngle(btScalar accAngle)
btJacobianEntry m_jac[3]
btTransform m_rbAFrame
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
#define btAssert(x)
Definition: btScalar.h:113
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
const btRigidBody & getRigidBodyA() const
btScalar m_motorTargetVelocity
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
static btVector3 vHinge(0, 0, btScalar(1))
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
#define SIMD_PI
Definition: btScalar.h:476
#define SIMD_INFINITY
Definition: btScalar.h:495
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btScalar getUpperLimit() const
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:880
#define _BT_USE_CENTER_LIMIT_
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:332
btTransform m_rbBFrame
void updateRHS(btScalar timeStep)
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btScalar getBiasFactor() const
Returns limit&#39;s bias factor.
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:362
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
#define HINGE_USE_FRAME_OFFSET
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:198
btScalar getLowerLimit() const
void getInfo1NonVirtual(btConstraintInfo1 *info)
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:351
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:446
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
virtual void buildJacobian()
internal method used by the constraint solver, don&#39;t use them directly
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btScalar btNormalizeAngle(btScalar angleInRadians)
Definition: btScalar.h:724
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btJacobianEntry m_jacAng[3]
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
btScalar getRelaxationFactor() const
Returns limit&#39;s relaxation factor.
btScalar btFmod(btScalar x, btScalar y)
Definition: btScalar.h:472
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don&#39;t use them directly
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don&#39;t use them directly
void fit(btScalar &angle) const
Checks given angle against limit.
static btScalar btNormalizeAnglePositive(btScalar angle)
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:294
btScalar getCorrection() const
Returns correction value evaluated when test() was invoked.
void setFrames(const btTransform &frameA, const btTransform &frameB)
void setMotorTarget(const btQuaternion &qAinB, btScalar dt)
void test(const btScalar angle)
Checks conastaint angle against limit.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
btScalar getHingeAngle()
The getHingeAngle gives the hinge angle in range [-PI,PI].
const btRigidBody & getRigidBodyB() const
btAngularLimit m_limit
#define btAssertConstrParams(_par)
void testLimit(const btTransform &transA, const btTransform &transB)
btHingeConstraint(btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btVector3 &axisInA, const btVector3 &axisInB, bool useReferenceFrameA=false)
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:106
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btScalar btFabs(btScalar x)
Definition: btScalar.h:449