Bullet Collision Detection & Physics Library
btGeneric6DofConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15/*
162007-09-09
17Refactored by Francisco Le?n
18email: projectileman@yahoo.com
19http://gimpact.sf.net
20*/
21
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
38btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
40, m_frameInA(frameInA)
41, m_frameInB(frameInB),
42m_useLinearReferenceFrameA(useLinearReferenceFrameA),
43m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
44m_flags(0),
45m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
46{
48}
49
50
51
52btGeneric6DofConstraint::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{
63}
64
65
66
67
68#define GENERIC_D6_DISABLE_WARMSTARTING 1
69
70
71
72btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
73btScalar 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
83bool 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
123{
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;
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;
147 return 2;
148 };
149
150 m_currentLimit = 0;//Free from violation
151 return 0;
152
153}
154
155
156
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
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
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
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);
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
354{
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.
373
374 m_calculatedAxis[1] = axis2.cross(axis0);
377
381
382}
383
385{
387}
388
390{
396 { // get weight factors depending on masses
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
416 btJacobianEntry & jacLinear,const btVector3 & normalWorld,
417 const btVector3 & pivotAInW,const btVector3 & pivotBInW)
418{
419 new (&jacLinear) btJacobianEntry(
422 pivotAInW - m_rbA.getCenterOfMassPosition(),
423 pivotBInW - m_rbB.getCenterOfMassPosition(),
424 normalWorld,
428 m_rbB.getInvMass());
429}
430
431
432
434 btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
435{
436 new (&jacAngular) btJacobianEntry(jointAxisW,
441
442}
443
444
445
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
459{
460#ifndef __SPU__
462 {
463
464 // Clear accumulated impulses for the next simulation step
466 int i;
467 for(i = 0; i < 3; i++)
468 {
470 }
471 //calculates transform
473
474 // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
475 // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
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 {
489 {
491 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
492 else
493 normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
494
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
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
521{
523 {
524 info->m_numConstraintRows = 0;
525 info->nub = 0;
526 } else
527 {
528 //prepare constraint
530 info->m_numConstraintRows = 0;
531 info->nub = 6;
532 int i;
533 //test linear limits
534 for(i = 0; i < 3; i++)
535 {
537 {
538 info->m_numConstraintRows++;
539 info->nub--;
540 }
541 }
542 //test angular limits
543 for (i=0;i<3 ;i++ )
544 {
546 {
547 info->m_numConstraintRows++;
548 info->nub--;
549 }
550 }
551 }
552}
553
555{
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
570{
572
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
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
594void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
595{
596
598 //prepare constraint
599 calculateTransforms(transA,transB);
600
601 int i;
602 for (i=0;i<3 ;i++ )
603 {
605 }
606
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
621int 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
626 for (int i=0;i<3 ;i++ )
627 {
629 { // re-use rotational motor code
630 limot.m_bounce = btScalar(0.f);
639 limot.m_maxLimitForce = btScalar(0.f);
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;
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
669int 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
704{
705 (void)timeStep;
706
707}
708
709
711{
712 m_frameInA = frameA;
713 m_frameInB = frameB;
716}
717
718
719
721{
722 return m_calculatedAxis[axis_index];
723}
724
725
727{
728 return m_calculatedLinearDiff[axisIndex];
729}
730
731
733{
734 return m_calculatedAxisAngleDiff[axisIndex];
735}
736
737
738
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 }
754 m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
755 return;
756}
757
758
759
761{
764 for(int i = 0; i < 3; i++)
765 {
768 }
769}
770
771
772
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 bool 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 : info->m_J2linearAxis;
785 J1[srow+0] = ax1[0];
786 J1[srow+1] = ax1[1];
787 J1[srow+2] = ax1[2];
788
789 J2[srow+0] = -ax1[0];
790 J2[srow+1] = -ax1[1];
791 J2[srow+2] = -ax1[2];
792
793 if((!rotational))
794 {
796 {
797 btVector3 tmpA, tmpB, relA, relB;
798 // get vector from bodyB to frameB in WCS
799 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
800 // get its projection to constraint axis
801 btVector3 projB = ax1 * relB.dot(ax1);
802 // get vector directed from bodyB to constraint axis (and orthogonal to it)
803 btVector3 orthoB = relB - projB;
804 // same for bodyA
805 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
806 btVector3 projA = ax1 * relA.dot(ax1);
807 btVector3 orthoA = relA - projA;
808 // get desired offset between frames A and B along constraint axis
809 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
810 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
811 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
812 // get offset vectors relA and relB
813 relA = orthoA + totalDist * m_factA;
814 relB = orthoB - totalDist * m_factB;
815 tmpA = relA.cross(ax1);
816 tmpB = relB.cross(ax1);
817 if(m_hasStaticBody && (!rotAllowed))
818 {
819 tmpA *= m_factA;
820 tmpB *= m_factB;
821 }
822 int i;
823 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
824 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
825 } else
826 {
827 btVector3 ltd; // Linear Torque Decoupling vector
829 ltd = c.cross(ax1);
830 info->m_J1angularAxis[srow+0] = ltd[0];
831 info->m_J1angularAxis[srow+1] = ltd[1];
832 info->m_J1angularAxis[srow+2] = ltd[2];
833
835 ltd = -c.cross(ax1);
836 info->m_J2angularAxis[srow+0] = ltd[0];
837 info->m_J2angularAxis[srow+1] = ltd[1];
838 info->m_J2angularAxis[srow+2] = ltd[2];
839 }
840 }
841 // if we're limited low and high simultaneously, the joint motor is
842 // ineffective
843 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
844 info->m_constraintError[srow] = btScalar(0.f);
845 if (powered)
846 {
847 info->cfm[srow] = limot->m_normalCFM;
848 if(!limit)
849 {
850 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
851
852 btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
853 limot->m_loLimit,
854 limot->m_hiLimit,
855 tag_vel,
856 info->fps * limot->m_stopERP);
857 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
858 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
859 info->m_upperLimit[srow] = limot->m_maxMotorForce;
860 }
861 }
862 if(limit)
863 {
864 btScalar k = info->fps * limot->m_stopERP;
865 if(!rotational)
866 {
867 info->m_constraintError[srow] += k * limot->m_currentLimitError;
868 }
869 else
870 {
871 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
872 }
873 info->cfm[srow] = limot->m_stopCFM;
874 if (limot->m_loLimit == limot->m_hiLimit)
875 { // limited low and high simultaneously
876 info->m_lowerLimit[srow] = -SIMD_INFINITY;
877 info->m_upperLimit[srow] = SIMD_INFINITY;
878 }
879 else
880 {
881 if (limit == 1)
882 {
883 info->m_lowerLimit[srow] = 0;
884 info->m_upperLimit[srow] = SIMD_INFINITY;
885 }
886 else
887 {
888 info->m_lowerLimit[srow] = -SIMD_INFINITY;
889 info->m_upperLimit[srow] = 0;
890 }
891 // deal with bounce
892 if (limot->m_bounce > 0)
893 {
894 // calculate joint velocity
895 btScalar vel;
896 if (rotational)
897 {
898 vel = angVelA.dot(ax1);
899//make sure that if no body -> angVelB == zero vec
900// if (body1)
901 vel -= angVelB.dot(ax1);
902 }
903 else
904 {
905 vel = linVelA.dot(ax1);
906//make sure that if no body -> angVelB == zero vec
907// if (body1)
908 vel -= linVelB.dot(ax1);
909 }
910 // only apply bounce if the velocity is incoming, and if the
911 // resulting c[] exceeds what we already have.
912 if (limit == 1)
913 {
914 if (vel < 0)
915 {
916 btScalar newc = -limot->m_bounce* vel;
917 if (newc > info->m_constraintError[srow])
918 info->m_constraintError[srow] = newc;
919 }
920 }
921 else
922 {
923 if (vel > 0)
924 {
925 btScalar newc = -limot->m_bounce * vel;
926 if (newc < info->m_constraintError[srow])
927 info->m_constraintError[srow] = newc;
928 }
929 }
930 }
931 }
932 }
933 return 1;
934 }
935 else return 0;
936}
937
938
939
940
941
942
945void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
946{
947 if((axis >= 0) && (axis < 3))
948 {
949 switch(num)
950 {
952 m_linearLimits.m_stopERP[axis] = value;
954 break;
956 m_linearLimits.m_stopCFM[axis] = value;
958 break;
959 case BT_CONSTRAINT_CFM :
960 m_linearLimits.m_normalCFM[axis] = value;
962 break;
963 default :
965 }
966 }
967 else if((axis >=3) && (axis < 6))
968 {
969 switch(num)
970 {
972 m_angularLimits[axis - 3].m_stopERP = value;
974 break;
976 m_angularLimits[axis - 3].m_stopCFM = value;
978 break;
979 case BT_CONSTRAINT_CFM :
980 m_angularLimits[axis - 3].m_normalCFM = value;
982 break;
983 default :
985 }
986 }
987 else
988 {
990 }
991}
992
995{
996 btScalar retVal = 0;
997 if((axis >= 0) && (axis < 3))
998 {
999 switch(num)
1000 {
1003 retVal = m_linearLimits.m_stopERP[axis];
1004 break;
1007 retVal = m_linearLimits.m_stopCFM[axis];
1008 break;
1009 case BT_CONSTRAINT_CFM :
1011 retVal = m_linearLimits.m_normalCFM[axis];
1012 break;
1013 default :
1015 }
1016 }
1017 else if((axis >=3) && (axis < 6))
1018 {
1019 switch(num)
1020 {
1023 retVal = m_angularLimits[axis - 3].m_stopERP;
1024 break;
1027 retVal = m_angularLimits[axis - 3].m_stopCFM;
1028 break;
1029 case BT_CONSTRAINT_CFM :
1031 retVal = m_angularLimits[axis - 3].m_normalCFM;
1032 break;
1033 default :
1035 }
1036 }
1037 else
1038 {
1040 }
1041 return retVal;
1042}
1043
1044
1045
1047{
1048 btVector3 zAxis = axis1.normalized();
1049 btVector3 yAxis = axis2.normalized();
1050 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1051
1052 btTransform frameInW;
1053 frameInW.setIdentity();
1054 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
1055 xAxis[1], yAxis[1], zAxis[1],
1056 xAxis[2], yAxis[2], zAxis[2]);
1057
1058 // now get constraint frame in local coordinate systems
1061
1063}
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
#define D6_USE_FRAME_OFFSET
#define D6_USE_OBSOLETE_METHOD
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
@ BT_6DOF_FLAGS_ERP_STOP
@ BT_6DOF_FLAGS_CFM_STOP
@ BT_6DOF_FLAGS_CFM_NORM
#define BT_6DOF_FLAGS_AXIS_SHIFT
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
#define SIMD_PI
Definition: btScalar.h:504
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
#define SIMD_INFINITY
Definition: btScalar.h:522
#define SIMD_EPSILON
Definition: btScalar.h:521
btScalar btAsin(btScalar x)
Definition: btScalar.h:487
#define SIMD_HALF_PI
Definition: btScalar.h:506
#define SIMD_2_PI
Definition: btScalar.h:505
#define btAssert(x)
Definition: btScalar.h:131
static T sum(const btAlignedObjectArray< T > &items)
#define btAssertConstrParams(_par)
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ D6_CONSTRAINT_TYPE
btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis locatio...
btTransform m_frameInA
relative_frames
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
btJacobianEntry m_jacLinear[3]
Jacobians.
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btRotationalLimitMotor * getRotationalLimitMotor(int index)
Retrieves the angular limit informacion.
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
btJacobianEntry m_jacAng[3]
3 orthogonal angular constraints
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
btTransform m_frameInB
the constraint space w.r.t body B
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)
void updateRHS(btScalar timeStep)
virtual void buildJacobian()
performs Jacobian calculation, and also calculates angle differences and axis
void buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
void calculateAngleInfo()
calcs the euler angles between the two bodies.
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
bool testAngularLimitMotor(int axis_index)
Test angular limit.
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
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)
btTranslationalLimitMotor m_linearLimits
Linear_Limit_parameters.
btRotationalLimitMotor m_angularLimits[3]
hinge_parameters
void setFrames(const btTransform &frameA, const btTransform &frameB)
void getInfo1NonVirtual(btConstraintInfo1 *info)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:198
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
void applyTorqueImpulse(const btVector3 &torque)
Definition: btRigidBody.h:329
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
btScalar getInvMass() const
Definition: btRigidBody.h:273
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:297
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:334
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:365
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:354
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:362
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
limit_parameters
btScalar m_currentLimitError
temp_variables
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.
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
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_stopCFM
Constraint force mixing factor when joint is at limit.
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
Linear_Limit_parameters.
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
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309