Bullet Collision Detection & Physics Library
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 
22 #include "btGeneric6DofConstraint.h"
23 #include "BulletDynamics/Dynamics/btRigidBody.h"
24 #include "LinearMath/btTransformUtil.h"
25 #include "LinearMath/btTransformUtil.h"
26 #include <new>
27 
28 
29 
30 #define D6_USE_OBSOLETE_METHOD false
31 #define D6_USE_FRAME_OFFSET true
32 
33 
34 
35 
36 
37 
38 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
39 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
40 , m_frameInA(frameInA)
41 , m_frameInB(frameInB),
42 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
43 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
44 m_flags(0),
45 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
46 {
47  calculateTransforms();
48 }
49 
50 
51 
52 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
53  : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
54  m_frameInB(frameInB),
55  m_useLinearReferenceFrameA(useLinearReferenceFrameB),
56  m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
57  m_flags(0),
58  m_useSolveConstraintObsolete(false)
59 {
61  m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
62  calculateTransforms();
63 }
64 
65 
66 
67 
68 #define GENERIC_D6_DISABLE_WARMSTARTING 1
69 
70 
71 
72 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
73 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
74 {
75  int i = index%3;
76  int j = index/3;
77  return mat[i][j];
78 }
79 
80 
81 
83 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
84 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
85 {
86  // // rot = cy*cz -cy*sz sy
87  // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
88  // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
89  //
90 
91  btScalar fi = btGetMatrixElem(mat,2);
92  if (fi < btScalar(1.0f))
93  {
94  if (fi > btScalar(-1.0f))
95  {
96  xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
97  xyz[1] = btAsin(btGetMatrixElem(mat,2));
98  xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
99  return true;
100  }
101  else
102  {
103  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
104  xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
105  xyz[1] = -SIMD_HALF_PI;
106  xyz[2] = btScalar(0.0);
107  return false;
108  }
109  }
110  else
111  {
112  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
113  xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
114  xyz[1] = SIMD_HALF_PI;
115  xyz[2] = 0.0;
116  }
117  return false;
118 }
119 
121 
122 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
123 {
124  if(m_loLimit>m_hiLimit)
125  {
126  m_currentLimit = 0;//Free from violation
127  return 0;
128  }
129  if (test_value < m_loLimit)
130  {
131  m_currentLimit = 1;//low limit violation
132  m_currentLimitError = test_value - m_loLimit;
133  if(m_currentLimitError>SIMD_PI)
134  m_currentLimitError-=SIMD_2_PI;
135  else if(m_currentLimitError<-SIMD_PI)
136  m_currentLimitError+=SIMD_2_PI;
137  return 1;
138  }
139  else if (test_value> m_hiLimit)
140  {
141  m_currentLimit = 2;//High limit violation
142  m_currentLimitError = test_value - m_hiLimit;
143  if(m_currentLimitError>SIMD_PI)
144  m_currentLimitError-=SIMD_2_PI;
145  else if(m_currentLimitError<-SIMD_PI)
146  m_currentLimitError+=SIMD_2_PI;
147  return 2;
148  };
149 
150  m_currentLimit = 0;//Free from violation
151  return 0;
152 
153 }
154 
155 
156 
157 btScalar btRotationalLimitMotor::solveAngularLimits(
158  btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
159  btRigidBody * body0, btRigidBody * body1 )
160 {
161  if (needApplyTorques()==false) return 0.0f;
162 
163  btScalar target_velocity = m_targetVelocity;
164  btScalar maxMotorForce = m_maxMotorForce;
165 
166  //current error correction
167  if (m_currentLimit!=0)
168  {
169  target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
170  maxMotorForce = m_maxLimitForce;
171  }
172 
173  maxMotorForce *= timeStep;
174 
175  // current velocity difference
176 
177  btVector3 angVelA = body0->getAngularVelocity();
178  btVector3 angVelB = body1->getAngularVelocity();
179 
180  btVector3 vel_diff;
181  vel_diff = angVelA-angVelB;
182 
183 
184 
185  btScalar rel_vel = axis.dot(vel_diff);
186 
187  // correction velocity
188  btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
189 
190 
191  if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
192  {
193  return 0.0f;//no need for applying force
194  }
195 
196 
197  // correction impulse
198  btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
199 
200  // clip correction impulse
201  btScalar clippedMotorImpulse;
202 
204  if (unclippedMotorImpulse>0.0f)
205  {
206  clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
207  }
208  else
209  {
210  clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
211  }
212 
213 
214  // sort with accumulated impulses
215  btScalar lo = btScalar(-BT_LARGE_FLOAT);
216  btScalar hi = btScalar(BT_LARGE_FLOAT);
217 
218  btScalar oldaccumImpulse = m_accumulatedImpulse;
219  btScalar sum = oldaccumImpulse + clippedMotorImpulse;
220  m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
221 
222  clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
223 
224  btVector3 motorImp = clippedMotorImpulse * axis;
225 
226  body0->applyTorqueImpulse(motorImp);
227  body1->applyTorqueImpulse(-motorImp);
228 
229  return clippedMotorImpulse;
230 
231 
232 }
233 
235 
236 
237 
238 
240 
241 
242 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
243 {
244  btScalar loLimit = m_lowerLimit[limitIndex];
245  btScalar hiLimit = m_upperLimit[limitIndex];
246  if(loLimit > hiLimit)
247  {
248  m_currentLimit[limitIndex] = 0;//Free from violation
249  m_currentLimitError[limitIndex] = btScalar(0.f);
250  return 0;
251  }
252 
253  if (test_value < loLimit)
254  {
255  m_currentLimit[limitIndex] = 2;//low limit violation
256  m_currentLimitError[limitIndex] = test_value - loLimit;
257  return 2;
258  }
259  else if (test_value> hiLimit)
260  {
261  m_currentLimit[limitIndex] = 1;//High limit violation
262  m_currentLimitError[limitIndex] = test_value - hiLimit;
263  return 1;
264  };
265 
266  m_currentLimit[limitIndex] = 0;//Free from violation
267  m_currentLimitError[limitIndex] = btScalar(0.f);
268  return 0;
269 }
270 
271 
272 
273 btScalar btTranslationalLimitMotor::solveLinearAxis(
274  btScalar timeStep,
275  btScalar jacDiagABInv,
276  btRigidBody& body1,const btVector3 &pointInA,
277  btRigidBody& body2,const btVector3 &pointInB,
278  int limit_index,
279  const btVector3 & axis_normal_on_a,
280  const btVector3 & anchorPos)
281 {
282 
284  // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
285  // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
286  btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
287  btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
288 
289  btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
290  btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
291  btVector3 vel = vel1 - vel2;
292 
293  btScalar rel_vel = axis_normal_on_a.dot(vel);
294 
295 
296 
298 
299  //positional error (zeroth order error)
300  btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
301  btScalar lo = btScalar(-BT_LARGE_FLOAT);
302  btScalar hi = btScalar(BT_LARGE_FLOAT);
303 
304  btScalar minLimit = m_lowerLimit[limit_index];
305  btScalar maxLimit = m_upperLimit[limit_index];
306 
307  //handle the limits
308  if (minLimit < maxLimit)
309  {
310  {
311  if (depth > maxLimit)
312  {
313  depth -= maxLimit;
314  lo = btScalar(0.);
315 
316  }
317  else
318  {
319  if (depth < minLimit)
320  {
321  depth -= minLimit;
322  hi = btScalar(0.);
323  }
324  else
325  {
326  return 0.0f;
327  }
328  }
329  }
330  }
331 
332  btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
333 
334 
335 
336 
337  btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
338  btScalar sum = oldNormalImpulse + normalImpulse;
339  m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
340  normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
341 
342  btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
343  body1.applyImpulse( impulse_vector, rel_pos1);
344  body2.applyImpulse(-impulse_vector, rel_pos2);
345 
346 
347 
348  return normalImpulse;
349 }
350 
352 
353 void btGeneric6DofConstraint::calculateAngleInfo()
354 {
355  btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
356  matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
357  // in euler angle mode we do not actually constrain the angular velocity
358  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
359  //
360  // to get constrain w2-w1 along ...not
361  // ------ --------------------- ------
362  // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
363  // d(angle[1])/dt = 0 ax[1]
364  // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
365  //
366  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
367  // to prove the result for angle[0], write the expression for angle[0] from
368  // GetInfo1 then take the derivative. to prove this for angle[2] it is
369  // easier to take the euler rate expression for d(angle[2])/dt with respect
370  // to the components of w and set that to 0.
371  btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
372  btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
373 
374  m_calculatedAxis[1] = axis2.cross(axis0);
375  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
376  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
377 
378  m_calculatedAxis[0].normalize();
379  m_calculatedAxis[1].normalize();
380  m_calculatedAxis[2].normalize();
381 
382 }
383 
384 void btGeneric6DofConstraint::calculateTransforms()
385 {
386  calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
387 }
388 
389 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
390 {
391  m_calculatedTransformA = transA * m_frameInA;
392  m_calculatedTransformB = transB * m_frameInB;
393  calculateLinearInfo();
394  calculateAngleInfo();
395  if(m_useOffsetForConstraintFrame)
396  { // get weight factors depending on masses
397  btScalar miA = getRigidBodyA().getInvMass();
398  btScalar miB = getRigidBodyB().getInvMass();
399  m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
400  btScalar miS = miA + miB;
401  if(miS > btScalar(0.f))
402  {
403  m_factA = miB / miS;
404  }
405  else
406  {
407  m_factA = btScalar(0.5f);
408  }
409  m_factB = btScalar(1.0f) - m_factA;
410  }
411 }
412 
413 
414 
415 void btGeneric6DofConstraint::buildLinearJacobian(
416  btJacobianEntry & jacLinear,const btVector3 & normalWorld,
417  const btVector3 & pivotAInW,const btVector3 & pivotBInW)
418 {
419  new (&jacLinear) btJacobianEntry(
420  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
421  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
422  pivotAInW - m_rbA.getCenterOfMassPosition(),
423  pivotBInW - m_rbB.getCenterOfMassPosition(),
424  normalWorld,
425  m_rbA.getInvInertiaDiagLocal(),
426  m_rbA.getInvMass(),
427  m_rbB.getInvInertiaDiagLocal(),
428  m_rbB.getInvMass());
429 }
430 
431 
432 
433 void btGeneric6DofConstraint::buildAngularJacobian(
434  btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
435 {
436  new (&jacAngular) btJacobianEntry(jointAxisW,
437  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
438  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
439  m_rbA.getInvInertiaDiagLocal(),
440  m_rbB.getInvInertiaDiagLocal());
441 
442 }
443 
444 
445 
446 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
447 {
448  btScalar angle = m_calculatedAxisAngleDiff[axis_index];
449  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
450  m_angularLimits[axis_index].m_currentPosition = angle;
451  //test limits
452  m_angularLimits[axis_index].testLimitValue(angle);
453  return m_angularLimits[axis_index].needApplyTorques();
454 }
455 
456 
457 
458 void btGeneric6DofConstraint::buildJacobian()
459 {
460 #ifndef __SPU__
461  if (m_useSolveConstraintObsolete)
462  {
463 
464  // Clear accumulated impulses for the next simulation step
465  m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
466  int i;
467  for(i = 0; i < 3; i++)
468  {
469  m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
470  }
471  //calculates transform
472  calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
473 
474  // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
475  // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
476  calcAnchorPos();
477  btVector3 pivotAInW = m_AnchorPos;
478  btVector3 pivotBInW = m_AnchorPos;
479 
480  // not used here
481  // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
482  // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
483 
484  btVector3 normalWorld;
485  //linear part
486  for (i=0;i<3;i++)
487  {
488  if (m_linearLimits.isLimited(i))
489  {
490  if (m_useLinearReferenceFrameA)
491  normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
492  else
493  normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
494 
495  buildLinearJacobian(
496  m_jacLinear[i],normalWorld ,
497  pivotAInW,pivotBInW);
498 
499  }
500  }
501 
502  // angular part
503  for (i=0;i<3;i++)
504  {
505  //calculates error angle
506  if (testAngularLimitMotor(i))
507  {
508  normalWorld = this->getAxis(i);
509  // Create angular atom
510  buildAngularJacobian(m_jacAng[i],normalWorld);
511  }
512  }
513 
514  }
515 #endif //__SPU__
516 
517 }
518 
519 
520 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
521 {
522  if (m_useSolveConstraintObsolete)
523  {
524  info->m_numConstraintRows = 0;
525  info->nub = 0;
526  } else
527  {
528  //prepare constraint
529  calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
530  info->m_numConstraintRows = 0;
531  info->nub = 6;
532  int i;
533  //test linear limits
534  for(i = 0; i < 3; i++)
535  {
536  if(m_linearLimits.needApplyForce(i))
537  {
538  info->m_numConstraintRows++;
539  info->nub--;
540  }
541  }
542  //test angular limits
543  for (i=0;i<3 ;i++ )
544  {
545  if(testAngularLimitMotor(i))
546  {
547  info->m_numConstraintRows++;
548  info->nub--;
549  }
550  }
551  }
552 }
553 
554 void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
555 {
556  if (m_useSolveConstraintObsolete)
557  {
558  info->m_numConstraintRows = 0;
559  info->nub = 0;
560  } else
561  {
562  //pre-allocate all 6
563  info->m_numConstraintRows = 6;
564  info->nub = 0;
565  }
566 }
567 
568 
569 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
570 {
571  btAssert(!m_useSolveConstraintObsolete);
572 
573  const btTransform& transA = m_rbA.getCenterOfMassTransform();
574  const btTransform& transB = m_rbB.getCenterOfMassTransform();
575  const btVector3& linVelA = m_rbA.getLinearVelocity();
576  const btVector3& linVelB = m_rbB.getLinearVelocity();
577  const btVector3& angVelA = m_rbA.getAngularVelocity();
578  const btVector3& angVelB = m_rbB.getAngularVelocity();
579 
580  if(m_useOffsetForConstraintFrame)
581  { // for stability better to solve angular limits first
582  int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
583  setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
584  }
585  else
586  { // leave old version for compatibility
587  int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
588  setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
589  }
590 
591 }
592 
593 
594 void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
595 {
596 
597  btAssert(!m_useSolveConstraintObsolete);
598  //prepare constraint
599  calculateTransforms(transA,transB);
600 
601  int i;
602  for (i=0;i<3 ;i++ )
603  {
604  testAngularLimitMotor(i);
605  }
606 
607  if(m_useOffsetForConstraintFrame)
608  { // for stability better to solve angular limits first
609  int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
610  setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
611  }
612  else
613  { // leave old version for compatibility
614  int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
615  setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
616  }
617 }
618 
619 
620 
621 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)
622 {
623 // int row = 0;
624  //solve linear limits
625  btRotationalLimitMotor limot;
626  for (int i=0;i<3 ;i++ )
627  {
628  if(m_linearLimits.needApplyForce(i))
629  { // re-use rotational motor code
630  limot.m_bounce = btScalar(0.f);
631  limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
632  limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
633  limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
634  limot.m_damping = m_linearLimits.m_damping;
635  limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
636  limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
637  limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
638  limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
639  limot.m_maxLimitForce = btScalar(0.f);
640  limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
641  limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
642  btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
643  int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
644  limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
645  limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
646  limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
647  if(m_useOffsetForConstraintFrame)
648  {
649  int indx1 = (i + 1) % 3;
650  int indx2 = (i + 2) % 3;
651  int rotAllowed = 1; // rotations around orthos to current axis
652  if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
653  {
654  rotAllowed = 0;
655  }
656  row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
657  }
658  else
659  {
660  row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
661  }
662  }
663  }
664  return row;
665 }
666 
667 
668 
669 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)
670 {
671  btGeneric6DofConstraint * d6constraint = this;
672  int row = row_offset;
673  //solve angular limits
674  for (int i=0;i<3 ;i++ )
675  {
676  if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
677  {
678  btVector3 axis = d6constraint->getAxis(i);
679  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
680  if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
681  {
682  m_angularLimits[i].m_normalCFM = info->cfm[0];
683  }
684  if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
685  {
686  m_angularLimits[i].m_stopCFM = info->cfm[0];
687  }
688  if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
689  {
690  m_angularLimits[i].m_stopERP = info->erp;
691  }
692  row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
693  transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
694  }
695  }
696 
697  return row;
698 }
699 
700 
701 
702 
703 void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
704 {
705  (void)timeStep;
706 
707 }
708 
709 
710 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
711 {
712  m_frameInA = frameA;
713  m_frameInB = frameB;
714  buildJacobian();
715  calculateTransforms();
716 }
717 
718 
719 
720 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
721 {
722  return m_calculatedAxis[axis_index];
723 }
724 
725 
726 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
727 {
728  return m_calculatedLinearDiff[axisIndex];
729 }
730 
731 
732 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
733 {
734  return m_calculatedAxisAngleDiff[axisIndex];
735 }
736 
737 
738 
739 void btGeneric6DofConstraint::calcAnchorPos(void)
740 {
741  btScalar imA = m_rbA.getInvMass();
742  btScalar imB = m_rbB.getInvMass();
743  btScalar weight;
744  if(imB == btScalar(0.0))
745  {
746  weight = btScalar(1.0);
747  }
748  else
749  {
750  weight = imA / (imA + imB);
751  }
752  const btVector3& pA = m_calculatedTransformA.getOrigin();
753  const btVector3& pB = m_calculatedTransformB.getOrigin();
754  m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
755  return;
756 }
757 
758 
759 
760 void btGeneric6DofConstraint::calculateLinearInfo()
761 {
762  m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
763  m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
764  for(int i = 0; i < 3; i++)
765  {
766  m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
767  m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
768  }
769 }
770 
771 
772 
773 int btGeneric6DofConstraint::get_limit_motor_info2(
774  btRotationalLimitMotor * limot,
775  const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
776  btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
777 {
778  int srow = row * info->rowskip;
779  int powered = limot->m_enableMotor;
780  int limit = limot->m_currentLimit;
781  if (powered || limit)
782  { // if the joint is powered, or has joint limits, add in the extra row
783  btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
784  btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
785  J1[srow+0] = ax1[0];
786  J1[srow+1] = ax1[1];
787  J1[srow+2] = ax1[2];
788  if(rotational)
789  {
790  J2[srow+0] = -ax1[0];
791  J2[srow+1] = -ax1[1];
792  J2[srow+2] = -ax1[2];
793  }
794  if((!rotational))
795  {
796  if (m_useOffsetForConstraintFrame)
797  {
798  btVector3 tmpA, tmpB, relA, relB;
799  // get vector from bodyB to frameB in WCS
800  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
801  // get its projection to constraint axis
802  btVector3 projB = ax1 * relB.dot(ax1);
803  // get vector directed from bodyB to constraint axis (and orthogonal to it)
804  btVector3 orthoB = relB - projB;
805  // same for bodyA
806  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
807  btVector3 projA = ax1 * relA.dot(ax1);
808  btVector3 orthoA = relA - projA;
809  // get desired offset between frames A and B along constraint axis
810  btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
811  // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
812  btVector3 totalDist = projA + ax1 * desiredOffs - projB;
813  // get offset vectors relA and relB
814  relA = orthoA + totalDist * m_factA;
815  relB = orthoB - totalDist * m_factB;
816  tmpA = relA.cross(ax1);
817  tmpB = relB.cross(ax1);
818  if(m_hasStaticBody && (!rotAllowed))
819  {
820  tmpA *= m_factA;
821  tmpB *= m_factB;
822  }
823  int i;
824  for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
825  for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
826  } else
827  {
828  btVector3 ltd; // Linear Torque Decoupling vector
829  btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
830  ltd = c.cross(ax1);
831  info->m_J1angularAxis[srow+0] = ltd[0];
832  info->m_J1angularAxis[srow+1] = ltd[1];
833  info->m_J1angularAxis[srow+2] = ltd[2];
834 
835  c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
836  ltd = -c.cross(ax1);
837  info->m_J2angularAxis[srow+0] = ltd[0];
838  info->m_J2angularAxis[srow+1] = ltd[1];
839  info->m_J2angularAxis[srow+2] = ltd[2];
840  }
841  }
842  // if we're limited low and high simultaneously, the joint motor is
843  // ineffective
844  if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
845  info->m_constraintError[srow] = btScalar(0.f);
846  if (powered)
847  {
848  info->cfm[srow] = limot->m_normalCFM;
849  if(!limit)
850  {
851  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
852 
853  btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
854  limot->m_loLimit,
855  limot->m_hiLimit,
856  tag_vel,
857  info->fps * limot->m_stopERP);
858  info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
859  info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
860  info->m_upperLimit[srow] = limot->m_maxMotorForce;
861  }
862  }
863  if(limit)
864  {
865  btScalar k = info->fps * limot->m_stopERP;
866  if(!rotational)
867  {
868  info->m_constraintError[srow] += k * limot->m_currentLimitError;
869  }
870  else
871  {
872  info->m_constraintError[srow] += -k * limot->m_currentLimitError;
873  }
874  info->cfm[srow] = limot->m_stopCFM;
875  if (limot->m_loLimit == limot->m_hiLimit)
876  { // limited low and high simultaneously
877  info->m_lowerLimit[srow] = -SIMD_INFINITY;
878  info->m_upperLimit[srow] = SIMD_INFINITY;
879  }
880  else
881  {
882  if (limit == 1)
883  {
884  info->m_lowerLimit[srow] = 0;
885  info->m_upperLimit[srow] = SIMD_INFINITY;
886  }
887  else
888  {
889  info->m_lowerLimit[srow] = -SIMD_INFINITY;
890  info->m_upperLimit[srow] = 0;
891  }
892  // deal with bounce
893  if (limot->m_bounce > 0)
894  {
895  // calculate joint velocity
896  btScalar vel;
897  if (rotational)
898  {
899  vel = angVelA.dot(ax1);
900 //make sure that if no body -> angVelB == zero vec
901 // if (body1)
902  vel -= angVelB.dot(ax1);
903  }
904  else
905  {
906  vel = linVelA.dot(ax1);
907 //make sure that if no body -> angVelB == zero vec
908 // if (body1)
909  vel -= linVelB.dot(ax1);
910  }
911  // only apply bounce if the velocity is incoming, and if the
912  // resulting c[] exceeds what we already have.
913  if (limit == 1)
914  {
915  if (vel < 0)
916  {
917  btScalar newc = -limot->m_bounce* vel;
918  if (newc > info->m_constraintError[srow])
919  info->m_constraintError[srow] = newc;
920  }
921  }
922  else
923  {
924  if (vel > 0)
925  {
926  btScalar newc = -limot->m_bounce * vel;
927  if (newc < info->m_constraintError[srow])
928  info->m_constraintError[srow] = newc;
929  }
930  }
931  }
932  }
933  }
934  return 1;
935  }
936  else return 0;
937 }
938 
939 
940 
941 
942 
943 
946 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
947 {
948  if((axis >= 0) && (axis < 3))
949  {
950  switch(num)
951  {
952  case BT_CONSTRAINT_STOP_ERP :
953  m_linearLimits.m_stopERP[axis] = value;
954  m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
955  break;
956  case BT_CONSTRAINT_STOP_CFM :
957  m_linearLimits.m_stopCFM[axis] = value;
958  m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
959  break;
960  case BT_CONSTRAINT_CFM :
961  m_linearLimits.m_normalCFM[axis] = value;
962  m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
963  break;
964  default :
965  btAssertConstrParams(0);
966  }
967  }
968  else if((axis >=3) && (axis < 6))
969  {
970  switch(num)
971  {
972  case BT_CONSTRAINT_STOP_ERP :
973  m_angularLimits[axis - 3].m_stopERP = value;
974  m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
975  break;
976  case BT_CONSTRAINT_STOP_CFM :
977  m_angularLimits[axis - 3].m_stopCFM = value;
978  m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
979  break;
980  case BT_CONSTRAINT_CFM :
981  m_angularLimits[axis - 3].m_normalCFM = value;
982  m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
983  break;
984  default :
985  btAssertConstrParams(0);
986  }
987  }
988  else
989  {
990  btAssertConstrParams(0);
991  }
992 }
993 
995 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
996 {
997  btScalar retVal = 0;
998  if((axis >= 0) && (axis < 3))
999  {
1000  switch(num)
1001  {
1002  case BT_CONSTRAINT_STOP_ERP :
1003  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1004  retVal = m_linearLimits.m_stopERP[axis];
1005  break;
1006  case BT_CONSTRAINT_STOP_CFM :
1007  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1008  retVal = m_linearLimits.m_stopCFM[axis];
1009  break;
1010  case BT_CONSTRAINT_CFM :
1011  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1012  retVal = m_linearLimits.m_normalCFM[axis];
1013  break;
1014  default :
1015  btAssertConstrParams(0);
1016  }
1017  }
1018  else if((axis >=3) && (axis < 6))
1019  {
1020  switch(num)
1021  {
1022  case BT_CONSTRAINT_STOP_ERP :
1023  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1024  retVal = m_angularLimits[axis - 3].m_stopERP;
1025  break;
1026  case BT_CONSTRAINT_STOP_CFM :
1027  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1028  retVal = m_angularLimits[axis - 3].m_stopCFM;
1029  break;
1030  case BT_CONSTRAINT_CFM :
1031  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1032  retVal = m_angularLimits[axis - 3].m_normalCFM;
1033  break;
1034  default :
1035  btAssertConstrParams(0);
1036  }
1037  }
1038  else
1039  {
1040  btAssertConstrParams(0);
1041  }
1042  return retVal;
1043 }
1044 
1045 
1046 
1047 void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
1048 {
1049  btVector3 zAxis = axis1.normalized();
1050  btVector3 yAxis = axis2.normalized();
1051  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1052 
1053  btTransform frameInW;
1054  frameInW.setIdentity();
1055  frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
1056  xAxis[1], yAxis[1], zAxis[1],
1057  xAxis[2], yAxis[2], zAxis[2]);
1058 
1059  // now get constraint frame in local coordinate systems
1060  m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1061  m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1062 
1063  calculateTransforms();
1064 }
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4