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