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
19 #include "btConeTwistConstraint.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "LinearMath/btTransformUtil.h"
22 #include "LinearMath/btMinMax.h"
23 #include <new>
24
25
26
27 //#define CONETWIST_USE_OBSOLETE_SOLVER true
28 #define CONETWIST_USE_OBSOLETE_SOLVER false
29 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
30
31
computeAngularImpulseDenominator(const btVector3 & axis,const btMatrix3x3 & invInertiaWorld)32 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
33 {
34 btVector3 vec = axis * invInertiaWorld;
35 return axis.dot(vec);
36 }
37
38
39
40
btConeTwistConstraint(btRigidBody & rbA,btRigidBody & rbB,const btTransform & rbAFrame,const btTransform & rbBFrame)41 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
42 const btTransform& rbAFrame,const btTransform& rbBFrame)
43 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
44 m_angularOnly(false),
45 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
46 {
47 init();
48 }
49
btConeTwistConstraint(btRigidBody & rbA,const btTransform & rbAFrame)50 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
51 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
52 m_angularOnly(false),
53 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
54 {
55 m_rbBFrame = m_rbAFrame;
56 m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
57 init();
58 }
59
60
init()61 void btConeTwistConstraint::init()
62 {
63 m_angularOnly = false;
64 m_solveTwistLimit = false;
65 m_solveSwingLimit = false;
66 m_bMotorEnabled = false;
67 m_maxMotorImpulse = btScalar(-1);
68
69 setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
70 m_damping = btScalar(0.01);
71 m_fixThresh = CONETWIST_DEF_FIX_THRESH;
72 m_flags = 0;
73 m_linCFM = btScalar(0.f);
74 m_linERP = btScalar(0.7f);
75 m_angCFM = btScalar(0.f);
76 }
77
78
getInfo1(btConstraintInfo1 * info)79 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
80 {
81 if (m_useSolveConstraintObsolete)
82 {
83 info->m_numConstraintRows = 0;
84 info->nub = 0;
85 }
86 else
87 {
88 info->m_numConstraintRows = 3;
89 info->nub = 3;
90 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
91 if(m_solveSwingLimit)
92 {
93 info->m_numConstraintRows++;
94 info->nub--;
95 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
96 {
97 info->m_numConstraintRows++;
98 info->nub--;
99 }
100 }
101 if(m_solveTwistLimit)
102 {
103 info->m_numConstraintRows++;
104 info->nub--;
105 }
106 }
107 }
108
getInfo1NonVirtual(btConstraintInfo1 * info)109 void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
110 {
111 //always reserve 6 rows: object transform is not available on SPU
112 info->m_numConstraintRows = 6;
113 info->nub = 0;
114
115 }
116
117
getInfo2(btConstraintInfo2 * info)118 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
119 {
120 getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
121 }
122
getInfo2NonVirtual(btConstraintInfo2 * info,const btTransform & transA,const btTransform & transB,const btMatrix3x3 & invInertiaWorldA,const btMatrix3x3 & invInertiaWorldB)123 void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
124 {
125 calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
126
127 btAssert(!m_useSolveConstraintObsolete);
128 // set jacobian
129 info->m_J1linearAxis[0] = 1;
130 info->m_J1linearAxis[info->rowskip+1] = 1;
131 info->m_J1linearAxis[2*info->rowskip+2] = 1;
132 btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
133 {
134 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
135 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
136 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
137 btVector3 a1neg = -a1;
138 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
139 }
140 info->m_J2linearAxis[0] = -1;
141 info->m_J2linearAxis[info->rowskip+1] = -1;
142 info->m_J2linearAxis[2*info->rowskip+2] = -1;
143 btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
144 {
145 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
146 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
147 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
148 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
149 }
150 // set right hand side
151 btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
152 btScalar k = info->fps * linERP;
153 int j;
154 for (j=0; j<3; j++)
155 {
156 info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
157 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
158 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
159 if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
160 {
161 info->cfm[j*info->rowskip] = m_linCFM;
162 }
163 }
164 int row = 3;
165 int srow = row * info->rowskip;
166 btVector3 ax1;
167 // angular limits
168 if(m_solveSwingLimit)
169 {
170 btScalar *J1 = info->m_J1angularAxis;
171 btScalar *J2 = info->m_J2angularAxis;
172 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
173 {
174 btTransform trA = transA*m_rbAFrame;
175 btVector3 p = trA.getBasis().getColumn(1);
176 btVector3 q = trA.getBasis().getColumn(2);
177 int srow1 = srow + info->rowskip;
178 J1[srow+0] = p[0];
179 J1[srow+1] = p[1];
180 J1[srow+2] = p[2];
181 J1[srow1+0] = q[0];
182 J1[srow1+1] = q[1];
183 J1[srow1+2] = q[2];
184 J2[srow+0] = -p[0];
185 J2[srow+1] = -p[1];
186 J2[srow+2] = -p[2];
187 J2[srow1+0] = -q[0];
188 J2[srow1+1] = -q[1];
189 J2[srow1+2] = -q[2];
190 btScalar fact = info->fps * m_relaxationFactor;
191 info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
192 info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
193 info->m_lowerLimit[srow] = -SIMD_INFINITY;
194 info->m_upperLimit[srow] = SIMD_INFINITY;
195 info->m_lowerLimit[srow1] = -SIMD_INFINITY;
196 info->m_upperLimit[srow1] = SIMD_INFINITY;
197 srow = srow1 + info->rowskip;
198 }
199 else
200 {
201 ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
202 J1[srow+0] = ax1[0];
203 J1[srow+1] = ax1[1];
204 J1[srow+2] = ax1[2];
205 J2[srow+0] = -ax1[0];
206 J2[srow+1] = -ax1[1];
207 J2[srow+2] = -ax1[2];
208 btScalar k = info->fps * m_biasFactor;
209
210 info->m_constraintError[srow] = k * m_swingCorrection;
211 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
212 {
213 info->cfm[srow] = m_angCFM;
214 }
215 // m_swingCorrection is always positive or 0
216 info->m_lowerLimit[srow] = 0;
217 info->m_upperLimit[srow] = SIMD_INFINITY;
218 srow += info->rowskip;
219 }
220 }
221 if(m_solveTwistLimit)
222 {
223 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
224 btScalar *J1 = info->m_J1angularAxis;
225 btScalar *J2 = info->m_J2angularAxis;
226 J1[srow+0] = ax1[0];
227 J1[srow+1] = ax1[1];
228 J1[srow+2] = ax1[2];
229 J2[srow+0] = -ax1[0];
230 J2[srow+1] = -ax1[1];
231 J2[srow+2] = -ax1[2];
232 btScalar k = info->fps * m_biasFactor;
233 info->m_constraintError[srow] = k * m_twistCorrection;
234 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
235 {
236 info->cfm[srow] = m_angCFM;
237 }
238 if(m_twistSpan > 0.0f)
239 {
240
241 if(m_twistCorrection > 0.0f)
242 {
243 info->m_lowerLimit[srow] = 0;
244 info->m_upperLimit[srow] = SIMD_INFINITY;
245 }
246 else
247 {
248 info->m_lowerLimit[srow] = -SIMD_INFINITY;
249 info->m_upperLimit[srow] = 0;
250 }
251 }
252 else
253 {
254 info->m_lowerLimit[srow] = -SIMD_INFINITY;
255 info->m_upperLimit[srow] = SIMD_INFINITY;
256 }
257 srow += info->rowskip;
258 }
259 }
260
261
262
buildJacobian()263 void btConeTwistConstraint::buildJacobian()
264 {
265 if (m_useSolveConstraintObsolete)
266 {
267 m_appliedImpulse = btScalar(0.);
268 m_accTwistLimitImpulse = btScalar(0.);
269 m_accSwingLimitImpulse = btScalar(0.);
270 m_accMotorImpulse = btVector3(0.,0.,0.);
271
272 if (!m_angularOnly)
273 {
274 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
275 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
276 btVector3 relPos = pivotBInW - pivotAInW;
277
278 btVector3 normal[3];
279 if (relPos.length2() > SIMD_EPSILON)
280 {
281 normal[0] = relPos.normalized();
282 }
283 else
284 {
285 normal[0].setValue(btScalar(1.0),0,0);
286 }
287
288 btPlaneSpace1(normal[0], normal[1], normal[2]);
289
290 for (int i=0;i<3;i++)
291 {
292 new (&m_jac[i]) btJacobianEntry(
293 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
294 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
295 pivotAInW - m_rbA.getCenterOfMassPosition(),
296 pivotBInW - m_rbB.getCenterOfMassPosition(),
297 normal[i],
298 m_rbA.getInvInertiaDiagLocal(),
299 m_rbA.getInvMass(),
300 m_rbB.getInvInertiaDiagLocal(),
301 m_rbB.getInvMass());
302 }
303 }
304
305 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
306 }
307 }
308
309
310
solveConstraintObsolete(btSolverBody & bodyA,btSolverBody & bodyB,btScalar timeStep)311 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
312 {
313 #ifndef __SPU__
314 if (m_useSolveConstraintObsolete)
315 {
316 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
317 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
318
319 btScalar tau = btScalar(0.3);
320
321 //linear part
322 if (!m_angularOnly)
323 {
324 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
325 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
326
327 btVector3 vel1;
328 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
329 btVector3 vel2;
330 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
331 btVector3 vel = vel1 - vel2;
332
333 for (int i=0;i<3;i++)
334 {
335 const btVector3& normal = m_jac[i].m_linearJointAxis;
336 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
337
338 btScalar rel_vel;
339 rel_vel = normal.dot(vel);
340 //positional error (zeroth order error)
341 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
342 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
343 m_appliedImpulse += impulse;
344
345 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
346 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
347 bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
348 bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
349
350 }
351 }
352
353 // apply motor
354 if (m_bMotorEnabled)
355 {
356 // compute current and predicted transforms
357 btTransform trACur = m_rbA.getCenterOfMassTransform();
358 btTransform trBCur = m_rbB.getCenterOfMassTransform();
359 btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
360 btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
361 btTransform trAPred; trAPred.setIdentity();
362 btVector3 zerovec(0,0,0);
363 btTransformUtil::integrateTransform(
364 trACur, zerovec, omegaA, timeStep, trAPred);
365 btTransform trBPred; trBPred.setIdentity();
366 btTransformUtil::integrateTransform(
367 trBCur, zerovec, omegaB, timeStep, trBPred);
368
369 // compute desired transforms in world
370 btTransform trPose(m_qTarget);
371 btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
372 btTransform trADes = trBPred * trABDes;
373 btTransform trBDes = trAPred * trABDes.inverse();
374
375 // compute desired omegas in world
376 btVector3 omegaADes, omegaBDes;
377
378 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
379 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
380
381 // compute delta omegas
382 btVector3 dOmegaA = omegaADes - omegaA;
383 btVector3 dOmegaB = omegaBDes - omegaB;
384
385 // compute weighted avg axis of dOmega (weighting based on inertias)
386 btVector3 axisA, axisB;
387 btScalar kAxisAInv = 0, kAxisBInv = 0;
388
389 if (dOmegaA.length2() > SIMD_EPSILON)
390 {
391 axisA = dOmegaA.normalized();
392 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
393 }
394
395 if (dOmegaB.length2() > SIMD_EPSILON)
396 {
397 axisB = dOmegaB.normalized();
398 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
399 }
400
401 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
402
403 static bool bDoTorque = true;
404 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
405 {
406 avgAxis.normalize();
407 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
408 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
409 btScalar kInvCombined = kAxisAInv + kAxisBInv;
410
411 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
412 (kInvCombined * kInvCombined);
413
414 if (m_maxMotorImpulse >= 0)
415 {
416 btScalar fMaxImpulse = m_maxMotorImpulse;
417 if (m_bNormalizedMotorStrength)
418 fMaxImpulse = fMaxImpulse/kAxisAInv;
419
420 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
421 btScalar newUnclampedMag = newUnclampedAccImpulse.length();
422 if (newUnclampedMag > fMaxImpulse)
423 {
424 newUnclampedAccImpulse.normalize();
425 newUnclampedAccImpulse *= fMaxImpulse;
426 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
427 }
428 m_accMotorImpulse += impulse;
429 }
430
431 btScalar impulseMag = impulse.length();
432 btVector3 impulseAxis = impulse / impulseMag;
433
434 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
435 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
436
437 }
438 }
439 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
440 {
441 btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
442 btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
443 btVector3 relVel = angVelB - angVelA;
444 if (relVel.length2() > SIMD_EPSILON)
445 {
446 btVector3 relVelAxis = relVel.normalized();
447 btScalar m_kDamping = btScalar(1.) /
448 (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
449 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
450 btVector3 impulse = m_damping * m_kDamping * relVel;
451
452 btScalar impulseMag = impulse.length();
453 btVector3 impulseAxis = impulse / impulseMag;
454 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
455 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
456 }
457 }
458
459 // joint limits
460 {
461 ///solve angular part
462 btVector3 angVelA;
463 bodyA.internalGetAngularVelocity(angVelA);
464 btVector3 angVelB;
465 bodyB.internalGetAngularVelocity(angVelB);
466
467 // solve swing limit
468 if (m_solveSwingLimit)
469 {
470 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
471 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
472 if (relSwingVel > 0)
473 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
474 btScalar impulseMag = amplitude * m_kSwing;
475
476 // Clamp the accumulated impulse
477 btScalar temp = m_accSwingLimitImpulse;
478 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
479 impulseMag = m_accSwingLimitImpulse - temp;
480
481 btVector3 impulse = m_swingAxis * impulseMag;
482
483 // don't let cone response affect twist
484 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
485 {
486 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
487 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
488 impulse = impulseNoTwistCouple;
489 }
490
491 impulseMag = impulse.length();
492 btVector3 noTwistSwingAxis = impulse / impulseMag;
493
494 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
495 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
496 }
497
498
499 // solve twist limit
500 if (m_solveTwistLimit)
501 {
502 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
503 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
504 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
505 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
506 btScalar impulseMag = amplitude * m_kTwist;
507
508 // Clamp the accumulated impulse
509 btScalar temp = m_accTwistLimitImpulse;
510 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
511 impulseMag = m_accTwistLimitImpulse - temp;
512
513 // btVector3 impulse = m_twistAxis * impulseMag;
514
515 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
516 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
517 }
518 }
519 }
520 #else
521 btAssert(0);
522 #endif //__SPU__
523 }
524
525
526
527
updateRHS(btScalar timeStep)528 void btConeTwistConstraint::updateRHS(btScalar timeStep)
529 {
530 (void)timeStep;
531
532 }
533
534
535 #ifndef __SPU__
calcAngleInfo()536 void btConeTwistConstraint::calcAngleInfo()
537 {
538 m_swingCorrection = btScalar(0.);
539 m_twistLimitSign = btScalar(0.);
540 m_solveTwistLimit = false;
541 m_solveSwingLimit = false;
542
543 btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
544 btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
545
546 b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
547 b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
548
549 btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
550
551 btScalar swx=btScalar(0.),swy = btScalar(0.);
552 btScalar thresh = btScalar(10.);
553 btScalar fact;
554
555 // Get Frame into world space
556 if (m_swingSpan1 >= btScalar(0.05f))
557 {
558 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
559 swx = b2Axis1.dot(b1Axis1);
560 swy = b2Axis1.dot(b1Axis2);
561 swing1 = btAtan2Fast(swy, swx);
562 fact = (swy*swy + swx*swx) * thresh * thresh;
563 fact = fact / (fact + btScalar(1.0));
564 swing1 *= fact;
565 }
566
567 if (m_swingSpan2 >= btScalar(0.05f))
568 {
569 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
570 swx = b2Axis1.dot(b1Axis1);
571 swy = b2Axis1.dot(b1Axis3);
572 swing2 = btAtan2Fast(swy, swx);
573 fact = (swy*swy + swx*swx) * thresh * thresh;
574 fact = fact / (fact + btScalar(1.0));
575 swing2 *= fact;
576 }
577
578 btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
579 btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
580 btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
581
582 if (EllipseAngle > 1.0f)
583 {
584 m_swingCorrection = EllipseAngle-1.0f;
585 m_solveSwingLimit = true;
586 // Calculate necessary axis & factors
587 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
588 m_swingAxis.normalize();
589 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
590 m_swingAxis *= swingAxisSign;
591 }
592
593 // Twist limits
594 if (m_twistSpan >= btScalar(0.))
595 {
596 btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
597 btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
598 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
599 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
600 m_twistAngle = twist;
601
602 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
603 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
604 if (twist <= -m_twistSpan*lockedFreeFactor)
605 {
606 m_twistCorrection = -(twist + m_twistSpan);
607 m_solveTwistLimit = true;
608 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
609 m_twistAxis.normalize();
610 m_twistAxis *= -1.0f;
611 }
612 else if (twist > m_twistSpan*lockedFreeFactor)
613 {
614 m_twistCorrection = (twist - m_twistSpan);
615 m_solveTwistLimit = true;
616 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
617 m_twistAxis.normalize();
618 }
619 }
620 }
621 #endif //__SPU__
622
623 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
624
625
626
calcAngleInfo2(const btTransform & transA,const btTransform & transB,const btMatrix3x3 & invInertiaWorldA,const btMatrix3x3 & invInertiaWorldB)627 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
628 {
629 m_swingCorrection = btScalar(0.);
630 m_twistLimitSign = btScalar(0.);
631 m_solveTwistLimit = false;
632 m_solveSwingLimit = false;
633 // compute rotation of A wrt B (in constraint space)
634 if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
635 { // it is assumed that setMotorTarget() was alredy called
636 // and motor target m_qTarget is within constraint limits
637 // TODO : split rotation to pure swing and pure twist
638 // compute desired transforms in world
639 btTransform trPose(m_qTarget);
640 btTransform trA = transA * m_rbAFrame;
641 btTransform trB = transB * m_rbBFrame;
642 btTransform trDeltaAB = trB * trPose * trA.inverse();
643 btQuaternion qDeltaAB = trDeltaAB.getRotation();
644 btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
645 float swingAxisLen2 = swingAxis.length2();
646 if(btFuzzyZero(swingAxisLen2))
647 {
648 return;
649 }
650 m_swingAxis = swingAxis;
651 m_swingAxis.normalize();
652 m_swingCorrection = qDeltaAB.getAngle();
653 if(!btFuzzyZero(m_swingCorrection))
654 {
655 m_solveSwingLimit = true;
656 }
657 return;
658 }
659
660
661 {
662 // compute rotation of A wrt B (in constraint space)
663 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
664 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
665 btQuaternion qAB = qB.inverse() * qA;
666 // split rotation into cone and twist
667 // (all this is done from B's perspective. Maybe I should be averaging axes...)
668 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
669 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
670 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
671
672 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
673 {
674 btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
675 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
676
677 if (swingAngle > swingLimit * m_limitSoftness)
678 {
679 m_solveSwingLimit = true;
680
681 // compute limit ratio: 0->1, where
682 // 0 == beginning of soft limit
683 // 1 == hard/real limit
684 m_swingLimitRatio = 1.f;
685 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
686 {
687 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
688 (swingLimit - swingLimit * m_limitSoftness);
689 }
690
691 // swing correction tries to get back to soft limit
692 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
693
694 // adjustment of swing axis (based on ellipse normal)
695 adjustSwingAxisToUseEllipseNormal(swingAxis);
696
697 // Calculate necessary axis & factors
698 m_swingAxis = quatRotate(qB, -swingAxis);
699
700 m_twistAxisA.setValue(0,0,0);
701
702 m_kSwing = btScalar(1.) /
703 (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
704 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
705 }
706 }
707 else
708 {
709 // you haven't set any limits;
710 // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
711 // anyway, we have either hinge or fixed joint
712 btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
713 btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
714 btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
715 btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
716 btVector3 target;
717 btScalar x = ivB.dot(ivA);
718 btScalar y = ivB.dot(jvA);
719 btScalar z = ivB.dot(kvA);
720 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
721 { // fixed. We'll need to add one more row to constraint
722 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
723 {
724 m_solveSwingLimit = true;
725 m_swingAxis = -ivB.cross(ivA);
726 }
727 }
728 else
729 {
730 if(m_swingSpan1 < m_fixThresh)
731 { // hinge around Y axis
732 // if(!(btFuzzyZero(y)))
733 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
734 {
735 m_solveSwingLimit = true;
736 if(m_swingSpan2 >= m_fixThresh)
737 {
738 y = btScalar(0.f);
739 btScalar span2 = btAtan2(z, x);
740 if(span2 > m_swingSpan2)
741 {
742 x = btCos(m_swingSpan2);
743 z = btSin(m_swingSpan2);
744 }
745 else if(span2 < -m_swingSpan2)
746 {
747 x = btCos(m_swingSpan2);
748 z = -btSin(m_swingSpan2);
749 }
750 }
751 }
752 }
753 else
754 { // hinge around Z axis
755 // if(!btFuzzyZero(z))
756 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
757 {
758 m_solveSwingLimit = true;
759 if(m_swingSpan1 >= m_fixThresh)
760 {
761 z = btScalar(0.f);
762 btScalar span1 = btAtan2(y, x);
763 if(span1 > m_swingSpan1)
764 {
765 x = btCos(m_swingSpan1);
766 y = btSin(m_swingSpan1);
767 }
768 else if(span1 < -m_swingSpan1)
769 {
770 x = btCos(m_swingSpan1);
771 y = -btSin(m_swingSpan1);
772 }
773 }
774 }
775 }
776 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
777 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
778 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
779 target.normalize();
780 m_swingAxis = -ivB.cross(target);
781 m_swingCorrection = m_swingAxis.length();
782
783 if (!btFuzzyZero(m_swingCorrection))
784 m_swingAxis.normalize();
785 }
786 }
787
788 if (m_twistSpan >= btScalar(0.f))
789 {
790 btVector3 twistAxis;
791 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
792
793 if (m_twistAngle > m_twistSpan*m_limitSoftness)
794 {
795 m_solveTwistLimit = true;
796
797 m_twistLimitRatio = 1.f;
798 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
799 {
800 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
801 (m_twistSpan - m_twistSpan * m_limitSoftness);
802 }
803
804 // twist correction tries to get back to soft limit
805 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
806
807 m_twistAxis = quatRotate(qB, -twistAxis);
808
809 m_kTwist = btScalar(1.) /
810 (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
811 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
812 }
813
814 if (m_solveSwingLimit)
815 m_twistAxisA = quatRotate(qA, -twistAxis);
816 }
817 else
818 {
819 m_twistAngle = btScalar(0.f);
820 }
821 }
822 }
823
824
825
826 // given a cone rotation in constraint space, (pre: twist must already be removed)
827 // this method computes its corresponding swing angle and axis.
828 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
computeConeLimitInfo(const btQuaternion & qCone,btScalar & swingAngle,btVector3 & vSwingAxis,btScalar & swingLimit)829 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
830 btScalar& swingAngle, // out
831 btVector3& vSwingAxis, // out
832 btScalar& swingLimit) // out
833 {
834 swingAngle = qCone.getAngle();
835 if (swingAngle > SIMD_EPSILON)
836 {
837 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
838 vSwingAxis.normalize();
839 #if 0
840 // non-zero twist?! this should never happen.
841 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
842 #endif
843
844 // Compute limit for given swing. tricky:
845 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
846 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
847
848 // For starters, compute the direction from center to surface of ellipse.
849 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
850 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
851 btScalar xEllipse = vSwingAxis.y();
852 btScalar yEllipse = -vSwingAxis.z();
853
854 // Now, we use the slope of the vector (using x/yEllipse) and find the length
855 // of the line that intersects the ellipse:
856 // x^2 y^2
857 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
858 // a^2 b^2
859 // Do the math and it should be clear.
860
861 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
862 if (fabs(xEllipse) > SIMD_EPSILON)
863 {
864 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
865 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
866 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
867 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
868 swingLimit = sqrt(swingLimit2);
869 }
870
871 // test!
872 /*swingLimit = m_swingSpan2;
873 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
874 {
875 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
876 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
877 btScalar phi = asin(sinphi);
878 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
879 btScalar alpha = 3.14159f - theta - phi;
880 btScalar sinalpha = sin(alpha);
881 swingLimit = m_swingSpan1 * sinphi/sinalpha;
882 }*/
883 }
884 else if (swingAngle < 0)
885 {
886 // this should never happen!
887 #if 0
888 btAssert(0);
889 #endif
890 }
891 }
892
GetPointForAngle(btScalar fAngleInRadians,btScalar fLength) const893 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
894 {
895 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
896 btScalar xEllipse = btCos(fAngleInRadians);
897 btScalar yEllipse = btSin(fAngleInRadians);
898
899 // Use the slope of the vector (using x/yEllipse) and find the length
900 // of the line that intersects the ellipse:
901 // x^2 y^2
902 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
903 // a^2 b^2
904 // Do the math and it should be clear.
905
906 float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
907 if (fabs(xEllipse) > SIMD_EPSILON)
908 {
909 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
910 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
911 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
912 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
913 swingLimit = sqrt(swingLimit2);
914 }
915
916 // convert into point in constraint space:
917 // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
918 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
919 btQuaternion qSwing(vSwingAxis, swingLimit);
920 btVector3 vPointInConstraintSpace(fLength,0,0);
921 return quatRotate(qSwing, vPointInConstraintSpace);
922 }
923
924 // given a twist rotation in constraint space, (pre: cone must already be removed)
925 // this method computes its corresponding angle and axis.
computeTwistLimitInfo(const btQuaternion & qTwist,btScalar & twistAngle,btVector3 & vTwistAxis)926 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
927 btScalar& twistAngle, // out
928 btVector3& vTwistAxis) // out
929 {
930 btQuaternion qMinTwist = qTwist;
931 twistAngle = qTwist.getAngle();
932
933 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
934 {
935 qMinTwist = -(qTwist);
936 twistAngle = qMinTwist.getAngle();
937 }
938 if (twistAngle < 0)
939 {
940 // this should never happen
941 #if 0
942 btAssert(0);
943 #endif
944 }
945
946 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
947 if (twistAngle > SIMD_EPSILON)
948 vTwistAxis.normalize();
949 }
950
951
adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const952 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
953 {
954 // the swing axis is computed as the "twist-free" cone rotation,
955 // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
956 // so, if we're outside the limits, the closest way back inside the cone isn't
957 // along the vector back to the center. better (and more stable) to use the ellipse normal.
958
959 // convert swing axis to direction from center to surface of ellipse
960 // (ie. rotate 2D vector by PI/2)
961 btScalar y = -vSwingAxis.z();
962 btScalar z = vSwingAxis.y();
963
964 // do the math...
965 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
966 {
967 // compute gradient/normal of ellipse surface at current "point"
968 btScalar grad = y/z;
969 grad *= m_swingSpan2 / m_swingSpan1;
970
971 // adjust y/z to represent normal at point (instead of vector to point)
972 if (y > 0)
973 y = fabs(grad * z);
974 else
975 y = -fabs(grad * z);
976
977 // convert ellipse direction back to swing axis
978 vSwingAxis.setZ(-y);
979 vSwingAxis.setY( z);
980 vSwingAxis.normalize();
981 }
982 }
983
984
985
setMotorTarget(const btQuaternion & q)986 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
987 {
988 //btTransform trACur = m_rbA.getCenterOfMassTransform();
989 //btTransform trBCur = m_rbB.getCenterOfMassTransform();
990 // btTransform trABCur = trBCur.inverse() * trACur;
991 // btQuaternion qABCur = trABCur.getRotation();
992 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
993 //btQuaternion qConstraintCur = trConstraintCur.getRotation();
994
995 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
996 setMotorTargetInConstraintSpace(qConstraint);
997 }
998
999
setMotorTargetInConstraintSpace(const btQuaternion & q)1000 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
1001 {
1002 m_qTarget = q;
1003
1004 // clamp motor target to within limits
1005 {
1006 btScalar softness = 1.f;//m_limitSoftness;
1007
1008 // split into twist and cone
1009 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
1010 btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
1011 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
1012
1013 // clamp cone
1014 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
1015 {
1016 btScalar swingAngle, swingLimit; btVector3 swingAxis;
1017 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1018
1019 if (fabs(swingAngle) > SIMD_EPSILON)
1020 {
1021 if (swingAngle > swingLimit*softness)
1022 swingAngle = swingLimit*softness;
1023 else if (swingAngle < -swingLimit*softness)
1024 swingAngle = -swingLimit*softness;
1025 qTargetCone = btQuaternion(swingAxis, swingAngle);
1026 }
1027 }
1028
1029 // clamp twist
1030 if (m_twistSpan >= btScalar(0.05f))
1031 {
1032 btScalar twistAngle; btVector3 twistAxis;
1033 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1034
1035 if (fabs(twistAngle) > SIMD_EPSILON)
1036 {
1037 // eddy todo: limitSoftness used here???
1038 if (twistAngle > m_twistSpan*softness)
1039 twistAngle = m_twistSpan*softness;
1040 else if (twistAngle < -m_twistSpan*softness)
1041 twistAngle = -m_twistSpan*softness;
1042 qTargetTwist = btQuaternion(twistAxis, twistAngle);
1043 }
1044 }
1045
1046 m_qTarget = qTargetCone * qTargetTwist;
1047 }
1048 }
1049
1050 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
1051 ///If no axis is provided, it uses the default axis for this constraint.
setParam(int num,btScalar value,int axis)1052 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1053 {
1054 switch(num)
1055 {
1056 case BT_CONSTRAINT_ERP :
1057 case BT_CONSTRAINT_STOP_ERP :
1058 if((axis >= 0) && (axis < 3))
1059 {
1060 m_linERP = value;
1061 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1062 }
1063 else
1064 {
1065 m_biasFactor = value;
1066 }
1067 break;
1068 case BT_CONSTRAINT_CFM :
1069 case BT_CONSTRAINT_STOP_CFM :
1070 if((axis >= 0) && (axis < 3))
1071 {
1072 m_linCFM = value;
1073 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1074 }
1075 else
1076 {
1077 m_angCFM = value;
1078 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1079 }
1080 break;
1081 default:
1082 btAssertConstrParams(0);
1083 break;
1084 }
1085 }
1086
1087 ///return the local value of parameter
getParam(int num,int axis) const1088 btScalar btConeTwistConstraint::getParam(int num, int axis) const
1089 {
1090 btScalar retVal = 0;
1091 switch(num)
1092 {
1093 case BT_CONSTRAINT_ERP :
1094 case BT_CONSTRAINT_STOP_ERP :
1095 if((axis >= 0) && (axis < 3))
1096 {
1097 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1098 retVal = m_linERP;
1099 }
1100 else if((axis >= 3) && (axis < 6))
1101 {
1102 retVal = m_biasFactor;
1103 }
1104 else
1105 {
1106 btAssertConstrParams(0);
1107 }
1108 break;
1109 case BT_CONSTRAINT_CFM :
1110 case BT_CONSTRAINT_STOP_CFM :
1111 if((axis >= 0) && (axis < 3))
1112 {
1113 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1114 retVal = m_linCFM;
1115 }
1116 else if((axis >= 3) && (axis < 6))
1117 {
1118 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1119 retVal = m_angCFM;
1120 }
1121 else
1122 {
1123 btAssertConstrParams(0);
1124 }
1125 break;
1126 default :
1127 btAssertConstrParams(0);
1128 }
1129 return retVal;
1130 }
1131
1132
setFrames(const btTransform & frameA,const btTransform & frameB)1133 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1134 {
1135 m_rbAFrame = frameA;
1136 m_rbBFrame = frameB;
1137 buildJacobian();
1138 //calculateTransforms();
1139 }
1140
1141
1142
1143
1144