2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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.
15 Written by: Marcus Hennix
18 #include "btConeTwistConstraint.h"
19 #include "BulletDynamics/Dynamics/btRigidBody.h"
20 #include "LinearMath/btTransformUtil.h"
21 #include "LinearMath/btMinMax.h"
25 //#define CONETWIST_USE_OBSOLETE_SOLVER true
26 #define CONETWIST_USE_OBSOLETE_SOLVER false
27 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
29 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
31 btVector3 vec = axis * invInertiaWorld;
35 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB,
36 const btTransform& rbAFrame, const btTransform& rbBFrame)
37 : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
42 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
43 : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
45 m_rbBFrame = m_rbAFrame;
46 m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
50 void btConeTwistConstraint::init()
52 m_angularOnly = false;
53 m_solveTwistLimit = false;
54 m_solveSwingLimit = false;
55 m_bMotorEnabled = false;
56 m_maxMotorImpulse = btScalar(-1);
58 setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
59 m_damping = btScalar(0.01);
60 m_fixThresh = CONETWIST_DEF_FIX_THRESH;
62 m_linCFM = btScalar(0.f);
63 m_linERP = btScalar(0.7f);
64 m_angCFM = btScalar(0.f);
67 void btConeTwistConstraint::getInfo1(btConstraintInfo1* info)
69 if (m_useSolveConstraintObsolete)
71 info->m_numConstraintRows = 0;
76 info->m_numConstraintRows = 3;
78 calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
79 if (m_solveSwingLimit)
81 info->m_numConstraintRows++;
83 if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
85 info->m_numConstraintRows++;
89 if (m_solveTwistLimit)
91 info->m_numConstraintRows++;
97 void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
99 //always reserve 6 rows: object transform is not available on SPU
100 info->m_numConstraintRows = 6;
104 void btConeTwistConstraint::getInfo2(btConstraintInfo2* info)
106 getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
109 void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
111 calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
113 btAssert(!m_useSolveConstraintObsolete);
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();
120 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
121 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
122 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
123 btVector3 a1neg = -a1;
124 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
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();
131 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
132 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
133 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
134 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
136 // set right hand side
137 btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
138 btScalar k = info->fps * linERP;
140 for (j = 0; j < 3; j++)
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;
145 if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
147 info->cfm[j * info->rowskip] = m_linCFM;
151 int srow = row * info->rowskip;
154 if (m_solveSwingLimit)
156 btScalar* J1 = info->m_J1angularAxis;
157 btScalar* J2 = info->m_J2angularAxis;
158 if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
160 btTransform trA = transA * m_rbAFrame;
161 btVector3 p = trA.getBasis().getColumn(1);
162 btVector3 q = trA.getBasis().getColumn(2);
163 int srow1 = srow + info->rowskip;
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];
176 btScalar fact = info->fps * m_relaxationFactor;
177 info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
178 info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
179 info->m_lowerLimit[srow] = -SIMD_INFINITY;
180 info->m_upperLimit[srow] = SIMD_INFINITY;
181 info->m_lowerLimit[srow1] = -SIMD_INFINITY;
182 info->m_upperLimit[srow1] = SIMD_INFINITY;
183 srow = srow1 + info->rowskip;
187 ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
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;
196 info->m_constraintError[srow] = k * m_swingCorrection;
197 if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
199 info->cfm[srow] = m_angCFM;
201 // m_swingCorrection is always positive or 0
202 info->m_lowerLimit[srow] = 0;
203 info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
204 srow += info->rowskip;
207 if (m_solveTwistLimit)
209 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
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;
219 info->m_constraintError[srow] = k * m_twistCorrection;
220 if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
222 info->cfm[srow] = m_angCFM;
224 if (m_twistSpan > 0.0f)
226 if (m_twistCorrection > 0.0f)
228 info->m_lowerLimit[srow] = 0;
229 info->m_upperLimit[srow] = SIMD_INFINITY;
233 info->m_lowerLimit[srow] = -SIMD_INFINITY;
234 info->m_upperLimit[srow] = 0;
239 info->m_lowerLimit[srow] = -SIMD_INFINITY;
240 info->m_upperLimit[srow] = SIMD_INFINITY;
242 srow += info->rowskip;
246 void btConeTwistConstraint::buildJacobian()
248 if (m_useSolveConstraintObsolete)
250 m_appliedImpulse = btScalar(0.);
251 m_accTwistLimitImpulse = btScalar(0.);
252 m_accSwingLimitImpulse = btScalar(0.);
253 m_accMotorImpulse = btVector3(0., 0., 0.);
257 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
258 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
259 btVector3 relPos = pivotBInW - pivotAInW;
262 if (relPos.length2() > SIMD_EPSILON)
264 normal[0] = relPos.normalized();
268 normal[0].setValue(btScalar(1.0), 0, 0);
271 btPlaneSpace1(normal[0], normal[1], normal[2]);
273 for (int i = 0; i < 3; i++)
275 new (&m_jac[i]) btJacobianEntry(
276 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
277 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
278 pivotAInW - m_rbA.getCenterOfMassPosition(),
279 pivotBInW - m_rbB.getCenterOfMassPosition(),
281 m_rbA.getInvInertiaDiagLocal(),
283 m_rbB.getInvInertiaDiagLocal(),
288 calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
292 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep)
295 if (m_useSolveConstraintObsolete)
297 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
298 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
300 btScalar tau = btScalar(0.3);
305 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
306 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
309 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
311 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
312 btVector3 vel = vel1 - vel2;
314 for (int i = 0; i < 3; i++)
316 const btVector3& normal = m_jac[i].m_linearJointAxis;
317 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
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
323 btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
324 m_appliedImpulse += impulse;
326 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
327 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
328 bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse);
329 bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse);
336 // compute current and predicted transforms
337 btTransform trACur = m_rbA.getCenterOfMassTransform();
338 btTransform trBCur = m_rbB.getCenterOfMassTransform();
340 bodyA.internalGetAngularVelocity(omegaA);
342 bodyB.internalGetAngularVelocity(omegaB);
344 trAPred.setIdentity();
345 btVector3 zerovec(0, 0, 0);
346 btTransformUtil::integrateTransform(
347 trACur, zerovec, omegaA, timeStep, trAPred);
349 trBPred.setIdentity();
350 btTransformUtil::integrateTransform(
351 trBCur, zerovec, omegaB, timeStep, trBPred);
353 // compute desired transforms in world
354 btTransform trPose(m_qTarget);
355 btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
356 btTransform trADes = trBPred * trABDes;
357 btTransform trBDes = trAPred * trABDes.inverse();
359 // compute desired omegas in world
360 btVector3 omegaADes, omegaBDes;
362 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
363 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
365 // compute delta omegas
366 btVector3 dOmegaA = omegaADes - omegaA;
367 btVector3 dOmegaB = omegaBDes - omegaB;
369 // compute weighted avg axis of dOmega (weighting based on inertias)
370 btVector3 axisA, axisB;
371 btScalar kAxisAInv = 0, kAxisBInv = 0;
373 if (dOmegaA.length2() > SIMD_EPSILON)
375 axisA = dOmegaA.normalized();
376 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
379 if (dOmegaB.length2() > SIMD_EPSILON)
381 axisB = dOmegaB.normalized();
382 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
385 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
387 static bool bDoTorque = true;
388 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
391 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
392 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
393 btScalar kInvCombined = kAxisAInv + kAxisBInv;
395 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
396 (kInvCombined * kInvCombined);
398 if (m_maxMotorImpulse >= 0)
400 btScalar fMaxImpulse = m_maxMotorImpulse;
401 if (m_bNormalizedMotorStrength)
402 fMaxImpulse = fMaxImpulse / kAxisAInv;
404 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
405 btScalar newUnclampedMag = newUnclampedAccImpulse.length();
406 if (newUnclampedMag > fMaxImpulse)
408 newUnclampedAccImpulse.normalize();
409 newUnclampedAccImpulse *= fMaxImpulse;
410 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
412 m_accMotorImpulse += impulse;
415 btScalar impulseMag = impulse.length();
416 btVector3 impulseAxis = impulse / impulseMag;
418 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
419 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
422 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
425 bodyA.internalGetAngularVelocity(angVelA);
427 bodyB.internalGetAngularVelocity(angVelB);
428 btVector3 relVel = angVelB - angVelA;
429 if (relVel.length2() > SIMD_EPSILON)
431 btVector3 relVelAxis = relVel.normalized();
432 btScalar m_kDamping = btScalar(1.) /
433 (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
434 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
435 btVector3 impulse = m_damping * m_kDamping * relVel;
437 btScalar impulseMag = impulse.length();
438 btVector3 impulseAxis = impulse / impulseMag;
439 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
440 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
446 ///solve angular part
448 bodyA.internalGetAngularVelocity(angVelA);
450 bodyB.internalGetAngularVelocity(angVelB);
453 if (m_solveSwingLimit)
455 btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
456 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
458 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
459 btScalar impulseMag = amplitude * m_kSwing;
461 // Clamp the accumulated impulse
462 btScalar temp = m_accSwingLimitImpulse;
463 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0));
464 impulseMag = m_accSwingLimitImpulse - temp;
466 btVector3 impulse = m_swingAxis * impulseMag;
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)
471 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
472 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
473 impulse = impulseNoTwistCouple;
476 impulseMag = impulse.length();
477 btVector3 noTwistSwingAxis = impulse / impulseMag;
479 bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag);
480 bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag);
484 if (m_solveTwistLimit)
486 btScalar amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
487 btScalar relTwistVel = (angVelB - angVelA).dot(m_twistAxis);
488 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
489 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
490 btScalar impulseMag = amplitude * m_kTwist;
492 // Clamp the accumulated impulse
493 btScalar temp = m_accTwistLimitImpulse;
494 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0));
495 impulseMag = m_accTwistLimitImpulse - temp;
497 // btVector3 impulse = m_twistAxis * impulseMag;
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);
509 void btConeTwistConstraint::updateRHS(btScalar timeStep)
515 void btConeTwistConstraint::calcAngleInfo()
517 m_swingCorrection = btScalar(0.);
518 m_twistLimitSign = btScalar(0.);
519 m_solveTwistLimit = false;
520 m_solveSwingLimit = false;
522 btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
523 btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
525 b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
526 b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
528 btScalar swing1 = btScalar(0.), swing2 = btScalar(0.);
530 btScalar swx = btScalar(0.), swy = btScalar(0.);
531 btScalar thresh = btScalar(10.);
534 // Get Frame into world space
535 if (m_swingSpan1 >= btScalar(0.05f))
537 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
538 swx = b2Axis1.dot(b1Axis1);
539 swy = b2Axis1.dot(b1Axis2);
540 swing1 = btAtan2Fast(swy, swx);
541 fact = (swy * swy + swx * swx) * thresh * thresh;
542 fact = fact / (fact + btScalar(1.0));
546 if (m_swingSpan2 >= btScalar(0.05f))
548 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
549 swx = b2Axis1.dot(b1Axis1);
550 swy = b2Axis1.dot(b1Axis3);
551 swing2 = btAtan2Fast(swy, swx);
552 fact = (swy * swy + swx * swx) * thresh * thresh;
553 fact = fact / (fact + btScalar(1.0));
557 btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
558 btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
559 btScalar EllipseAngle = btFabs(swing1 * swing1) * RMaxAngle1Sq + btFabs(swing2 * swing2) * RMaxAngle2Sq;
561 if (EllipseAngle > 1.0f)
563 m_swingCorrection = EllipseAngle - 1.0f;
564 m_solveSwingLimit = true;
565 // Calculate necessary axis & factors
566 m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
567 m_swingAxis.normalize();
568 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
569 m_swingAxis *= swingAxisSign;
573 if (m_twistSpan >= btScalar(0.))
575 btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
576 btQuaternion rotationArc = shortestArcQuat(b2Axis1, b1Axis1);
577 btVector3 TwistRef = quatRotate(rotationArc, b2Axis2);
578 btScalar twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
579 m_twistAngle = twist;
581 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
582 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
583 if (twist <= -m_twistSpan * lockedFreeFactor)
585 m_twistCorrection = -(twist + m_twistSpan);
586 m_solveTwistLimit = true;
587 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
588 m_twistAxis.normalize();
589 m_twistAxis *= -1.0f;
591 else if (twist > m_twistSpan * lockedFreeFactor)
593 m_twistCorrection = (twist - m_twistSpan);
594 m_solveTwistLimit = true;
595 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
596 m_twistAxis.normalize();
602 static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space
604 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
606 m_swingCorrection = btScalar(0.);
607 m_twistLimitSign = btScalar(0.);
608 m_solveTwistLimit = false;
609 m_solveSwingLimit = false;
610 // compute rotation of A wrt B (in constraint space)
611 if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
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
616 btTransform trPose(m_qTarget);
617 btTransform trA = transA * m_rbAFrame;
618 btTransform trB = transB * m_rbBFrame;
619 btTransform trDeltaAB = trB * trPose * trA.inverse();
620 btQuaternion qDeltaAB = trDeltaAB.getRotation();
621 btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
622 btScalar swingAxisLen2 = swingAxis.length2();
623 if (btFuzzyZero(swingAxisLen2))
627 m_swingAxis = swingAxis;
628 m_swingAxis.normalize();
629 m_swingCorrection = qDeltaAB.getAngle();
630 if (!btFuzzyZero(m_swingCorrection))
632 m_solveSwingLimit = true;
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();
641 btQuaternion qAB = qB.inverse() * qA;
642 // split rotation into cone and twist
643 // (all this is done from B's perspective. Maybe I should be averaging axes...)
644 btVector3 vConeNoTwist = quatRotate(qAB, vTwist);
645 vConeNoTwist.normalize();
646 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
648 btQuaternion qABTwist = qABCone.inverse() * qAB;
649 qABTwist.normalize();
651 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
653 btScalar swingAngle, swingLimit = 0;
655 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
657 if (swingAngle > swingLimit * m_limitSoftness)
659 m_solveSwingLimit = true;
661 // compute limit ratio: 0->1, where
662 // 0 == beginning of soft limit
663 // 1 == hard/real limit
664 m_swingLimitRatio = 1.f;
665 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
667 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
668 (swingLimit - swingLimit * m_limitSoftness);
671 // swing correction tries to get back to soft limit
672 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
674 // adjustment of swing axis (based on ellipse normal)
675 adjustSwingAxisToUseEllipseNormal(swingAxis);
677 // Calculate necessary axis & factors
678 m_swingAxis = quatRotate(qB, -swingAxis);
680 m_twistAxisA.setValue(0, 0, 0);
682 m_kSwing = btScalar(1.) /
683 (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) +
684 computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB));
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);
697 btScalar x = ivB.dot(ivA);
698 btScalar y = ivB.dot(jvA);
699 btScalar z = ivB.dot(kvA);
700 if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
701 { // fixed. We'll need to add one more row to constraint
702 if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
704 m_solveSwingLimit = true;
705 m_swingAxis = -ivB.cross(ivA);
710 if (m_swingSpan1 < m_fixThresh)
711 { // hinge around Y axis
712 // if(!(btFuzzyZero(y)))
713 if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
715 m_solveSwingLimit = true;
716 if (m_swingSpan2 >= m_fixThresh)
719 btScalar span2 = btAtan2(z, x);
720 if (span2 > m_swingSpan2)
722 x = btCos(m_swingSpan2);
723 z = btSin(m_swingSpan2);
725 else if (span2 < -m_swingSpan2)
727 x = btCos(m_swingSpan2);
728 z = -btSin(m_swingSpan2);
734 { // hinge around Z axis
735 // if(!btFuzzyZero(z))
736 if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
738 m_solveSwingLimit = true;
739 if (m_swingSpan1 >= m_fixThresh)
742 btScalar span1 = btAtan2(y, x);
743 if (span1 > m_swingSpan1)
745 x = btCos(m_swingSpan1);
746 y = btSin(m_swingSpan1);
748 else if (span1 < -m_swingSpan1)
750 x = btCos(m_swingSpan1);
751 y = -btSin(m_swingSpan1);
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];
760 m_swingAxis = -ivB.cross(target);
761 m_swingCorrection = m_swingAxis.length();
763 if (!btFuzzyZero(m_swingCorrection))
764 m_swingAxis.normalize();
768 if (m_twistSpan >= btScalar(0.f))
771 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
773 if (m_twistAngle > m_twistSpan * m_limitSoftness)
775 m_solveTwistLimit = true;
777 m_twistLimitRatio = 1.f;
778 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
780 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
781 (m_twistSpan - m_twistSpan * m_limitSoftness);
784 // twist correction tries to get back to soft limit
785 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
787 m_twistAxis = quatRotate(qB, -twistAxis);
789 m_kTwist = btScalar(1.) /
790 (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) +
791 computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB));
794 if (m_solveSwingLimit)
795 m_twistAxisA = quatRotate(qA, -twistAxis);
799 m_twistAngle = btScalar(0.f);
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".
807 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
808 btScalar& swingAngle, // out
809 btVector3& vSwingAxis, // out
810 btScalar& swingLimit) // out
812 swingAngle = qCone.getAngle();
813 if (swingAngle > SIMD_EPSILON)
815 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
816 vSwingAxis.normalize();
818 // non-zero twist?! this should never happen.
819 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
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.)
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.)
829 btScalar xEllipse = vSwingAxis.y();
830 btScalar yEllipse = -vSwingAxis.z();
832 // Now, we use the slope of the vector (using x/yEllipse) and find the length
833 // of the line that intersects the ellipse:
835 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
837 // Do the math and it should be clear.
839 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
840 if (fabs(xEllipse) > SIMD_EPSILON)
842 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
843 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
844 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
845 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
846 swingLimit = std::sqrt(swingLimit2);
850 /*swingLimit = m_swingSpan2;
851 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
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;
862 else if (swingAngle < 0)
864 // this should never happen!
871 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
873 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
874 btScalar xEllipse = btCos(fAngleInRadians);
875 btScalar yEllipse = btSin(fAngleInRadians);
877 // Use the slope of the vector (using x/yEllipse) and find the length
878 // of the line that intersects the ellipse:
880 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
882 // Do the math and it should be clear.
884 btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
885 if (fabs(xEllipse) > SIMD_EPSILON)
887 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
888 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
889 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
890 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
891 swingLimit = std::sqrt(swingLimit2);
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
896 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
897 btQuaternion qSwing(vSwingAxis, swingLimit);
898 btVector3 vPointInConstraintSpace(fLength, 0, 0);
899 return quatRotate(qSwing, vPointInConstraintSpace);
902 // given a twist rotation in constraint space, (pre: cone must already be removed)
903 // this method computes its corresponding angle and axis.
904 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
905 btScalar& twistAngle, // out
906 btVector3& vTwistAxis) // out
908 btQuaternion qMinTwist = qTwist;
909 twistAngle = qTwist.getAngle();
911 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
913 qMinTwist = -(qTwist);
914 twistAngle = qMinTwist.getAngle();
918 // this should never happen
924 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
925 if (twistAngle > SIMD_EPSILON)
926 vTwistAxis.normalize();
929 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
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.
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();
942 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
944 // compute gradient/normal of ellipse surface at current "point"
945 btScalar grad = y / z;
946 grad *= m_swingSpan2 / m_swingSpan1;
948 // adjust y/z to represent normal at point (instead of vector to point)
954 // convert ellipse direction back to swing axis
957 vSwingAxis.normalize();
961 void btConeTwistConstraint::setMotorTarget(const btQuaternion& q)
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();
970 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
971 setMotorTargetInConstraintSpace(qConstraint);
974 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q)
978 // clamp motor target to within limits
980 btScalar softness = 1.f; //m_limitSoftness;
982 // split into twist and cone
983 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
984 btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted);
985 qTargetCone.normalize();
986 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget;
987 qTargetTwist.normalize();
990 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
992 btScalar swingAngle, swingLimit;
994 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
996 if (fabs(swingAngle) > SIMD_EPSILON)
998 if (swingAngle > swingLimit * softness)
999 swingAngle = swingLimit * softness;
1000 else if (swingAngle < -swingLimit * softness)
1001 swingAngle = -swingLimit * softness;
1002 qTargetCone = btQuaternion(swingAxis, swingAngle);
1007 if (m_twistSpan >= btScalar(0.05f))
1009 btScalar twistAngle;
1010 btVector3 twistAxis;
1011 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1013 if (fabs(twistAngle) > SIMD_EPSILON)
1015 // eddy todo: limitSoftness used here???
1016 if (twistAngle > m_twistSpan * softness)
1017 twistAngle = m_twistSpan * softness;
1018 else if (twistAngle < -m_twistSpan * softness)
1019 twistAngle = -m_twistSpan * softness;
1020 qTargetTwist = btQuaternion(twistAxis, twistAngle);
1024 m_qTarget = qTargetCone * qTargetTwist;
1028 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
1029 ///If no axis is provided, it uses the default axis for this constraint.
1030 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1034 case BT_CONSTRAINT_ERP:
1035 case BT_CONSTRAINT_STOP_ERP:
1036 if ((axis >= 0) && (axis < 3))
1039 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1043 m_biasFactor = value;
1046 case BT_CONSTRAINT_CFM:
1047 case BT_CONSTRAINT_STOP_CFM:
1048 if ((axis >= 0) && (axis < 3))
1051 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1056 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1060 btAssertConstrParams(0);
1065 ///return the local value of parameter
1066 btScalar btConeTwistConstraint::getParam(int num, int axis) const
1068 btScalar retVal = 0;
1071 case BT_CONSTRAINT_ERP:
1072 case BT_CONSTRAINT_STOP_ERP:
1073 if ((axis >= 0) && (axis < 3))
1075 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1078 else if ((axis >= 3) && (axis < 6))
1080 retVal = m_biasFactor;
1084 btAssertConstrParams(0);
1087 case BT_CONSTRAINT_CFM:
1088 case BT_CONSTRAINT_STOP_CFM:
1089 if ((axis >= 0) && (axis < 3))
1091 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1094 else if ((axis >= 3) && (axis < 6))
1096 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1101 btAssertConstrParams(0);
1105 btAssertConstrParams(0);
1110 void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
1112 m_rbAFrame = frameA;
1113 m_rbBFrame = frameB;
1115 //calculateTransforms();