1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
4
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:
10
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.
14
15 Written by: Marcus Hennix
16 */
17
18 #include "btConeTwistConstraint.h"
19 #include "BulletDynamics/Dynamics/btRigidBody.h"
20 #include "LinearMath/btTransformUtil.h"
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
computeAngularImpulseDenominator(const btVector3 & axis,const btMatrix3x3 & invInertiaWorld)29 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
30 {
31 btVector3 vec = axis * invInertiaWorld;
32 return axis.dot(vec);
33 }
34
btConeTwistConstraint(btRigidBody & rbA,btRigidBody & rbB,const btTransform & rbAFrame,const btTransform & rbBFrame)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)
38 {
39 init();
40 }
41
btConeTwistConstraint(btRigidBody & rbA,const btTransform & rbAFrame)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)
44 {
45 m_rbBFrame = m_rbAFrame;
46 m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
47 init();
48 }
49
init()50 void btConeTwistConstraint::init()
51 {
52 m_angularOnly = false;
53 m_solveTwistLimit = false;
54 m_solveSwingLimit = false;
55 m_bMotorEnabled = false;
56 m_maxMotorImpulse = btScalar(-1);
57
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;
61 m_flags = 0;
62 m_linCFM = btScalar(0.f);
63 m_linERP = btScalar(0.7f);
64 m_angCFM = btScalar(0.f);
65 }
66
getInfo1(btConstraintInfo1 * info)67 void btConeTwistConstraint::getInfo1(btConstraintInfo1* info)
68 {
69 if (m_useSolveConstraintObsolete)
70 {
71 info->m_numConstraintRows = 0;
72 info->nub = 0;
73 }
74 else
75 {
76 info->m_numConstraintRows = 3;
77 info->nub = 3;
78 calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
79 if (m_solveSwingLimit)
80 {
81 info->m_numConstraintRows++;
82 info->nub--;
83 if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
84 {
85 info->m_numConstraintRows++;
86 info->nub--;
87 }
88 }
89 if (m_solveTwistLimit)
90 {
91 info->m_numConstraintRows++;
92 info->nub--;
93 }
94 }
95 }
96
getInfo1NonVirtual(btConstraintInfo1 * info)97 void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
98 {
99 //always reserve 6 rows: object transform is not available on SPU
100 info->m_numConstraintRows = 6;
101 info->nub = 0;
102 }
103
getInfo2(btConstraintInfo2 * info)104 void btConeTwistConstraint::getInfo2(btConstraintInfo2* info)
105 {
106 getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
107 }
108
getInfo2NonVirtual(btConstraintInfo2 * info,const btTransform & transA,const btTransform & transB,const btMatrix3x3 & invInertiaWorldA,const btMatrix3x3 & invInertiaWorldB)109 void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
110 {
111 calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
112
113 btAssert(!m_useSolveConstraintObsolete);
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 {
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);
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 {
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);
135 }
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;
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;
145 if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
146 {
147 info->cfm[j * info->rowskip] = m_linCFM;
148 }
149 }
150 int row = 3;
151 int srow = row * info->rowskip;
152 btVector3 ax1;
153 // angular limits
154 if (m_solveSwingLimit)
155 {
156 btScalar* J1 = info->m_J1angularAxis;
157 btScalar* J2 = info->m_J2angularAxis;
158 if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
159 {
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;
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];
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;
184 }
185 else
186 {
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;
195
196 info->m_constraintError[srow] = k * m_swingCorrection;
197 if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
198 {
199 info->cfm[srow] = m_angCFM;
200 }
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;
205 }
206 }
207 if (m_solveTwistLimit)
208 {
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)
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;
229 info->m_upperLimit[srow] = SIMD_INFINITY;
230 }
231 else
232 {
233 info->m_lowerLimit[srow] = -SIMD_INFINITY;
234 info->m_upperLimit[srow] = 0;
235 }
236 }
237 else
238 {
239 info->m_lowerLimit[srow] = -SIMD_INFINITY;
240 info->m_upperLimit[srow] = SIMD_INFINITY;
241 }
242 srow += info->rowskip;
243 }
244 }
245
buildJacobian()246 void btConeTwistConstraint::buildJacobian()
247 {
248 if (m_useSolveConstraintObsolete)
249 {
250 m_appliedImpulse = btScalar(0.);
251 m_accTwistLimitImpulse = btScalar(0.);
252 m_accSwingLimitImpulse = btScalar(0.);
253 m_accMotorImpulse = btVector3(0., 0., 0.);
254
255 if (!m_angularOnly)
256 {
257 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
258 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
259 btVector3 relPos = pivotBInW - pivotAInW;
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(
276 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
277 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
278 pivotAInW - m_rbA.getCenterOfMassPosition(),
279 pivotBInW - m_rbB.getCenterOfMassPosition(),
280 normal[i],
281 m_rbA.getInvInertiaDiagLocal(),
282 m_rbA.getInvMass(),
283 m_rbB.getInvInertiaDiagLocal(),
284 m_rbB.getInvMass());
285 }
286 }
287
288 calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
289 }
290 }
291
solveConstraintObsolete(btSolverBody & bodyA,btSolverBody & bodyB,btScalar timeStep)292 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep)
293 {
294 #ifndef __SPU__
295 if (m_useSolveConstraintObsolete)
296 {
297 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
298 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
299
300 btScalar tau = btScalar(0.3);
301
302 //linear part
303 if (!m_angularOnly)
304 {
305 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
306 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
307
308 btVector3 vel1;
309 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
310 btVector3 vel2;
311 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
312 btVector3 vel = vel1 - vel2;
313
314 for (int i = 0; i < 3; i++)
315 {
316 const btVector3& normal = m_jac[i].m_linearJointAxis;
317 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
318
319 btScalar rel_vel;
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;
325
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);
330 }
331 }
332
333 // apply motor
334 if (m_bMotorEnabled)
335 {
336 // compute current and predicted transforms
337 btTransform trACur = m_rbA.getCenterOfMassTransform();
338 btTransform trBCur = m_rbB.getCenterOfMassTransform();
339 btVector3 omegaA;
340 bodyA.internalGetAngularVelocity(omegaA);
341 btVector3 omegaB;
342 bodyB.internalGetAngularVelocity(omegaB);
343 btTransform trAPred;
344 trAPred.setIdentity();
345 btVector3 zerovec(0, 0, 0);
346 btTransformUtil::integrateTransform(
347 trACur, zerovec, omegaA, timeStep, trAPred);
348 btTransform trBPred;
349 trBPred.setIdentity();
350 btTransformUtil::integrateTransform(
351 trBCur, zerovec, omegaB, timeStep, trBPred);
352
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();
358
359 // compute desired omegas in world
360 btVector3 omegaADes, omegaBDes;
361
362 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
363 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
364
365 // compute delta omegas
366 btVector3 dOmegaA = omegaADes - omegaA;
367 btVector3 dOmegaB = omegaBDes - omegaB;
368
369 // compute weighted avg axis of dOmega (weighting based on inertias)
370 btVector3 axisA, axisB;
371 btScalar kAxisAInv = 0, kAxisBInv = 0;
372
373 if (dOmegaA.length2() > SIMD_EPSILON)
374 {
375 axisA = dOmegaA.normalized();
376 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
377 }
378
379 if (dOmegaB.length2() > SIMD_EPSILON)
380 {
381 axisB = dOmegaB.normalized();
382 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
383 }
384
385 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
386
387 static bool bDoTorque = true;
388 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
389 {
390 avgAxis.normalize();
391 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
392 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
393 btScalar kInvCombined = kAxisAInv + kAxisBInv;
394
395 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
396 (kInvCombined * kInvCombined);
397
398 if (m_maxMotorImpulse >= 0)
399 {
400 btScalar fMaxImpulse = m_maxMotorImpulse;
401 if (m_bNormalizedMotorStrength)
402 fMaxImpulse = fMaxImpulse / kAxisAInv;
403
404 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
405 btScalar newUnclampedMag = newUnclampedAccImpulse.length();
406 if (newUnclampedMag > fMaxImpulse)
407 {
408 newUnclampedAccImpulse.normalize();
409 newUnclampedAccImpulse *= fMaxImpulse;
410 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
411 }
412 m_accMotorImpulse += impulse;
413 }
414
415 btScalar impulseMag = impulse.length();
416 btVector3 impulseAxis = impulse / impulseMag;
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 {
424 btVector3 angVelA;
425 bodyA.internalGetAngularVelocity(angVelA);
426 btVector3 angVelB;
427 bodyB.internalGetAngularVelocity(angVelB);
428 btVector3 relVel = angVelB - angVelA;
429 if (relVel.length2() > SIMD_EPSILON)
430 {
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;
436
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);
441 }
442 }
443
444 // joint limits
445 {
446 ///solve angular part
447 btVector3 angVelA;
448 bodyA.internalGetAngularVelocity(angVelA);
449 btVector3 angVelB;
450 bodyB.internalGetAngularVelocity(angVelB);
451
452 // solve swing limit
453 if (m_solveSwingLimit)
454 {
455 btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
456 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
457 if (relSwingVel > 0)
458 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
459 btScalar impulseMag = amplitude * m_kSwing;
460
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;
465
466 btVector3 impulse = m_swingAxis * impulseMag;
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 {
471 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
472 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
473 impulse = impulseNoTwistCouple;
474 }
475
476 impulseMag = impulse.length();
477 btVector3 noTwistSwingAxis = impulse / impulseMag;
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
484 if (m_solveTwistLimit)
485 {
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;
491
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;
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
updateRHS(btScalar timeStep)509 void btConeTwistConstraint::updateRHS(btScalar timeStep)
510 {
511 (void)timeStep;
512 }
513
514 #ifndef __SPU__
calcAngleInfo()515 void btConeTwistConstraint::calcAngleInfo()
516 {
517 m_swingCorrection = btScalar(0.);
518 m_twistLimitSign = btScalar(0.);
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
525 b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
526 b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
527
528 btScalar swing1 = btScalar(0.), swing2 = btScalar(0.);
529
530 btScalar swx = btScalar(0.), swy = btScalar(0.);
531 btScalar thresh = btScalar(10.);
532 btScalar fact;
533
534 // Get Frame into world space
535 if (m_swingSpan1 >= btScalar(0.05f))
536 {
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));
543 swing1 *= fact;
544 }
545
546 if (m_swingSpan2 >= btScalar(0.05f))
547 {
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));
554 swing2 *= fact;
555 }
556
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;
560
561 if (EllipseAngle > 1.0f)
562 {
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;
570 }
571
572 // Twist limits
573 if (m_twistSpan >= btScalar(0.))
574 {
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;
580
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)
584 {
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;
590 }
591 else if (twist > m_twistSpan * lockedFreeFactor)
592 {
593 m_twistCorrection = (twist - m_twistSpan);
594 m_solveTwistLimit = true;
595 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
596 m_twistAxis.normalize();
597 }
598 }
599 }
600 #endif //__SPU__
601
602 static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space
603
calcAngleInfo2(const btTransform & transA,const btTransform & transB,const btMatrix3x3 & invInertiaWorldA,const btMatrix3x3 & invInertiaWorldB)604 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
605 {
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))
624 {
625 return;
626 }
627 m_swingAxis = swingAxis;
628 m_swingAxis.normalize();
629 m_swingCorrection = qDeltaAB.getAngle();
630 if (!btFuzzyZero(m_swingCorrection))
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();
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);
647 qABCone.normalize();
648 btQuaternion qABTwist = qABCone.inverse() * qAB;
649 qABTwist.normalize();
650
651 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
652 {
653 btScalar swingAngle, swingLimit = 0;
654 btVector3 swingAxis;
655 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
656
657 if (swingAngle > swingLimit * m_limitSoftness)
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;
665 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
666 {
667 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
668 (swingLimit - swingLimit * m_limitSoftness);
669 }
670
671 // swing correction tries to get back to soft limit
672 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
673
674 // adjustment of swing axis (based on ellipse normal)
675 adjustSwingAxisToUseEllipseNormal(swingAxis);
676
677 // Calculate necessary axis & factors
678 m_swingAxis = quatRotate(qB, -swingAxis);
679
680 m_twistAxisA.setValue(0, 0, 0);
681
682 m_kSwing = btScalar(1.) /
683 (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) +
684 computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB));
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);
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))))
703 {
704 m_solveSwingLimit = true;
705 m_swingAxis = -ivB.cross(ivA);
706 }
707 }
708 else
709 {
710 if (m_swingSpan1 < m_fixThresh)
711 { // hinge around Y axis
712 // if(!(btFuzzyZero(y)))
713 if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
714 {
715 m_solveSwingLimit = true;
716 if (m_swingSpan2 >= m_fixThresh)
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;
739 if (m_swingSpan1 >= m_fixThresh)
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);
761 m_swingCorrection = m_swingAxis.length();
762
763 if (!btFuzzyZero(m_swingCorrection))
764 m_swingAxis.normalize();
765 }
766 }
767
768 if (m_twistSpan >= btScalar(0.f))
769 {
770 btVector3 twistAxis;
771 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
772
773 if (m_twistAngle > m_twistSpan * m_limitSoftness)
774 {
775 m_solveTwistLimit = true;
776
777 m_twistLimitRatio = 1.f;
778 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
779 {
780 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
781 (m_twistSpan - m_twistSpan * m_limitSoftness);
782 }
783
784 // twist correction tries to get back to soft limit
785 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
786
787 m_twistAxis = quatRotate(qB, -twistAxis);
788
789 m_kTwist = btScalar(1.) /
790 (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) +
791 computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB));
792 }
793
794 if (m_solveSwingLimit)
795 m_twistAxisA = quatRotate(qA, -twistAxis);
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".
computeConeLimitInfo(const btQuaternion & qCone,btScalar & swingAngle,btVector3 & vSwingAxis,btScalar & swingLimit)807 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
808 btScalar& swingAngle, // out
809 btVector3& vSwingAxis, // out
810 btScalar& swingLimit) // out
811 {
812 swingAngle = qCone.getAngle();
813 if (swingAngle > SIMD_EPSILON)
814 {
815 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
816 vSwingAxis.normalize();
817 #if 0
818 // non-zero twist?! this should never happen.
819 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
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.)
829 btScalar xEllipse = vSwingAxis.y();
830 btScalar yEllipse = -vSwingAxis.z();
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
840 if (fabs(xEllipse) > SIMD_EPSILON)
841 {
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);
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
GetPointForAngle(btScalar fAngleInRadians,btScalar fLength) const871 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
872 {
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);
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)
885 if (fabs(xEllipse) > SIMD_EPSILON)
886 {
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);
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
896 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
897 btQuaternion qSwing(vSwingAxis, swingLimit);
898 btVector3 vPointInConstraintSpace(fLength, 0, 0);
899 return quatRotate(qSwing, vPointInConstraintSpace);
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.
computeTwistLimitInfo(const btQuaternion & qTwist,btScalar & twistAngle,btVector3 & vTwistAxis)904 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
905 btScalar& twistAngle, // out
906 btVector3& vTwistAxis) // out
907 {
908 btQuaternion qMinTwist = qTwist;
909 twistAngle = qTwist.getAngle();
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
924 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
925 if (twistAngle > SIMD_EPSILON)
926 vTwistAxis.normalize();
927 }
928
adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const929 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
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;
946 grad *= m_swingSpan2 / m_swingSpan1;
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
setMotorTarget(const btQuaternion & q)961 void btConeTwistConstraint::setMotorTarget(const btQuaternion& q)
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
970 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
971 setMotorTargetInConstraintSpace(qConstraint);
972 }
973
setMotorTargetInConstraintSpace(const btQuaternion & q)974 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q)
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
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();
988
989 // clamp cone
990 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
991 {
992 btScalar swingAngle, swingLimit;
993 btVector3 swingAxis;
994 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
995
996 if (fabs(swingAngle) > SIMD_EPSILON)
997 {
998 if (swingAngle > swingLimit * softness)
999 swingAngle = swingLimit * softness;
1000 else if (swingAngle < -swingLimit * softness)
1001 swingAngle = -swingLimit * softness;
1002 qTargetCone = btQuaternion(swingAxis, swingAngle);
1003 }
1004 }
1005
1006 // clamp twist
1007 if (m_twistSpan >= btScalar(0.05f))
1008 {
1009 btScalar twistAngle;
1010 btVector3 twistAxis;
1011 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1012
1013 if (fabs(twistAngle) > SIMD_EPSILON)
1014 {
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);
1021 }
1022 }
1023
1024 m_qTarget = qTargetCone * qTargetTwist;
1025 }
1026 }
1027
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.
setParam(int num,btScalar value,int axis)1030 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1031 {
1032 switch (num)
1033 {
1034 case BT_CONSTRAINT_ERP:
1035 case BT_CONSTRAINT_STOP_ERP:
1036 if ((axis >= 0) && (axis < 3))
1037 {
1038 m_linERP = value;
1039 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1040 }
1041 else
1042 {
1043 m_biasFactor = value;
1044 }
1045 break;
1046 case BT_CONSTRAINT_CFM:
1047 case BT_CONSTRAINT_STOP_CFM:
1048 if ((axis >= 0) && (axis < 3))
1049 {
1050 m_linCFM = value;
1051 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1052 }
1053 else
1054 {
1055 m_angCFM = value;
1056 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1057 }
1058 break;
1059 default:
1060 btAssertConstrParams(0);
1061 break;
1062 }
1063 }
1064
1065 ///return the local value of parameter
getParam(int num,int axis) const1066 btScalar btConeTwistConstraint::getParam(int num, int axis) const
1067 {
1068 btScalar retVal = 0;
1069 switch (num)
1070 {
1071 case BT_CONSTRAINT_ERP:
1072 case BT_CONSTRAINT_STOP_ERP:
1073 if ((axis >= 0) && (axis < 3))
1074 {
1075 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1076 retVal = m_linERP;
1077 }
1078 else if ((axis >= 3) && (axis < 6))
1079 {
1080 retVal = m_biasFactor;
1081 }
1082 else
1083 {
1084 btAssertConstrParams(0);
1085 }
1086 break;
1087 case BT_CONSTRAINT_CFM:
1088 case BT_CONSTRAINT_STOP_CFM:
1089 if ((axis >= 0) && (axis < 3))
1090 {
1091 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1092 retVal = m_linCFM;
1093 }
1094 else if ((axis >= 3) && (axis < 6))
1095 {
1096 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1097 retVal = m_angCFM;
1098 }
1099 else
1100 {
1101 btAssertConstrParams(0);
1102 }
1103 break;
1104 default:
1105 btAssertConstrParams(0);
1106 }
1107 return retVal;
1108 }
1109
setFrames(const btTransform & frameA,const btTransform & frameB)1110 void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
1111 {
1112 m_rbAFrame = frameA;
1113 m_rbBFrame = frameB;
1114 buildJacobian();
1115 //calculateTransforms();
1116 }
1117