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