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