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