Blender  V3.3
btGeneric6DofConstraint.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 2007-09-09
17 Refactored by Francisco Le?n
18 email: projectileman@yahoo.com
19 http://gimpact.sf.net
20 */
21 
26 #include <new>
27 
28 #define D6_USE_OBSOLETE_METHOD false
29 #define D6_USE_FRAME_OFFSET true
30 
33 {
35 }
36 
40  m_useLinearReferenceFrameA(useLinearReferenceFrameB),
42  m_flags(0),
44 {
48 }
49 
50 #define GENERIC_D6_DISABLE_WARMSTARTING 1
51 
52 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
53 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
54 {
55  int i = index % 3;
56  int j = index / 3;
57  return mat[i][j];
58 }
59 
61 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
62 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
63 {
64  // // rot = cy*cz -cy*sz sy
65  // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
66  // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
67  //
68 
69  btScalar fi = btGetMatrixElem(mat, 2);
70  if (fi < btScalar(1.0f))
71  {
72  if (fi > btScalar(-1.0f))
73  {
74  xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
75  xyz[1] = btAsin(btGetMatrixElem(mat, 2));
76  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
77  return true;
78  }
79  else
80  {
81  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
82  xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
83  xyz[1] = -SIMD_HALF_PI;
84  xyz[2] = btScalar(0.0);
85  return false;
86  }
87  }
88  else
89  {
90  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
91  xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
92  xyz[1] = SIMD_HALF_PI;
93  xyz[2] = 0.0;
94  }
95  return false;
96 }
97 
99 
101 {
102  if (m_loLimit > m_hiLimit)
103  {
104  m_currentLimit = 0; //Free from violation
105  return 0;
106  }
107  if (test_value < m_loLimit)
108  {
109  m_currentLimit = 1; //low limit violation
110  m_currentLimitError = test_value - m_loLimit;
113  else if (m_currentLimitError < -SIMD_PI)
115  return 1;
116  }
117  else if (test_value > m_hiLimit)
118  {
119  m_currentLimit = 2; //High limit violation
120  m_currentLimitError = test_value - m_hiLimit;
123  else if (m_currentLimitError < -SIMD_PI)
125  return 2;
126  };
127 
128  m_currentLimit = 0; //Free from violation
129  return 0;
130 }
131 
133  btScalar timeStep, btVector3& axis, btScalar jacDiagABInv,
134  btRigidBody* body0, btRigidBody* body1)
135 {
136  if (needApplyTorques() == false) return 0.0f;
137 
138  btScalar target_velocity = m_targetVelocity;
139  btScalar maxMotorForce = m_maxMotorForce;
140 
141  //current error correction
142  if (m_currentLimit != 0)
143  {
144  target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
145  maxMotorForce = m_maxLimitForce;
146  }
147 
148  maxMotorForce *= timeStep;
149 
150  // current velocity difference
151 
152  btVector3 angVelA = body0->getAngularVelocity();
153  btVector3 angVelB = body1->getAngularVelocity();
154 
155  btVector3 vel_diff;
156  vel_diff = angVelA - angVelB;
157 
158  btScalar rel_vel = axis.dot(vel_diff);
159 
160  // correction velocity
161  btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
162 
163  if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON)
164  {
165  return 0.0f; //no need for applying force
166  }
167 
168  // correction impulse
169  btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
170 
171  // clip correction impulse
172  btScalar clippedMotorImpulse;
173 
175  if (unclippedMotorImpulse > 0.0f)
176  {
177  clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
178  }
179  else
180  {
181  clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
182  }
183 
184  // sort with accumulated impulses
187 
188  btScalar oldaccumImpulse = m_accumulatedImpulse;
189  btScalar sum = oldaccumImpulse + clippedMotorImpulse;
190  m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
191 
192  clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
193 
194  btVector3 motorImp = clippedMotorImpulse * axis;
195 
196  body0->applyTorqueImpulse(motorImp);
197  body1->applyTorqueImpulse(-motorImp);
198 
199  return clippedMotorImpulse;
200 }
201 
203 
205 
207 {
208  btScalar loLimit = m_lowerLimit[limitIndex];
209  btScalar hiLimit = m_upperLimit[limitIndex];
210  if (loLimit > hiLimit)
211  {
212  m_currentLimit[limitIndex] = 0; //Free from violation
213  m_currentLimitError[limitIndex] = btScalar(0.f);
214  return 0;
215  }
216 
217  if (test_value < loLimit)
218  {
219  m_currentLimit[limitIndex] = 2; //low limit violation
220  m_currentLimitError[limitIndex] = test_value - loLimit;
221  return 2;
222  }
223  else if (test_value > hiLimit)
224  {
225  m_currentLimit[limitIndex] = 1; //High limit violation
226  m_currentLimitError[limitIndex] = test_value - hiLimit;
227  return 1;
228  };
229 
230  m_currentLimit[limitIndex] = 0; //Free from violation
231  m_currentLimitError[limitIndex] = btScalar(0.f);
232  return 0;
233 }
234 
236  btScalar timeStep,
237  btScalar jacDiagABInv,
238  btRigidBody& body1, const btVector3& pointInA,
239  btRigidBody& body2, const btVector3& pointInB,
240  int limit_index,
241  const btVector3& axis_normal_on_a,
242  const btVector3& anchorPos)
243 {
245  // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
246  // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
247  btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
248  btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
249 
250  btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
251  btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
252  btVector3 vel = vel1 - vel2;
253 
254  btScalar rel_vel = axis_normal_on_a.dot(vel);
255 
257 
258  //positional error (zeroth order error)
259  btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
262 
263  btScalar minLimit = m_lowerLimit[limit_index];
264  btScalar maxLimit = m_upperLimit[limit_index];
265 
266  //handle the limits
267  if (minLimit < maxLimit)
268  {
269  {
270  if (depth > maxLimit)
271  {
272  depth -= maxLimit;
273  lo = btScalar(0.);
274  }
275  else
276  {
277  if (depth < minLimit)
278  {
279  depth -= minLimit;
280  hi = btScalar(0.);
281  }
282  else
283  {
284  return 0.0f;
285  }
286  }
287  }
288  }
289 
290  btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
291 
292  btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
293  btScalar sum = oldNormalImpulse + normalImpulse;
294  m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
295  normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
296 
297  btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
298  body1.applyImpulse(impulse_vector, rel_pos1);
299  body2.applyImpulse(-impulse_vector, rel_pos2);
300 
301  return normalImpulse;
302 }
303 
305 
307 {
308  btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
310  // in euler angle mode we do not actually constrain the angular velocity
311  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
312  //
313  // to get constrain w2-w1 along ...not
314  // ------ --------------------- ------
315  // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
316  // d(angle[1])/dt = 0 ax[1]
317  // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
318  //
319  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
320  // to prove the result for angle[0], write the expression for angle[0] from
321  // GetInfo1 then take the derivative. to prove this for angle[2] it is
322  // easier to take the euler rate expression for d(angle[2])/dt with respect
323  // to the components of w and set that to 0.
324  btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
325  btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
326 
327  m_calculatedAxis[1] = axis2.cross(axis0);
328  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
329  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
330 
331  m_calculatedAxis[0].normalize();
332  m_calculatedAxis[1].normalize();
333  m_calculatedAxis[2].normalize();
334 }
335 
337 {
339 }
340 
342 {
348  { // get weight factors depending on masses
351  m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
352  btScalar miS = miA + miB;
353  if (miS > btScalar(0.f))
354  {
355  m_factA = miB / miS;
356  }
357  else
358  {
359  m_factA = btScalar(0.5f);
360  }
361  m_factB = btScalar(1.0f) - m_factA;
362  }
363 }
364 
366  btJacobianEntry& jacLinear, const btVector3& normalWorld,
367  const btVector3& pivotAInW, const btVector3& pivotBInW)
368 {
369  new (&jacLinear) btJacobianEntry(
370  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
371  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
372  pivotAInW - m_rbA.getCenterOfMassPosition(),
373  pivotBInW - m_rbB.getCenterOfMassPosition(),
374  normalWorld,
376  m_rbA.getInvMass(),
378  m_rbB.getInvMass());
379 }
380 
382  btJacobianEntry& jacAngular, const btVector3& jointAxisW)
383 {
384  new (&jacAngular) btJacobianEntry(jointAxisW,
385  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
386  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
389 }
390 
392 {
394  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
396  //test limits
397  m_angularLimits[axis_index].testLimitValue(angle);
398  return m_angularLimits[axis_index].needApplyTorques();
399 }
400 
402 {
403 #ifndef __SPU__
405  {
406  // Clear accumulated impulses for the next simulation step
408  int i;
409  for (i = 0; i < 3; i++)
410  {
412  }
413  //calculates transform
415 
416  // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
417  // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
418  calcAnchorPos();
419  btVector3 pivotAInW = m_AnchorPos;
420  btVector3 pivotBInW = m_AnchorPos;
421 
422  // not used here
423  // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
424  // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
425 
426  btVector3 normalWorld;
427  //linear part
428  for (i = 0; i < 3; i++)
429  {
430  if (m_linearLimits.isLimited(i))
431  {
433  normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
434  else
435  normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
436 
438  m_jacLinear[i], normalWorld,
439  pivotAInW, pivotBInW);
440  }
441  }
442 
443  // angular part
444  for (i = 0; i < 3; i++)
445  {
446  //calculates error angle
447  if (testAngularLimitMotor(i))
448  {
449  normalWorld = this->getAxis(i);
450  // Create angular atom
451  buildAngularJacobian(m_jacAng[i], normalWorld);
452  }
453  }
454  }
455 #endif //__SPU__
456 }
457 
459 {
461  {
462  info->m_numConstraintRows = 0;
463  info->nub = 0;
464  }
465  else
466  {
467  //prepare constraint
469  info->m_numConstraintRows = 0;
470  info->nub = 6;
471  int i;
472  //test linear limits
473  for (i = 0; i < 3; i++)
474  {
476  {
477  info->m_numConstraintRows++;
478  info->nub--;
479  }
480  }
481  //test angular limits
482  for (i = 0; i < 3; i++)
483  {
484  if (testAngularLimitMotor(i))
485  {
486  info->m_numConstraintRows++;
487  info->nub--;
488  }
489  }
490  }
491 }
492 
494 {
496  {
497  info->m_numConstraintRows = 0;
498  info->nub = 0;
499  }
500  else
501  {
502  //pre-allocate all 6
503  info->m_numConstraintRows = 6;
504  info->nub = 0;
505  }
506 }
507 
509 {
511 
512  const btTransform& transA = m_rbA.getCenterOfMassTransform();
513  const btTransform& transB = m_rbB.getCenterOfMassTransform();
514  const btVector3& linVelA = m_rbA.getLinearVelocity();
515  const btVector3& linVelB = m_rbB.getLinearVelocity();
516  const btVector3& angVelA = m_rbA.getAngularVelocity();
517  const btVector3& angVelB = m_rbB.getAngularVelocity();
518 
520  { // for stability better to solve angular limits first
521  int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
522  setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
523  }
524  else
525  { // leave old version for compatibility
526  int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
527  setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
528  }
529 }
530 
531 void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
532 {
534  //prepare constraint
535  calculateTransforms(transA, transB);
536 
537  int i;
538  for (i = 0; i < 3; i++)
539  {
541  }
542 
544  { // for stability better to solve angular limits first
545  int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
546  setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
547  }
548  else
549  { // leave old version for compatibility
550  int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
551  setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
552  }
553 }
554 
555 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
556 {
557  // int row = 0;
558  //solve linear limits
560  for (int i = 0; i < 3; i++)
561  {
563  { // re-use rotational motor code
564  limot.m_bounce = btScalar(0.f);
573  limot.m_maxLimitForce = btScalar(0.f);
576  btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
577  int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
578  limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
579  limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
580  limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
582  {
583  int indx1 = (i + 1) % 3;
584  int indx2 = (i + 2) % 3;
585  int rotAllowed = 1; // rotations around orthos to current axis
586  if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
587  {
588  rotAllowed = 0;
589  }
590  row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
591  }
592  else
593  {
594  row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
595  }
596  }
597  }
598  return row;
599 }
600 
601 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
602 {
603  btGeneric6DofConstraint* d6constraint = this;
604  int row = row_offset;
605  //solve angular limits
606  for (int i = 0; i < 3; i++)
607  {
608  if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
609  {
610  btVector3 axis = d6constraint->getAxis(i);
611  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
612  if (!(flags & BT_6DOF_FLAGS_CFM_NORM))
613  {
614  m_angularLimits[i].m_normalCFM = info->cfm[0];
615  }
616  if (!(flags & BT_6DOF_FLAGS_CFM_STOP))
617  {
618  m_angularLimits[i].m_stopCFM = info->cfm[0];
619  }
620  if (!(flags & BT_6DOF_FLAGS_ERP_STOP))
621  {
622  m_angularLimits[i].m_stopERP = info->erp;
623  }
624  row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
625  transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
626  }
627  }
628 
629  return row;
630 }
631 
633 {
634  (void)timeStep;
635 }
636 
637 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
638 {
639  m_frameInA = frameA;
640  m_frameInB = frameB;
641  buildJacobian();
643 }
644 
645 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
646 {
647  return m_calculatedAxis[axis_index];
648 }
649 
651 {
652  return m_calculatedLinearDiff[axisIndex];
653 }
654 
655 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
656 {
657  return m_calculatedAxisAngleDiff[axisIndex];
658 }
659 
661 {
662  btScalar imA = m_rbA.getInvMass();
663  btScalar imB = m_rbB.getInvMass();
664  btScalar weight;
665  if (imB == btScalar(0.0))
666  {
667  weight = btScalar(1.0);
668  }
669  else
670  {
671  weight = imA / (imA + imB);
672  }
673  const btVector3& pA = m_calculatedTransformA.getOrigin();
674  const btVector3& pB = m_calculatedTransformB.getOrigin();
675  m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
676  return;
677 }
678 
680 {
683  for (int i = 0; i < 3; i++)
684  {
687  }
688 }
689 
691  btRotationalLimitMotor* limot,
692  const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
693  btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
694 {
695  int srow = row * info->rowskip;
696  bool powered = limot->m_enableMotor;
697  int limit = limot->m_currentLimit;
698  if (powered || limit)
699  { // if the joint is powered, or has joint limits, add in the extra row
700  btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
701  btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
702  J1[srow + 0] = ax1[0];
703  J1[srow + 1] = ax1[1];
704  J1[srow + 2] = ax1[2];
705 
706  J2[srow + 0] = -ax1[0];
707  J2[srow + 1] = -ax1[1];
708  J2[srow + 2] = -ax1[2];
709 
710  if ((!rotational))
711  {
713  {
714  btVector3 tmpA, tmpB, relA, relB;
715  // get vector from bodyB to frameB in WCS
716  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
717  // get its projection to constraint axis
718  btVector3 projB = ax1 * relB.dot(ax1);
719  // get vector directed from bodyB to constraint axis (and orthogonal to it)
720  btVector3 orthoB = relB - projB;
721  // same for bodyA
722  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
723  btVector3 projA = ax1 * relA.dot(ax1);
724  btVector3 orthoA = relA - projA;
725  // get desired offset between frames A and B along constraint axis
726  btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
727  // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
728  btVector3 totalDist = projA + ax1 * desiredOffs - projB;
729  // get offset vectors relA and relB
730  relA = orthoA + totalDist * m_factA;
731  relB = orthoB - totalDist * m_factB;
732  tmpA = relA.cross(ax1);
733  tmpB = relB.cross(ax1);
734  if (m_hasStaticBody && (!rotAllowed))
735  {
736  tmpA *= m_factA;
737  tmpB *= m_factB;
738  }
739  int i;
740  for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
741  for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
742  }
743  else
744  {
745  btVector3 ltd; // Linear Torque Decoupling vector
746  btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
747  ltd = c.cross(ax1);
748  info->m_J1angularAxis[srow + 0] = ltd[0];
749  info->m_J1angularAxis[srow + 1] = ltd[1];
750  info->m_J1angularAxis[srow + 2] = ltd[2];
751 
752  c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
753  ltd = -c.cross(ax1);
754  info->m_J2angularAxis[srow + 0] = ltd[0];
755  info->m_J2angularAxis[srow + 1] = ltd[1];
756  info->m_J2angularAxis[srow + 2] = ltd[2];
757  }
758  }
759  // if we're limited low and high simultaneously, the joint motor is
760  // ineffective
761  if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
762  info->m_constraintError[srow] = btScalar(0.f);
763  if (powered)
764  {
765  info->cfm[srow] = limot->m_normalCFM;
766  if (!limit)
767  {
768  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
769 
770  btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
771  limot->m_loLimit,
772  limot->m_hiLimit,
773  tag_vel,
774  info->fps * limot->m_stopERP);
775  info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
776  info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
777  info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
778  }
779  }
780  if (limit)
781  {
782  btScalar k = info->fps * limot->m_stopERP;
783  if (!rotational)
784  {
785  info->m_constraintError[srow] += k * limot->m_currentLimitError;
786  }
787  else
788  {
789  info->m_constraintError[srow] += -k * limot->m_currentLimitError;
790  }
791  info->cfm[srow] = limot->m_stopCFM;
792  if (limot->m_loLimit == limot->m_hiLimit)
793  { // limited low and high simultaneously
794  info->m_lowerLimit[srow] = -SIMD_INFINITY;
795  info->m_upperLimit[srow] = SIMD_INFINITY;
796  }
797  else
798  {
799  if (limit == 1)
800  {
801  info->m_lowerLimit[srow] = 0;
802  info->m_upperLimit[srow] = SIMD_INFINITY;
803  }
804  else
805  {
806  info->m_lowerLimit[srow] = -SIMD_INFINITY;
807  info->m_upperLimit[srow] = 0;
808  }
809  // deal with bounce
810  if (limot->m_bounce > 0)
811  {
812  // calculate joint velocity
813  btScalar vel;
814  if (rotational)
815  {
816  vel = angVelA.dot(ax1);
817  //make sure that if no body -> angVelB == zero vec
818  // if (body1)
819  vel -= angVelB.dot(ax1);
820  }
821  else
822  {
823  vel = linVelA.dot(ax1);
824  //make sure that if no body -> angVelB == zero vec
825  // if (body1)
826  vel -= linVelB.dot(ax1);
827  }
828  // only apply bounce if the velocity is incoming, and if the
829  // resulting c[] exceeds what we already have.
830  if (limit == 1)
831  {
832  if (vel < 0)
833  {
834  btScalar newc = -limot->m_bounce * vel;
835  if (newc > info->m_constraintError[srow])
836  info->m_constraintError[srow] = newc;
837  }
838  }
839  else
840  {
841  if (vel > 0)
842  {
843  btScalar newc = -limot->m_bounce * vel;
844  if (newc < info->m_constraintError[srow])
845  info->m_constraintError[srow] = newc;
846  }
847  }
848  }
849  }
850  }
851  return 1;
852  }
853  else
854  return 0;
855 }
856 
859 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
860 {
861  if ((axis >= 0) && (axis < 3))
862  {
863  switch (num)
864  {
866  m_linearLimits.m_stopERP[axis] = value;
868  break;
870  m_linearLimits.m_stopCFM[axis] = value;
872  break;
873  case BT_CONSTRAINT_CFM:
874  m_linearLimits.m_normalCFM[axis] = value;
876  break;
877  default:
879  }
880  }
881  else if ((axis >= 3) && (axis < 6))
882  {
883  switch (num)
884  {
886  m_angularLimits[axis - 3].m_stopERP = value;
888  break;
890  m_angularLimits[axis - 3].m_stopCFM = value;
892  break;
893  case BT_CONSTRAINT_CFM:
894  m_angularLimits[axis - 3].m_normalCFM = value;
896  break;
897  default:
899  }
900  }
901  else
902  {
904  }
905 }
906 
908 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
909 {
910  btScalar retVal = 0;
911  if ((axis >= 0) && (axis < 3))
912  {
913  switch (num)
914  {
917  retVal = m_linearLimits.m_stopERP[axis];
918  break;
921  retVal = m_linearLimits.m_stopCFM[axis];
922  break;
923  case BT_CONSTRAINT_CFM:
925  retVal = m_linearLimits.m_normalCFM[axis];
926  break;
927  default:
929  }
930  }
931  else if ((axis >= 3) && (axis < 6))
932  {
933  switch (num)
934  {
937  retVal = m_angularLimits[axis - 3].m_stopERP;
938  break;
941  retVal = m_angularLimits[axis - 3].m_stopCFM;
942  break;
943  case BT_CONSTRAINT_CFM:
945  retVal = m_angularLimits[axis - 3].m_normalCFM;
946  break;
947  default:
949  }
950  }
951  else
952  {
954  }
955  return retVal;
956 }
957 
958 void btGeneric6DofConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
959 {
960  btVector3 zAxis = axis1.normalized();
961  btVector3 yAxis = axis2.normalized();
962  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
963 
964  btTransform frameInW;
965  frameInW.setIdentity();
966  frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
967  xAxis[1], yAxis[1], zAxis[1],
968  xAxis[2], yAxis[2], zAxis[2]);
969 
970  // now get constraint frame in local coordinate systems
971  m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
972  m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
973 
975 }
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
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
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
const btRigidBody & getRigidBodyB() const
int m_flags
void getInfo1NonVirtual(btConstraintInfo1 *info)
btFixedConstraint btRigidBody & rbB
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
#define D6_USE_FRAME_OFFSET
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
#define D6_USE_OBSOLETE_METHOD
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
@ BT_6DOF_FLAGS_ERP_STOP
@ BT_6DOF_FLAGS_CFM_STOP
@ BT_6DOF_FLAGS_CFM_NORM
virtual void calcAnchorPos(void)
btTransform m_calculatedTransformB
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
btScalar m_factA
btScalar m_factB
btVector3 m_calculatedAxis[3]
void calculateLinearInfo()
btVector3 m_calculatedLinearDiff
btJacobianEntry m_jacAng[3]
btRotationalLimitMotor m_angularLimits[3]
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
btVector3 m_AnchorPos
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btJacobianEntry m_jacLinear[3]
3 orthogonal linear constraints
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
bool m_useLinearReferenceFrameA
int get_limit_motor_info2(btRotationalLimitMotor *limot, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB, btConstraintInfo2 *info, int row, btVector3 &ax1, int rotational, int rotAllowed=false)
btTranslationalLimitMotor m_linearLimits
btTransform m_frameInB
void calculateAngleInfo()
calcs the euler angles between the two bodies.
void buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
void calculateTransforms(const btTransform &transA, const btTransform &transB)
Calcs global transform of the offsets.
btVector3 m_calculatedAxisAngleDiff
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
bool testAngularLimitMotor(int axis_index)
Test angular limit.
int setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btTransform m_calculatedTransformA
bool m_hasStaticBody
bool m_useOffsetForConstraintFrame
#define BT_6DOF_FLAGS_AXIS_SHIFT
btJacobianEntry
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
#define SIMD_PI
Definition: btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
Definition: btScalar.h:509
#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
#define SIMD_HALF_PI
Definition: btScalar.h:528
#define SIMD_2_PI
Definition: btScalar.h:527
#define btAssert(x)
Definition: btScalar.h:295
btTransform m_frameInA
static T sum(const btAlignedObjectArray< T > &items)
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
static btRigidBody & getFixedBody()
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ D6_CONSTRAINT_TYPE
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
void applyTorqueImpulse(const btVector3 &torque)
Definition: btRigidBody.h:327
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btScalar getInvMass() const
Definition: btRigidBody.h:263
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:335
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:433
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:423
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
Rotation Limit structure for generic joints.
btScalar m_currentPosition
How much is violated this limit.
btScalar m_targetVelocity
target motor velocity
btScalar m_hiLimit
joint limit
btScalar m_normalCFM
Relaxation factor.
btScalar m_maxMotorForce
max force on motor
btScalar m_bounce
restitution factor
btScalar m_loLimit
joint limit
btScalar m_stopERP
Error tolerance factor when joint is at limit.
bool needApplyTorques() const
Need apply correction.
int testLimitValue(btScalar test_value)
calculates error
btScalar m_maxLimitForce
max force on limit
int m_currentLimit
current value of angle
btScalar solveAngularLimits(btScalar timeStep, btVector3 &axis, btScalar jacDiagABInv, btRigidBody *body0, btRigidBody *body1)
apply the correction impulses for two bodies
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
int m_currentLimit[3]
Current relative offset of constraint frames.
btScalar solveLinearAxis(btScalar timeStep, btScalar jacDiagABInv, btRigidBody &body1, const btVector3 &pointInA, btRigidBody &body2, const btVector3 &pointInB, int limit_index, const btVector3 &axis_normal_on_a, const btVector3 &anchorPos)
btVector3 m_maxMotorForce
max force on motor
int testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_currentLinearDiff
How much is violated this limit.
btVector3 m_normalCFM
Bounce parameter for linear limit.
bool needApplyForce(int limitIndex) const
btScalar m_limitSoftness
Softness for linear limit.
btVector3 m_targetVelocity
target motor velocity
btVector3 m_lowerLimit
the constraint lower limits
bool isLimited(int limitIndex) const
Test limit.
btScalar m_damping
Damping for linear limit.
btVector3 m_upperLimit
the constraint upper limits
SyclQueue void void size_t num_bytes void
static unsigned c
Definition: RandGen.cpp:83
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError