bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btConeTwistConstraint.cpp
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
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
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
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
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
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
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
118 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
119 {
120         getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
121 }
122
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
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
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
528 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
529 {
530         (void)timeStep;
531
532 }
533
534
535 #ifndef __SPU__
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,b1Axis2,b1Axis3;
544         btVector3 b2Axis1,b2Axis2;
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
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                                 m_swingAxis.normalize();
783                         }
784                 }
785
786                 if (m_twistSpan >= btScalar(0.f))
787                 {
788                         btVector3 twistAxis;
789                         computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
790
791                         if (m_twistAngle > m_twistSpan*m_limitSoftness)
792                         {
793                                 m_solveTwistLimit = true;
794
795                                 m_twistLimitRatio = 1.f;
796                                 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
797                                 {
798                                         m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
799                                                                                 (m_twistSpan  - m_twistSpan * m_limitSoftness);
800                                 }
801
802                                 // twist correction tries to get back to soft limit
803                                 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
804
805                                 m_twistAxis = quatRotate(qB, -twistAxis);
806
807                                 m_kTwist = btScalar(1.) /
808                                         (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
809                                          computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
810                         }
811
812                         if (m_solveSwingLimit)
813                                 m_twistAxisA = quatRotate(qA, -twistAxis);
814                 }
815                 else
816                 {
817                         m_twistAngle = btScalar(0.f);
818                 }
819         }
820 }
821
822
823
824 // given a cone rotation in constraint space, (pre: twist must already be removed)
825 // this method computes its corresponding swing angle and axis.
826 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
827 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
828                                                                                                  btScalar& swingAngle, // out
829                                                                                                  btVector3& vSwingAxis, // out
830                                                                                                  btScalar& swingLimit) // out
831 {
832         swingAngle = qCone.getAngle();
833         if (swingAngle > SIMD_EPSILON)
834         {
835                 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
836                 vSwingAxis.normalize();
837 #if 0
838         // non-zero twist?! this should never happen.
839        btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
840 #endif
841         
842                 // Compute limit for given swing. tricky:
843                 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
844                 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
845
846                 // For starters, compute the direction from center to surface of ellipse.
847                 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
848                 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
849                 btScalar xEllipse =  vSwingAxis.y();
850                 btScalar yEllipse = -vSwingAxis.z();
851
852                 // Now, we use the slope of the vector (using x/yEllipse) and find the length
853                 // of the line that intersects the ellipse:
854                 //  x^2   y^2
855                 //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
856                 //  a^2   b^2
857                 // Do the math and it should be clear.
858
859                 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
860                 if (fabs(xEllipse) > SIMD_EPSILON)
861                 {
862                         btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
863                         btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
864                         norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
865                         btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
866                         swingLimit = sqrt(swingLimit2);
867                 }
868
869                 // test!
870                 /*swingLimit = m_swingSpan2;
871                 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
872                 {
873                 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
874                 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
875                 btScalar phi = asin(sinphi);
876                 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
877                 btScalar alpha = 3.14159f - theta - phi;
878                 btScalar sinalpha = sin(alpha);
879                 swingLimit = m_swingSpan1 * sinphi/sinalpha;
880                 }*/
881         }
882         else if (swingAngle < 0)
883         {
884                 // this should never happen!
885 #if 0
886         btAssert(0);
887 #endif
888         }
889 }
890
891 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
892 {
893         // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
894         btScalar xEllipse = btCos(fAngleInRadians);
895         btScalar yEllipse = btSin(fAngleInRadians);
896
897         // Use the slope of the vector (using x/yEllipse) and find the length
898         // of the line that intersects the ellipse:
899         //  x^2   y^2
900         //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
901         //  a^2   b^2
902         // Do the math and it should be clear.
903
904         float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
905         if (fabs(xEllipse) > SIMD_EPSILON)
906         {
907                 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
908                 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
909                 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
910                 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
911                 swingLimit = sqrt(swingLimit2);
912         }
913
914         // convert into point in constraint space:
915         // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
916         btVector3 vSwingAxis(0, xEllipse, -yEllipse);
917         btQuaternion qSwing(vSwingAxis, swingLimit);
918         btVector3 vPointInConstraintSpace(fLength,0,0);
919         return quatRotate(qSwing, vPointInConstraintSpace);
920 }
921
922 // given a twist rotation in constraint space, (pre: cone must already be removed)
923 // this method computes its corresponding angle and axis.
924 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
925                                                                                                   btScalar& twistAngle, // out
926                                                                                                   btVector3& vTwistAxis) // out
927 {
928         btQuaternion qMinTwist = qTwist;
929         twistAngle = qTwist.getAngle();
930
931         if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
932         {
933                 qMinTwist = -(qTwist);
934                 twistAngle = qMinTwist.getAngle();
935         }
936         if (twistAngle < 0)
937         {
938                 // this should never happen
939 #if 0
940         btAssert(0);
941 #endif
942         }
943
944         vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
945         if (twistAngle > SIMD_EPSILON)
946                 vTwistAxis.normalize();
947 }
948
949
950 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
951 {
952         // the swing axis is computed as the "twist-free" cone rotation,
953         // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
954         // so, if we're outside the limits, the closest way back inside the cone isn't 
955         // along the vector back to the center. better (and more stable) to use the ellipse normal.
956
957         // convert swing axis to direction from center to surface of ellipse
958         // (ie. rotate 2D vector by PI/2)
959         btScalar y = -vSwingAxis.z();
960         btScalar z =  vSwingAxis.y();
961
962         // do the math...
963         if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
964         {
965                 // compute gradient/normal of ellipse surface at current "point"
966                 btScalar grad = y/z;
967                 grad *= m_swingSpan2 / m_swingSpan1;
968
969                 // adjust y/z to represent normal at point (instead of vector to point)
970                 if (y > 0)
971                         y =  fabs(grad * z);
972                 else
973                         y = -fabs(grad * z);
974
975                 // convert ellipse direction back to swing axis
976                 vSwingAxis.setZ(-y);
977                 vSwingAxis.setY( z);
978                 vSwingAxis.normalize();
979         }
980 }
981
982
983
984 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
985 {
986         btTransform trACur = m_rbA.getCenterOfMassTransform();
987         btTransform trBCur = m_rbB.getCenterOfMassTransform();
988 //      btTransform trABCur = trBCur.inverse() * trACur;
989 //      btQuaternion qABCur = trABCur.getRotation();
990 //      btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
991         //btQuaternion qConstraintCur = trConstraintCur.getRotation();
992
993         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
994         setMotorTargetInConstraintSpace(qConstraint);
995 }
996
997
998 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
999 {
1000         m_qTarget = q;
1001
1002         // clamp motor target to within limits
1003         {
1004                 btScalar softness = 1.f;//m_limitSoftness;
1005
1006                 // split into twist and cone
1007                 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
1008                 btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
1009                 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
1010
1011                 // clamp cone
1012                 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
1013                 {
1014                         btScalar swingAngle, swingLimit; btVector3 swingAxis;
1015                         computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1016
1017                         if (fabs(swingAngle) > SIMD_EPSILON)
1018                         {
1019                                 if (swingAngle > swingLimit*softness)
1020                                         swingAngle = swingLimit*softness;
1021                                 else if (swingAngle < -swingLimit*softness)
1022                                         swingAngle = -swingLimit*softness;
1023                                 qTargetCone = btQuaternion(swingAxis, swingAngle);
1024                         }
1025                 }
1026
1027                 // clamp twist
1028                 if (m_twistSpan >= btScalar(0.05f))
1029                 {
1030                         btScalar twistAngle; btVector3 twistAxis;
1031                         computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1032
1033                         if (fabs(twistAngle) > SIMD_EPSILON)
1034                         {
1035                                 // eddy todo: limitSoftness used here???
1036                                 if (twistAngle > m_twistSpan*softness)
1037                                         twistAngle = m_twistSpan*softness;
1038                                 else if (twistAngle < -m_twistSpan*softness)
1039                                         twistAngle = -m_twistSpan*softness;
1040                                 qTargetTwist = btQuaternion(twistAxis, twistAngle);
1041                         }
1042                 }
1043
1044                 m_qTarget = qTargetCone * qTargetTwist;
1045         }
1046 }
1047
1048 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
1049 ///If no axis is provided, it uses the default axis for this constraint.
1050 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1051 {
1052         switch(num)
1053         {
1054                 case BT_CONSTRAINT_ERP :
1055                 case BT_CONSTRAINT_STOP_ERP :
1056                         if((axis >= 0) && (axis < 3)) 
1057                         {
1058                                 m_linERP = value;
1059                                 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1060                         }
1061                         else
1062                         {
1063                                 m_biasFactor = value;
1064                         }
1065                         break;
1066                 case BT_CONSTRAINT_CFM :
1067                 case BT_CONSTRAINT_STOP_CFM :
1068                         if((axis >= 0) && (axis < 3)) 
1069                         {
1070                                 m_linCFM = value;
1071                                 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1072                         }
1073                         else
1074                         {
1075                                 m_angCFM = value;
1076                                 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1077                         }
1078                         break;
1079                 default:
1080                         btAssertConstrParams(0);
1081                         break;
1082         }
1083 }
1084
1085 ///return the local value of parameter
1086 btScalar btConeTwistConstraint::getParam(int num, int axis) const 
1087 {
1088         btScalar retVal = 0;
1089         switch(num)
1090         {
1091                 case BT_CONSTRAINT_ERP :
1092                 case BT_CONSTRAINT_STOP_ERP :
1093                         if((axis >= 0) && (axis < 3)) 
1094                         {
1095                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1096                                 retVal = m_linERP;
1097                         }
1098                         else if((axis >= 3) && (axis < 6)) 
1099                         {
1100                                 retVal = m_biasFactor;
1101                         }
1102                         else
1103                         {
1104                                 btAssertConstrParams(0);
1105                         }
1106                         break;
1107                 case BT_CONSTRAINT_CFM :
1108                 case BT_CONSTRAINT_STOP_CFM :
1109                         if((axis >= 0) && (axis < 3)) 
1110                         {
1111                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1112                                 retVal = m_linCFM;
1113                         }
1114                         else if((axis >= 3) && (axis < 6)) 
1115                         {
1116                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1117                                 retVal = m_angCFM;
1118                         }
1119                         else
1120                         {
1121                                 btAssertConstrParams(0);
1122                         }
1123                         break;
1124                 default : 
1125                         btAssertConstrParams(0);
1126         }
1127         return retVal;
1128 }
1129
1130
1131 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1132 {
1133         m_rbAFrame = frameA;
1134         m_rbBFrame = frameB;
1135         buildJacobian();
1136         //calculateTransforms();
1137 }
1138
1139  
1140
1141