Bullet Collision Detection & Physics Library
btConeTwistConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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
15Written by: Marcus Hennix
16*/
17
21#include "LinearMath/btMinMax.h"
22#include <cmath>
23#include <new>
24
25//#define CONETWIST_USE_OBSOLETE_SOLVER true
26#define CONETWIST_USE_OBSOLETE_SOLVER false
27#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
28
30{
32 return axis.dot(vec);
33}
34
37 : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
38{
39 init();
40}
41
43 : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
44{
46 m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
47 init();
48}
49
51{
52 m_angularOnly = false;
53 m_solveTwistLimit = false;
54 m_solveSwingLimit = false;
55 m_bMotorEnabled = false;
57
59 m_damping = btScalar(0.01);
61 m_flags = 0;
62 m_linCFM = btScalar(0.f);
63 m_linERP = btScalar(0.7f);
64 m_angCFM = btScalar(0.f);
65}
66
68{
70 {
71 info->m_numConstraintRows = 0;
72 info->nub = 0;
73 }
74 else
75 {
76 info->m_numConstraintRows = 3;
77 info->nub = 3;
80 {
81 info->m_numConstraintRows++;
82 info->nub--;
84 {
85 info->m_numConstraintRows++;
86 info->nub--;
87 }
88 }
90 {
91 info->m_numConstraintRows++;
92 info->nub--;
93 }
94 }
95}
96
98{
99 //always reserve 6 rows: object transform is not available on SPU
100 info->m_numConstraintRows = 6;
101 info->nub = 0;
102}
103
105{
107}
108
110{
112
114 // set jacobian
115 info->m_J1linearAxis[0] = 1;
116 info->m_J1linearAxis[info->rowskip + 1] = 1;
117 info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
118 btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
119 {
122 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
123 btVector3 a1neg = -a1;
125 }
126 info->m_J2linearAxis[0] = -1;
127 info->m_J2linearAxis[info->rowskip + 1] = -1;
128 info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
129 btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
130 {
133 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
135 }
136 // set right hand side
138 btScalar k = info->fps * linERP;
139 int j;
140 for (j = 0; j < 3; j++)
141 {
142 info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
143 info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY;
144 info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY;
146 {
147 info->cfm[j * info->rowskip] = m_linCFM;
148 }
149 }
150 int row = 3;
151 int srow = row * info->rowskip;
153 // angular limits
155 {
156 btScalar* J1 = info->m_J1angularAxis;
157 btScalar* J2 = info->m_J2angularAxis;
159 {
161 btVector3 p = trA.getBasis().getColumn(1);
162 btVector3 q = trA.getBasis().getColumn(2);
163 int srow1 = srow + info->rowskip;
164 J1[srow + 0] = p[0];
165 J1[srow + 1] = p[1];
166 J1[srow + 2] = p[2];
167 J1[srow1 + 0] = q[0];
168 J1[srow1 + 1] = q[1];
169 J1[srow1 + 2] = q[2];
170 J2[srow + 0] = -p[0];
171 J2[srow + 1] = -p[1];
172 J2[srow + 2] = -p[2];
173 J2[srow1 + 0] = -q[0];
174 J2[srow1 + 1] = -q[1];
175 J2[srow1 + 2] = -q[2];
183 srow = srow1 + info->rowskip;
184 }
185 else
186 {
188 J1[srow + 0] = ax1[0];
189 J1[srow + 1] = ax1[1];
190 J1[srow + 2] = ax1[2];
191 J2[srow + 0] = -ax1[0];
192 J2[srow + 1] = -ax1[1];
193 J2[srow + 2] = -ax1[2];
194 btScalar k = info->fps * m_biasFactor;
195
198 {
199 info->cfm[srow] = m_angCFM;
200 }
201 // m_swingCorrection is always positive or 0
202 info->m_lowerLimit[srow] = 0;
204 srow += info->rowskip;
205 }
206 }
208 {
210 btScalar* J1 = info->m_J1angularAxis;
211 btScalar* J2 = info->m_J2angularAxis;
212 J1[srow + 0] = ax1[0];
213 J1[srow + 1] = ax1[1];
214 J1[srow + 2] = ax1[2];
215 J2[srow + 0] = -ax1[0];
216 J2[srow + 1] = -ax1[1];
217 J2[srow + 2] = -ax1[2];
218 btScalar k = info->fps * m_biasFactor;
221 {
222 info->cfm[srow] = m_angCFM;
223 }
224 if (m_twistSpan > 0.0f)
225 {
226 if (m_twistCorrection > 0.0f)
227 {
228 info->m_lowerLimit[srow] = 0;
230 }
231 else
232 {
234 info->m_upperLimit[srow] = 0;
235 }
236 }
237 else
238 {
241 }
242 srow += info->rowskip;
243 }
244}
245
247{
249 {
253 m_accMotorImpulse = btVector3(0., 0., 0.);
254
255 if (!m_angularOnly)
256 {
260
261 btVector3 normal[3];
262 if (relPos.length2() > SIMD_EPSILON)
263 {
264 normal[0] = relPos.normalized();
265 }
266 else
267 {
268 normal[0].setValue(btScalar(1.0), 0, 0);
269 }
270
271 btPlaneSpace1(normal[0], normal[1], normal[2]);
272
273 for (int i = 0; i < 3; i++)
274 {
275 new (&m_jac[i]) btJacobianEntry(
280 normal[i],
284 m_rbB.getInvMass());
285 }
286 }
287
289 }
290}
291
293{
294#ifndef __SPU__
296 {
299
300 btScalar tau = btScalar(0.3);
301
302 //linear part
303 if (!m_angularOnly)
304 {
307
309 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
311 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
313
314 for (int i = 0; i < 3; i++)
315 {
316 const btVector3& normal = m_jac[i].m_linearJointAxis;
318
320 rel_vel = normal.dot(vel);
321 //positional error (zeroth order error)
322 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
325
328 bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse);
329 bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse);
330 }
331 }
332
333 // apply motor
334 if (m_bMotorEnabled)
335 {
336 // compute current and predicted transforms
340 bodyA.internalGetAngularVelocity(omegaA);
342 bodyB.internalGetAngularVelocity(omegaB);
345 btVector3 zerovec(0, 0, 0);
347 trACur, zerovec, omegaA, timeStep, trAPred);
351 trBCur, zerovec, omegaB, timeStep, trBPred);
352
353 // compute desired transforms in world
358
359 // compute desired omegas in world
361
364
365 // compute delta omegas
368
369 // compute weighted avg axis of dOmega (weighting based on inertias)
372
373 if (dOmegaA.length2() > SIMD_EPSILON)
374 {
377 }
378
379 if (dOmegaB.length2() > SIMD_EPSILON)
380 {
381 axisB = dOmegaB.normalized();
383 }
384
386
387 static bool bDoTorque = true;
388 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
389 {
394
397
398 if (m_maxMotorImpulse >= 0)
399 {
403
407 {
408 newUnclampedAccImpulse.normalize();
411 }
413 }
414
417
418 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
419 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
420 }
421 }
422 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
423 {
425 bodyA.internalGetAngularVelocity(angVelA);
427 bodyB.internalGetAngularVelocity(angVelB);
429 if (relVel.length2() > SIMD_EPSILON)
430 {
436
439 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
440 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
441 }
442 }
443
444 // joint limits
445 {
448 bodyA.internalGetAngularVelocity(angVelA);
450 bodyB.internalGetAngularVelocity(angVelB);
451
452 // solve swing limit
454 {
457 if (relSwingVel > 0)
460
461 // Clamp the accumulated impulse
465
467
468 // don't let cone response affect twist
469 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
470 {
474 }
475
478
479 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag);
480 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag);
481 }
482
483 // solve twist limit
485 {
488 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
491
492 // Clamp the accumulated impulse
496
497 // btVector3 impulse = m_twistAxis * impulseMag;
498
499 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag);
500 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag);
501 }
502 }
503 }
504#else
505 btAssert(0);
506#endif //__SPU__
507}
508
510{
511 (void)timeStep;
512}
513
514#ifndef __SPU__
516{
519 m_solveTwistLimit = false;
520 m_solveSwingLimit = false;
521
522 btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
523 btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
524
527
529
530 btScalar swx = btScalar(0.), swy = btScalar(0.);
531 btScalar thresh = btScalar(10.);
533
534 // Get Frame into world space
535 if (m_swingSpan1 >= btScalar(0.05f))
536 {
538 swx = b2Axis1.dot(b1Axis1);
539 swy = b2Axis1.dot(b1Axis2);
541 fact = (swy * swy + swx * swx) * thresh * thresh;
542 fact = fact / (fact + btScalar(1.0));
543 swing1 *= fact;
544 }
545
546 if (m_swingSpan2 >= btScalar(0.05f))
547 {
549 swx = b2Axis1.dot(b1Axis1);
550 swy = b2Axis1.dot(b1Axis3);
552 fact = (swy * swy + swx * swx) * thresh * thresh;
553 fact = fact / (fact + btScalar(1.0));
554 swing2 *= fact;
555 }
556
560
561 if (EllipseAngle > 1.0f)
562 {
564 m_solveSwingLimit = true;
565 // Calculate necessary axis & factors
568 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
570 }
571
572 // Twist limits
573 if (m_twistSpan >= btScalar(0.))
574 {
580
581 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
584 {
586 m_solveTwistLimit = true;
587 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
589 m_twistAxis *= -1.0f;
590 }
592 {
594 m_solveTwistLimit = true;
595 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
597 }
598 }
599}
600#endif //__SPU__
601
602static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space
603
605{
608 m_solveTwistLimit = false;
609 m_solveSwingLimit = false;
610 // compute rotation of A wrt B (in constraint space)
612 { // it is assumed that setMotorTarget() was alredy called
613 // and motor target m_qTarget is within constraint limits
614 // TODO : split rotation to pure swing and pure twist
615 // compute desired transforms in world
620 btQuaternion qDeltaAB = trDeltaAB.getRotation();
622 btScalar swingAxisLen2 = swingAxis.length2();
624 {
625 return;
626 }
629 m_swingCorrection = qDeltaAB.getAngle();
631 {
632 m_solveSwingLimit = true;
633 }
634 return;
635 }
636
637 {
638 // compute rotation of A wrt B (in constraint space)
639 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
640 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
642 // split rotation into cone and twist
643 // (all this is done from B's perspective. Maybe I should be averaging axes...)
645 vConeNoTwist.normalize();
647 qABCone.normalize();
650
652 {
656
658 {
659 m_solveSwingLimit = true;
660
661 // compute limit ratio: 0->1, where
662 // 0 == beginning of soft limit
663 // 1 == hard/real limit
664 m_swingLimitRatio = 1.f;
666 {
669 }
670
671 // swing correction tries to get back to soft limit
673
674 // adjustment of swing axis (based on ellipse normal)
676
677 // Calculate necessary axis & factors
679
680 m_twistAxisA.setValue(0, 0, 0);
681
682 m_kSwing = btScalar(1.) /
685 }
686 }
687 else
688 {
689 // you haven't set any limits;
690 // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
691 // anyway, we have either hinge or fixed joint
692 btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
693 btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
694 btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
695 btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
696 btVector3 target;
697 btScalar x = ivB.dot(ivA);
698 btScalar y = ivB.dot(jvA);
699 btScalar z = ivB.dot(kvA);
701 { // fixed. We'll need to add one more row to constraint
702 if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
703 {
704 m_solveSwingLimit = true;
706 }
707 }
708 else
709 {
711 { // hinge around Y axis
712 // if(!(btFuzzyZero(y)))
713 if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
714 {
715 m_solveSwingLimit = true;
717 {
718 y = btScalar(0.f);
719 btScalar span2 = btAtan2(z, x);
720 if (span2 > m_swingSpan2)
721 {
722 x = btCos(m_swingSpan2);
723 z = btSin(m_swingSpan2);
724 }
725 else if (span2 < -m_swingSpan2)
726 {
727 x = btCos(m_swingSpan2);
728 z = -btSin(m_swingSpan2);
729 }
730 }
731 }
732 }
733 else
734 { // hinge around Z axis
735 // if(!btFuzzyZero(z))
736 if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
737 {
738 m_solveSwingLimit = true;
740 {
741 z = btScalar(0.f);
742 btScalar span1 = btAtan2(y, x);
743 if (span1 > m_swingSpan1)
744 {
745 x = btCos(m_swingSpan1);
746 y = btSin(m_swingSpan1);
747 }
748 else if (span1 < -m_swingSpan1)
749 {
750 x = btCos(m_swingSpan1);
751 y = -btSin(m_swingSpan1);
752 }
753 }
754 }
755 }
756 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
757 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
758 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
759 target.normalize();
760 m_swingAxis = -ivB.cross(target);
762
765 }
766 }
767
768 if (m_twistSpan >= btScalar(0.f))
769 {
772
774 {
775 m_solveTwistLimit = true;
776
777 m_twistLimitRatio = 1.f;
779 {
782 }
783
784 // twist correction tries to get back to soft limit
786
788
789 m_kTwist = btScalar(1.) /
792 }
793
796 }
797 else
798 {
799 m_twistAngle = btScalar(0.f);
800 }
801 }
802}
803
804// given a cone rotation in constraint space, (pre: twist must already be removed)
805// this method computes its corresponding swing angle and axis.
806// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
808 btScalar& swingAngle, // out
809 btVector3& vSwingAxis, // out
810 btScalar& swingLimit) // out
811{
812 swingAngle = qCone.getAngle();
814 {
815 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
816 vSwingAxis.normalize();
817#if 0
818 // non-zero twist?! this should never happen.
820#endif
821
822 // Compute limit for given swing. tricky:
823 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
824 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
825
826 // For starters, compute the direction from center to surface of ellipse.
827 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
828 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
831
832 // Now, we use the slope of the vector (using x/yEllipse) and find the length
833 // of the line that intersects the ellipse:
834 // x^2 y^2
835 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
836 // a^2 b^2
837 // Do the math and it should be clear.
838
839 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
841 {
843 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
845 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
846 swingLimit = std::sqrt(swingLimit2);
847 }
848
849 // test!
850 /*swingLimit = m_swingSpan2;
851 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
852 {
853 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
854 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
855 btScalar phi = asin(sinphi);
856 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
857 btScalar alpha = 3.14159f - theta - phi;
858 btScalar sinalpha = sin(alpha);
859 swingLimit = m_swingSpan1 * sinphi/sinalpha;
860 }*/
861 }
862 else if (swingAngle < 0)
863 {
864 // this should never happen!
865#if 0
866 btAssert(0);
867#endif
868 }
869}
870
872{
873 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
876
877 // Use the slope of the vector (using x/yEllipse) and find the length
878 // of the line that intersects the ellipse:
879 // x^2 y^2
880 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
881 // a^2 b^2
882 // Do the math and it should be clear.
883
884 btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
886 {
888 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
890 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
891 swingLimit = std::sqrt(swingLimit2);
892 }
893
894 // convert into point in constraint space:
895 // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
900}
901
902// given a twist rotation in constraint space, (pre: cone must already be removed)
903// this method computes its corresponding angle and axis.
905 btScalar& twistAngle, // out
906 btVector3& vTwistAxis) // out
907{
910
911 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
912 {
913 qMinTwist = -(qTwist);
914 twistAngle = qMinTwist.getAngle();
915 }
916 if (twistAngle < 0)
917 {
918 // this should never happen
919#if 0
920 btAssert(0);
921#endif
922 }
923
926 vTwistAxis.normalize();
927}
928
930{
931 // the swing axis is computed as the "twist-free" cone rotation,
932 // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
933 // so, if we're outside the limits, the closest way back inside the cone isn't
934 // along the vector back to the center. better (and more stable) to use the ellipse normal.
935
936 // convert swing axis to direction from center to surface of ellipse
937 // (ie. rotate 2D vector by PI/2)
938 btScalar y = -vSwingAxis.z();
939 btScalar z = vSwingAxis.y();
940
941 // do the math...
942 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
943 {
944 // compute gradient/normal of ellipse surface at current "point"
945 btScalar grad = y / z;
947
948 // adjust y/z to represent normal at point (instead of vector to point)
949 if (y > 0)
950 y = fabs(grad * z);
951 else
952 y = -fabs(grad * z);
953
954 // convert ellipse direction back to swing axis
955 vSwingAxis.setZ(-y);
956 vSwingAxis.setY(z);
957 vSwingAxis.normalize();
958 }
959}
960
962{
963 //btTransform trACur = m_rbA.getCenterOfMassTransform();
964 //btTransform trBCur = m_rbB.getCenterOfMassTransform();
965 // btTransform trABCur = trBCur.inverse() * trACur;
966 // btQuaternion qABCur = trABCur.getRotation();
967 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
968 //btQuaternion qConstraintCur = trConstraintCur.getRotation();
969
972}
973
975{
976 m_qTarget = q;
977
978 // clamp motor target to within limits
979 {
980 btScalar softness = 1.f; //m_limitSoftness;
981
982 // split into twist and cone
985 qTargetCone.normalize();
988
989 // clamp cone
990 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
991 {
995
997 {
1000 else if (swingAngle < -swingLimit * softness)
1003 }
1004 }
1005
1006 // clamp twist
1007 if (m_twistSpan >= btScalar(0.05f))
1008 {
1012
1014 {
1015 // eddy todo: limitSoftness used here???
1018 else if (twistAngle < -m_twistSpan * softness)
1021 }
1022 }
1023
1025 }
1026}
1027
1031{
1032 switch (num)
1033 {
1034 case BT_CONSTRAINT_ERP:
1036 if ((axis >= 0) && (axis < 3))
1037 {
1038 m_linERP = value;
1040 }
1041 else
1042 {
1043 m_biasFactor = value;
1044 }
1045 break;
1046 case BT_CONSTRAINT_CFM:
1048 if ((axis >= 0) && (axis < 3))
1049 {
1050 m_linCFM = value;
1052 }
1053 else
1054 {
1055 m_angCFM = value;
1057 }
1058 break;
1059 default:
1061 break;
1062 }
1063}
1064
1067{
1068 btScalar retVal = 0;
1069 switch (num)
1070 {
1071 case BT_CONSTRAINT_ERP:
1073 if ((axis >= 0) && (axis < 3))
1074 {
1076 retVal = m_linERP;
1077 }
1078 else if ((axis >= 3) && (axis < 6))
1079 {
1081 }
1082 else
1083 {
1085 }
1086 break;
1087 case BT_CONSTRAINT_CFM:
1089 if ((axis >= 0) && (axis < 3))
1090 {
1092 retVal = m_linCFM;
1093 }
1094 else if ((axis >= 3) && (axis < 6))
1095 {
1097 retVal = m_angCFM;
1098 }
1099 else
1100 {
1102 }
1103 break;
1104 default:
1106 }
1107 return retVal;
1108}
1109
1111{
1114 buildJacobian();
1115 //calculateTransforms();
1116}
static btVector3 vTwist(1, 0, 0)
#define CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_DEF_FIX_THRESH
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
#define SIMD_PI
Definition btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define BT_LARGE_FLOAT
Definition btScalar.h:316
btScalar btAtan2(btScalar x, btScalar y)
Definition btScalar.h:518
btScalar btSin(btScalar x)
Definition btScalar.h:499
btScalar btFabs(btScalar x)
Definition btScalar.h:497
#define SIMD_INFINITY
Definition btScalar.h:544
#define SIMD_FORCE_INLINE
Definition btScalar.h:98
btScalar btCos(btScalar x)
Definition btScalar.h:498
btScalar btAtan2Fast(btScalar y, btScalar x)
Definition btScalar.h:553
#define SIMD_EPSILON
Definition btScalar.h:543
bool btFuzzyZero(btScalar x)
Definition btScalar.h:572
#define btAssert(x)
Definition btScalar.h:153
#define btAssertConstrParams(_par)
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ CONETWIST_CONSTRAINT_TYPE
void btPlaneSpace1(const T &n, T &p, T &q)
Definition btVector3.h:1251
const btRigidBody & getRigidBodyB() const
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
void setLimit(int limitIndex, btScalar limitValue)
void setMotorTargetInConstraintSpace(const btQuaternion &q)
const btRigidBody & getRigidBodyA() const
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
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 getInfo1NonVirtual(btConstraintInfo1 *info)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
void updateRHS(btScalar timeStep)
void setMotorTarget(const btQuaternion &q)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
btScalar getDiagonal() const
btVector3 m_linearJointAxis
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
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 & getCenterOfMassPosition() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
void setIdentity()
Set this transformation to the identity.
btQuaternion getRotation() const
Return a quaternion representing the rotation.
btVector3 & getOrigin()
Return the origin vector translation.
void setOrigin(const btVector3 &origin)
Set the translational element.
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btScalar length() const
Return the length of the vector.
Definition btVector3.h:257
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
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition btVector3.h:303
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...