2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 #include "btHingeConstraint.h"
17 #include "BulletDynamics/Dynamics/btRigidBody.h"
18 #include "LinearMath/btTransformUtil.h"
19 #include "LinearMath/btMinMax.h"
21 #include "btSolverBody.h"
23 //#define HINGE_USE_OBSOLETE_SOLVER false
24 #define HINGE_USE_OBSOLETE_SOLVER false
26 #define HINGE_USE_FRAME_OFFSET true
30 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
31 const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
32 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
33 #ifdef _BT_USE_CENTER_LIMIT_
37 m_enableAngularMotor(false),
38 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
39 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
40 m_useReferenceFrameA(useReferenceFrameA),
47 m_rbAFrame.getOrigin() = pivotInA;
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);
53 btScalar projection = axisInA.dot(rbAxisA1);
54 if (projection >= 1.0f - SIMD_EPSILON)
56 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
57 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
59 else if (projection <= -1.0f + SIMD_EPSILON)
61 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
62 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
66 rbAxisA2 = axisInA.cross(rbAxisA1);
67 rbAxisA1 = rbAxisA2.cross(axisInA);
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());
74 btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
75 btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
76 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
78 m_rbBFrame.getOrigin() = pivotInB;
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());
83 #ifndef _BT_USE_CENTER_LIMIT_
85 m_lowerLimit = btScalar(1.0f);
86 m_upperLimit = btScalar(-1.0f);
88 m_relaxationFactor = 1.0f;
89 m_limitSoftness = 0.9f;
92 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
95 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
96 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
97 #ifdef _BT_USE_CENTER_LIMIT_
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),
111 // since no frame is given, assume this to be zero angle and just pick rb transform axis
112 // fixed axis in worldspace
113 btVector3 rbAxisA1, rbAxisA2;
114 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
116 m_rbAFrame.getOrigin() = pivotInA;
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());
121 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
123 btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
124 btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
125 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
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());
132 #ifndef _BT_USE_CENTER_LIMIT_
134 m_lowerLimit = btScalar(1.0f);
135 m_upperLimit = btScalar(-1.0f);
137 m_relaxationFactor = 1.0f;
138 m_limitSoftness = 0.9f;
139 m_solveLimit = false;
141 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
144 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
145 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
146 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
147 #ifdef _BT_USE_CENTER_LIMIT_
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),
161 #ifndef _BT_USE_CENTER_LIMIT_
163 m_lowerLimit = btScalar(1.0f);
164 m_upperLimit = btScalar(-1.0f);
166 m_relaxationFactor = 1.0f;
167 m_limitSoftness = 0.9f;
168 m_solveLimit = false;
170 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
173 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
174 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
175 #ifdef _BT_USE_CENTER_LIMIT_
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),
189 ///not providing rigidbody B means implicitly using worldspace for body B
191 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
192 #ifndef _BT_USE_CENTER_LIMIT_
194 m_lowerLimit = btScalar(1.0f);
195 m_upperLimit = btScalar(-1.0f);
197 m_relaxationFactor = 1.0f;
198 m_limitSoftness = 0.9f;
199 m_solveLimit = false;
201 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
204 void btHingeConstraint::buildJacobian()
206 if (m_useSolveConstraintObsolete)
208 m_appliedImpulse = btScalar(0.);
209 m_accMotorImpulse = btScalar(0.);
213 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
214 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
215 btVector3 relPos = pivotBInW - pivotAInW;
218 if (relPos.length2() > SIMD_EPSILON)
220 normal[0] = relPos.normalized();
224 normal[0].setValue(btScalar(1.0), 0, 0);
227 btPlaneSpace1(normal[0], normal[1], normal[2]);
229 for (int i = 0; i < 3; i++)
231 new (&m_jac[i]) btJacobianEntry(
232 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
233 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
234 pivotAInW - m_rbA.getCenterOfMassPosition(),
235 pivotBInW - m_rbB.getCenterOfMassPosition(),
237 m_rbA.getInvInertiaDiagLocal(),
239 m_rbB.getInvInertiaDiagLocal(),
244 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
245 //these two jointAxis require equal angular velocities for both bodies
247 //this is unused for now, it's a todo
248 btVector3 jointAxis0local;
249 btVector3 jointAxis1local;
251 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
253 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
254 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
255 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
257 new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
258 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
259 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
260 m_rbA.getInvInertiaDiagLocal(),
261 m_rbB.getInvInertiaDiagLocal());
263 new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
264 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
265 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
266 m_rbA.getInvInertiaDiagLocal(),
267 m_rbB.getInvInertiaDiagLocal());
269 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
270 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
271 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
272 m_rbA.getInvInertiaDiagLocal(),
273 m_rbB.getInvInertiaDiagLocal());
276 m_accLimitImpulse = btScalar(0.);
278 // test angular limit
279 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
281 //Compute K = J*W*J' for hinge axis
282 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
283 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
284 getRigidBodyB().computeAngularImpulseDenominator(axisA));
290 static inline btScalar btNormalizeAnglePositive(btScalar angle)
292 return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
295 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
297 btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
298 btNormalizeAnglePositive(accAngle)));
302 static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
305 btScalar result = btShortestAngularDistance(accAngle, curAngle);
307 if (btFabs(result) > tol)
310 return accAngle + result;
315 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
317 btScalar hingeAngle = getHingeAngle();
318 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
319 return m_accumulatedAngle;
321 void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
323 m_accumulatedAngle = accAngle;
326 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
328 //update m_accumulatedAngle
329 btScalar curHingeAngle = getHingeAngle();
330 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
332 btHingeConstraint::getInfo1(info);
335 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
337 if (m_useSolveConstraintObsolete)
339 info->m_numConstraintRows = 0;
344 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
346 //always add the row, to avoid computation (data is not available yet)
348 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
349 if (getSolveLimit() || getEnableAngularMotor())
351 info->m_numConstraintRows++; // limit 3rd anguar as well
357 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
359 if (m_useSolveConstraintObsolete)
361 info->m_numConstraintRows = 0;
366 //always add the 'limit' row, to avoid computation (data is not available yet)
367 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
372 void btHingeConstraint::getInfo2(btConstraintInfo2* info)
374 if (m_useOffsetForConstraintFrame)
376 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
380 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
384 void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
386 ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
387 testLimit(transA, transB);
389 getInfo2Internal(info, transA, transB, angVelA, angVelB);
392 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
394 btAssert(!m_useSolveConstraintObsolete);
395 int i, skip = info->rowskip;
396 // transforms in world space
397 btTransform trA = transA * m_rbAFrame;
398 btTransform trB = transB * m_rbBFrame;
400 btVector3 pivotAInW = trA.getOrigin();
401 btVector3 pivotBInW = trB.getOrigin();
407 info->m_J1linearAxis[i*skip]=0;
408 info->m_J1linearAxis[i*skip+1]=0;
409 info->m_J1linearAxis[i*skip+2]=0;
411 info->m_J1angularAxis[i*skip]=0;
412 info->m_J1angularAxis[i*skip+1]=0;
413 info->m_J1angularAxis[i*skip+2]=0;
415 info->m_J2linearAxis[i*skip]=0;
416 info->m_J2linearAxis[i*skip+1]=0;
417 info->m_J2linearAxis[i*skip+2]=0;
419 info->m_J2angularAxis[i*skip]=0;
420 info->m_J2angularAxis[i*skip+1]=0;
421 info->m_J2angularAxis[i*skip+2]=0;
423 info->m_constraintError[i*skip]=0.f;
427 // linear (all fixed)
431 info->m_J1linearAxis[0] = 1;
432 info->m_J1linearAxis[skip + 1] = 1;
433 info->m_J1linearAxis[2 * skip + 2] = 1;
435 info->m_J2linearAxis[0] = -1;
436 info->m_J2linearAxis[skip + 1] = -1;
437 info->m_J2linearAxis[2 * skip + 2] = -1;
440 btVector3 a1 = pivotAInW - transA.getOrigin();
442 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
443 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
444 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
445 btVector3 a1neg = -a1;
446 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
448 btVector3 a2 = pivotBInW - transB.getOrigin();
450 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
451 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
452 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
453 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
456 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
458 btScalar k = info->fps * normalErp;
461 for (i = 0; i < 3; i++)
463 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
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
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;
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];
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);
512 btVector3 u = ax1.cross(ax2);
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
518 btScalar limit_err = btScalar(0.0);
522 #ifdef _BT_USE_CENTER_LIMIT_
523 limit_err = m_limit.getCorrection() * m_referenceSign;
525 limit_err = m_correction * m_referenceSign;
527 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
529 // if the hinge has joint limits or motor, add in the extra row
530 bool powered = getEnableAngularMotor();
531 if (limit || powered)
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];
539 info->m_J2angularAxis[srow + 0] = -ax1[0];
540 info->m_J2angularAxis[srow + 1] = -ax1[1];
541 info->m_J2angularAxis[srow + 2] = -ax1[2];
543 btScalar lostop = getLowerLimit();
544 btScalar histop = getUpperLimit();
545 if (limit && (lostop == histop))
546 { // the joint motor is ineffective
549 info->m_constraintError[srow] = btScalar(0.0f);
550 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
553 if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
555 info->cfm[srow] = m_normalCFM;
557 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
558 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
559 info->m_lowerLimit[srow] = -m_maxMotorImpulse;
560 info->m_upperLimit[srow] = m_maxMotorImpulse;
564 k = info->fps * currERP;
565 info->m_constraintError[srow] += k * limit_err;
566 if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
568 info->cfm[srow] = m_stopCFM;
570 if (lostop == histop)
572 // limited low and high simultaneously
573 info->m_lowerLimit[srow] = -SIMD_INFINITY;
574 info->m_upperLimit[srow] = SIMD_INFINITY;
578 info->m_lowerLimit[srow] = 0;
579 info->m_upperLimit[srow] = SIMD_INFINITY;
583 info->m_lowerLimit[srow] = -SIMD_INFINITY;
584 info->m_upperLimit[srow] = 0;
586 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
587 #ifdef _BT_USE_CENTER_LIMIT_
588 btScalar bounce = m_limit.getRelaxationFactor();
590 btScalar bounce = m_relaxationFactor;
592 if (bounce > btScalar(0.0))
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.
602 btScalar newc = -bounce * vel;
603 if (newc > info->m_constraintError[srow])
605 info->m_constraintError[srow] = newc;
610 { // high limit - all those computations are reversed
613 btScalar newc = -bounce * vel;
614 if (newc < info->m_constraintError[srow])
616 info->m_constraintError[srow] = newc;
621 #ifdef _BT_USE_CENTER_LIMIT_
622 info->m_constraintError[srow] *= m_limit.getBiasFactor();
624 info->m_constraintError[srow] *= m_biasFactor;
627 } // if angular limit or powered
630 void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
637 void btHingeConstraint::updateRHS(btScalar timeStep)
642 btScalar btHingeConstraint::getHingeAngle()
644 return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
647 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
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));
653 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
654 return m_referenceSign * angle;
657 void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
659 // Compute limit information
660 m_hingeAngle = getHingeAngle(transA, transB);
661 #ifdef _BT_USE_CENTER_LIMIT_
662 m_limit.test(m_hingeAngle);
664 m_correction = btScalar(0.);
665 m_limitSign = btScalar(0.);
666 m_solveLimit = false;
667 if (m_lowerLimit <= m_upperLimit)
669 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
670 if (m_hingeAngle <= m_lowerLimit)
672 m_correction = (m_lowerLimit - m_hingeAngle);
676 else if (m_hingeAngle >= m_upperLimit)
678 m_correction = m_upperLimit - m_hingeAngle;
687 static btVector3 vHinge(0, 0, btScalar(1));
689 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
691 // convert target from body to constraint space
692 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
693 qConstraint.normalize();
695 // extract "pure" hinge component
696 btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
697 vNoHinge.normalize();
698 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
699 btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
702 // compute angular target, clamped to limits
703 btScalar targetAngle = qHinge.getAngle();
704 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
707 targetAngle = qHinge.getAngle();
709 if (qHinge.getZ() < 0)
710 targetAngle = -targetAngle;
712 setMotorTarget(targetAngle, dt);
715 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
717 #ifdef _BT_USE_CENTER_LIMIT_
718 m_limit.fit(targetAngle);
720 if (m_lowerLimit < m_upperLimit)
722 if (targetAngle < m_lowerLimit)
723 targetAngle = m_lowerLimit;
724 else if (targetAngle > m_upperLimit)
725 targetAngle = m_upperLimit;
728 // compute angular velocity
729 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
730 btScalar dAngle = targetAngle - curAngle;
731 m_motorTargetVelocity = dAngle / dt;
734 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
736 btAssert(!m_useSolveConstraintObsolete);
737 int i, s = info->rowskip;
738 // transforms in world space
739 btTransform trA = transA * m_rbAFrame;
740 btTransform trB = transB * m_rbBFrame;
742 // btVector3 pivotAInW = trA.getOrigin();
743 // btVector3 pivotBInW = trB.getOrigin();
745 // difference between frames in WCS
746 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
747 // now get weight factors depending on masses
748 btScalar miA = getRigidBodyA().getInvMass();
749 btScalar miB = getRigidBodyB().getInvMass();
750 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
751 btScalar miS = miA + miB;
752 btScalar factA, factB;
753 if (miS > btScalar(0.f))
759 factA = btScalar(0.5f);
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);
766 btVector3 ax1 = ax1A * factA + ax1B * factB;
767 if (ax1.length2()<SIMD_EPSILON)
771 ax1 = ax1A * factA + ax1B * factB;
775 // we want: velA + wA x relA == velB + wB x relB
776 btTransform bodyA_trans = transA;
777 btTransform bodyB_trans = transB;
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
786 btVector3 projB = ax1 * relB.dot(ax1);
787 // get vector directed from bodyB to hinge axis (and orthogonal to it)
788 btVector3 orthoB = relB - projB;
790 relA = trA.getOrigin() - bodyA_trans.getOrigin();
791 btVector3 projA = ax1 * relA.dot(ax1);
792 btVector3 orthoA = relA - projA;
793 btVector3 totalDist = projA - projB;
794 // get offset vectors relA and relB
795 relA = orthoA + totalDist * factA;
796 relB = orthoB - totalDist * factB;
797 // now choose average ortho to hinge axis
798 p = orthoB * factA + orthoA * factB;
799 btScalar len2 = p.length2();
800 if (len2 > SIMD_EPSILON)
806 p = trA.getBasis().getColumn(1);
808 // make one more ortho
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);
817 if (hasStaticBody && getSolveLimit())
818 { // to make constraint between static and dynamic objects more rigid
819 // remove wA (or wB) from equation if angular limit is hit
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);
828 { // to make constraint between static and dynamic objects more rigid
829 // remove wA (or wB) from equation
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];
836 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
837 btScalar k = info->fps * normalErp;
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];
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];
849 // compute three elements of right hand side
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;
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
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.
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];
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; //??
897 btVector3 u = ax1A.cross(ax1B);
898 info->m_constraintError[s3] = k * u.dot(p);
899 info->m_constraintError[s4] = k * u.dot(q);
901 // check angular limits
902 nrow = 4; // last filled row
904 btScalar limit_err = btScalar(0.0);
908 #ifdef _BT_USE_CENTER_LIMIT_
909 limit_err = m_limit.getCorrection() * m_referenceSign;
911 limit_err = m_correction * m_referenceSign;
913 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
915 // if the hinge has joint limits or motor, add in the extra row
916 bool powered = getEnableAngularMotor();
917 if (limit || powered)
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];
925 info->m_J2angularAxis[srow + 0] = -ax1[0];
926 info->m_J2angularAxis[srow + 1] = -ax1[1];
927 info->m_J2angularAxis[srow + 2] = -ax1[2];
929 btScalar lostop = getLowerLimit();
930 btScalar histop = getUpperLimit();
931 if (limit && (lostop == histop))
932 { // the joint motor is ineffective
935 info->m_constraintError[srow] = btScalar(0.0f);
936 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
939 if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
941 info->cfm[srow] = m_normalCFM;
943 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
944 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
945 info->m_lowerLimit[srow] = -m_maxMotorImpulse;
946 info->m_upperLimit[srow] = m_maxMotorImpulse;
950 k = info->fps * currERP;
951 info->m_constraintError[srow] += k * limit_err;
952 if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
954 info->cfm[srow] = m_stopCFM;
956 if (lostop == histop)
958 // limited low and high simultaneously
959 info->m_lowerLimit[srow] = -SIMD_INFINITY;
960 info->m_upperLimit[srow] = SIMD_INFINITY;
964 info->m_lowerLimit[srow] = 0;
965 info->m_upperLimit[srow] = SIMD_INFINITY;
969 info->m_lowerLimit[srow] = -SIMD_INFINITY;
970 info->m_upperLimit[srow] = 0;
972 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
973 #ifdef _BT_USE_CENTER_LIMIT_
974 btScalar bounce = m_limit.getRelaxationFactor();
976 btScalar bounce = m_relaxationFactor;
978 if (bounce > btScalar(0.0))
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.
988 btScalar newc = -bounce * vel;
989 if (newc > info->m_constraintError[srow])
991 info->m_constraintError[srow] = newc;
996 { // high limit - all those computations are reversed
999 btScalar newc = -bounce * vel;
1000 if (newc < info->m_constraintError[srow])
1002 info->m_constraintError[srow] = newc;
1007 #ifdef _BT_USE_CENTER_LIMIT_
1008 info->m_constraintError[srow] *= m_limit.getBiasFactor();
1010 info->m_constraintError[srow] *= m_biasFactor;
1013 } // if angular limit or powered
1016 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
1017 ///If no axis is provided, it uses the default axis for this constraint.
1018 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1020 if ((axis == -1) || (axis == 5))
1024 case BT_CONSTRAINT_STOP_ERP:
1026 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
1028 case BT_CONSTRAINT_STOP_CFM:
1030 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1032 case BT_CONSTRAINT_CFM:
1033 m_normalCFM = value;
1034 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1036 case BT_CONSTRAINT_ERP:
1037 m_normalERP = value;
1038 m_flags |= BT_HINGE_FLAGS_ERP_NORM;
1041 btAssertConstrParams(0);
1046 btAssertConstrParams(0);
1050 ///return the local value of parameter
1051 btScalar btHingeConstraint::getParam(int num, int axis) const
1053 btScalar retVal = 0;
1054 if ((axis == -1) || (axis == 5))
1058 case BT_CONSTRAINT_STOP_ERP:
1059 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1062 case BT_CONSTRAINT_STOP_CFM:
1063 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1066 case BT_CONSTRAINT_CFM:
1067 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1068 retVal = m_normalCFM;
1070 case BT_CONSTRAINT_ERP:
1071 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
1072 retVal = m_normalERP;
1075 btAssertConstrParams(0);
1080 btAssertConstrParams(0);