migrated NDOF code from soc-2010-merwin, SpaceNavigator now works on Mac blender
[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                 m_swingAxis = swingAxis;
642                 m_swingAxis.normalize();
643                 m_swingCorrection = qDeltaAB.getAngle();
644                 if(!btFuzzyZero(m_swingCorrection))
645                 {
646                         m_solveSwingLimit = true;
647                 }
648                 return;
649         }
650
651
652         {
653                 // compute rotation of A wrt B (in constraint space)
654                 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
655                 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
656                 btQuaternion qAB = qB.inverse() * qA;
657                 // split rotation into cone and twist
658                 // (all this is done from B's perspective. Maybe I should be averaging axes...)
659                 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
660                 btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
661                 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
662
663                 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
664                 {
665                         btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
666                         computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
667
668                         if (swingAngle > swingLimit * m_limitSoftness)
669                         {
670                                 m_solveSwingLimit = true;
671
672                                 // compute limit ratio: 0->1, where
673                                 // 0 == beginning of soft limit
674                                 // 1 == hard/real limit
675                                 m_swingLimitRatio = 1.f;
676                                 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
677                                 {
678                                         m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
679                                                                                 (swingLimit - swingLimit * m_limitSoftness);
680                                 }                               
681
682                                 // swing correction tries to get back to soft limit
683                                 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
684
685                                 // adjustment of swing axis (based on ellipse normal)
686                                 adjustSwingAxisToUseEllipseNormal(swingAxis);
687
688                                 // Calculate necessary axis & factors           
689                                 m_swingAxis = quatRotate(qB, -swingAxis);
690
691                                 m_twistAxisA.setValue(0,0,0);
692
693                                 m_kSwing =  btScalar(1.) /
694                                         (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
695                                          computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
696                         }
697                 }
698                 else
699                 {
700                         // you haven't set any limits;
701                         // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
702                         // anyway, we have either hinge or fixed joint
703                         btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
704                         btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
705                         btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
706                         btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
707                         btVector3 target;
708                         btScalar x = ivB.dot(ivA);
709                         btScalar y = ivB.dot(jvA);
710                         btScalar z = ivB.dot(kvA);
711                         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
712                         { // fixed. We'll need to add one more row to constraint
713                                 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
714                                 {
715                                         m_solveSwingLimit = true;
716                                         m_swingAxis = -ivB.cross(ivA);
717                                 }
718                         }
719                         else
720                         {
721                                 if(m_swingSpan1 < m_fixThresh)
722                                 { // hinge around Y axis
723                                         if(!(btFuzzyZero(y)))
724                                         {
725                                                 m_solveSwingLimit = true;
726                                                 if(m_swingSpan2 >= m_fixThresh)
727                                                 {
728                                                         y = btScalar(0.f);
729                                                         btScalar span2 = btAtan2(z, x);
730                                                         if(span2 > m_swingSpan2)
731                                                         {
732                                                                 x = btCos(m_swingSpan2);
733                                                                 z = btSin(m_swingSpan2);
734                                                         }
735                                                         else if(span2 < -m_swingSpan2)
736                                                         {
737                                                                 x =  btCos(m_swingSpan2);
738                                                                 z = -btSin(m_swingSpan2);
739                                                         }
740                                                 }
741                                         }
742                                 }
743                                 else
744                                 { // hinge around Z axis
745                                         if(!btFuzzyZero(z))
746                                         {
747                                                 m_solveSwingLimit = true;
748                                                 if(m_swingSpan1 >= m_fixThresh)
749                                                 {
750                                                         z = btScalar(0.f);
751                                                         btScalar span1 = btAtan2(y, x);
752                                                         if(span1 > m_swingSpan1)
753                                                         {
754                                                                 x = btCos(m_swingSpan1);
755                                                                 y = btSin(m_swingSpan1);
756                                                         }
757                                                         else if(span1 < -m_swingSpan1)
758                                                         {
759                                                                 x =  btCos(m_swingSpan1);
760                                                                 y = -btSin(m_swingSpan1);
761                                                         }
762                                                 }
763                                         }
764                                 }
765                                 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
766                                 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
767                                 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
768                                 target.normalize();
769                                 m_swingAxis = -ivB.cross(target);
770                                 m_swingCorrection = m_swingAxis.length();
771                                 m_swingAxis.normalize();
772                         }
773                 }
774
775                 if (m_twistSpan >= btScalar(0.f))
776                 {
777                         btVector3 twistAxis;
778                         computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
779
780                         if (m_twistAngle > m_twistSpan*m_limitSoftness)
781                         {
782                                 m_solveTwistLimit = true;
783
784                                 m_twistLimitRatio = 1.f;
785                                 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
786                                 {
787                                         m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
788                                                                                 (m_twistSpan  - m_twistSpan * m_limitSoftness);
789                                 }
790
791                                 // twist correction tries to get back to soft limit
792                                 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
793
794                                 m_twistAxis = quatRotate(qB, -twistAxis);
795
796                                 m_kTwist = btScalar(1.) /
797                                         (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
798                                          computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
799                         }
800
801                         if (m_solveSwingLimit)
802                                 m_twistAxisA = quatRotate(qA, -twistAxis);
803                 }
804                 else
805                 {
806                         m_twistAngle = btScalar(0.f);
807                 }
808         }
809 }
810
811
812
813 // given a cone rotation in constraint space, (pre: twist must already be removed)
814 // this method computes its corresponding swing angle and axis.
815 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
816 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
817                                                                                                  btScalar& swingAngle, // out
818                                                                                                  btVector3& vSwingAxis, // out
819                                                                                                  btScalar& swingLimit) // out
820 {
821         swingAngle = qCone.getAngle();
822         if (swingAngle > SIMD_EPSILON)
823         {
824                 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
825                 vSwingAxis.normalize();
826                 if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
827                 {
828                         // non-zero twist?! this should never happen.
829                         int wtf = 0; wtf = wtf;
830                 }
831
832                 // Compute limit for given swing. tricky:
833                 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
834                 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
835
836                 // For starters, compute the direction from center to surface of ellipse.
837                 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
838                 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
839                 btScalar xEllipse =  vSwingAxis.y();
840                 btScalar yEllipse = -vSwingAxis.z();
841
842                 // Now, we use the slope of the vector (using x/yEllipse) and find the length
843                 // of the line that intersects the ellipse:
844                 //  x^2   y^2
845                 //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
846                 //  a^2   b^2
847                 // Do the math and it should be clear.
848
849                 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
850                 if (fabs(xEllipse) > SIMD_EPSILON)
851                 {
852                         btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
853                         btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
854                         norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
855                         btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
856                         swingLimit = sqrt(swingLimit2);
857                 }
858
859                 // test!
860                 /*swingLimit = m_swingSpan2;
861                 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
862                 {
863                 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
864                 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
865                 btScalar phi = asin(sinphi);
866                 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
867                 btScalar alpha = 3.14159f - theta - phi;
868                 btScalar sinalpha = sin(alpha);
869                 swingLimit = m_swingSpan1 * sinphi/sinalpha;
870                 }*/
871         }
872         else if (swingAngle < 0)
873         {
874                 // this should never happen!
875                 int wtf = 0; wtf = wtf;
876         }
877 }
878
879 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
880 {
881         // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
882         btScalar xEllipse = btCos(fAngleInRadians);
883         btScalar yEllipse = btSin(fAngleInRadians);
884
885         // Use the slope of the vector (using x/yEllipse) and find the length
886         // of the line that intersects the ellipse:
887         //  x^2   y^2
888         //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
889         //  a^2   b^2
890         // Do the math and it should be clear.
891
892         float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
893         if (fabs(xEllipse) > SIMD_EPSILON)
894         {
895                 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
896                 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
897                 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
898                 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
899                 swingLimit = sqrt(swingLimit2);
900         }
901
902         // convert into point in constraint space:
903         // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
904         btVector3 vSwingAxis(0, xEllipse, -yEllipse);
905         btQuaternion qSwing(vSwingAxis, swingLimit);
906         btVector3 vPointInConstraintSpace(fLength,0,0);
907         return quatRotate(qSwing, vPointInConstraintSpace);
908 }
909
910 // given a twist rotation in constraint space, (pre: cone must already be removed)
911 // this method computes its corresponding angle and axis.
912 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
913                                                                                                   btScalar& twistAngle, // out
914                                                                                                   btVector3& vTwistAxis) // out
915 {
916         btQuaternion qMinTwist = qTwist;
917         twistAngle = qTwist.getAngle();
918
919         if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
920         {
921                 qMinTwist = operator-(qTwist);
922                 twistAngle = qMinTwist.getAngle();
923         }
924         if (twistAngle < 0)
925         {
926                 // this should never happen
927                 int wtf = 0; wtf = wtf;                 
928         }
929
930         vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
931         if (twistAngle > SIMD_EPSILON)
932                 vTwistAxis.normalize();
933 }
934
935
936 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
937 {
938         // the swing axis is computed as the "twist-free" cone rotation,
939         // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
940         // so, if we're outside the limits, the closest way back inside the cone isn't 
941         // along the vector back to the center. better (and more stable) to use the ellipse normal.
942
943         // convert swing axis to direction from center to surface of ellipse
944         // (ie. rotate 2D vector by PI/2)
945         btScalar y = -vSwingAxis.z();
946         btScalar z =  vSwingAxis.y();
947
948         // do the math...
949         if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
950         {
951                 // compute gradient/normal of ellipse surface at current "point"
952                 btScalar grad = y/z;
953                 grad *= m_swingSpan2 / m_swingSpan1;
954
955                 // adjust y/z to represent normal at point (instead of vector to point)
956                 if (y > 0)
957                         y =  fabs(grad * z);
958                 else
959                         y = -fabs(grad * z);
960
961                 // convert ellipse direction back to swing axis
962                 vSwingAxis.setZ(-y);
963                 vSwingAxis.setY( z);
964                 vSwingAxis.normalize();
965         }
966 }
967
968
969
970 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
971 {
972         btTransform trACur = m_rbA.getCenterOfMassTransform();
973         btTransform trBCur = m_rbB.getCenterOfMassTransform();
974         btTransform trABCur = trBCur.inverse() * trACur;
975         btQuaternion qABCur = trABCur.getRotation();
976         btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
977         btQuaternion qConstraintCur = trConstraintCur.getRotation();
978
979         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
980         setMotorTargetInConstraintSpace(qConstraint);
981 }
982
983
984 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
985 {
986         m_qTarget = q;
987
988         // clamp motor target to within limits
989         {
990                 btScalar softness = 1.f;//m_limitSoftness;
991
992                 // split into twist and cone
993                 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
994                 btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
995                 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
996
997                 // clamp cone
998                 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
999                 {
1000                         btScalar swingAngle, swingLimit; btVector3 swingAxis;
1001                         computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1002
1003                         if (fabs(swingAngle) > SIMD_EPSILON)
1004                         {
1005                                 if (swingAngle > swingLimit*softness)
1006                                         swingAngle = swingLimit*softness;
1007                                 else if (swingAngle < -swingLimit*softness)
1008                                         swingAngle = -swingLimit*softness;
1009                                 qTargetCone = btQuaternion(swingAxis, swingAngle);
1010                         }
1011                 }
1012
1013                 // clamp twist
1014                 if (m_twistSpan >= btScalar(0.05f))
1015                 {
1016                         btScalar twistAngle; btVector3 twistAxis;
1017                         computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1018
1019                         if (fabs(twistAngle) > SIMD_EPSILON)
1020                         {
1021                                 // eddy todo: limitSoftness used here???
1022                                 if (twistAngle > m_twistSpan*softness)
1023                                         twistAngle = m_twistSpan*softness;
1024                                 else if (twistAngle < -m_twistSpan*softness)
1025                                         twistAngle = -m_twistSpan*softness;
1026                                 qTargetTwist = btQuaternion(twistAxis, twistAngle);
1027                         }
1028                 }
1029
1030                 m_qTarget = qTargetCone * qTargetTwist;
1031         }
1032 }
1033
1034 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
1035 ///If no axis is provided, it uses the default axis for this constraint.
1036 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1037 {
1038         switch(num)
1039         {
1040                 case BT_CONSTRAINT_ERP :
1041                 case BT_CONSTRAINT_STOP_ERP :
1042                         if((axis >= 0) && (axis < 3)) 
1043                         {
1044                                 m_linERP = value;
1045                                 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1046                         }
1047                         else
1048                         {
1049                                 m_biasFactor = value;
1050                         }
1051                         break;
1052                 case BT_CONSTRAINT_CFM :
1053                 case BT_CONSTRAINT_STOP_CFM :
1054                         if((axis >= 0) && (axis < 3)) 
1055                         {
1056                                 m_linCFM = value;
1057                                 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1058                         }
1059                         else
1060                         {
1061                                 m_angCFM = value;
1062                                 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1063                         }
1064                         break;
1065                 default:
1066                         btAssertConstrParams(0);
1067                         break;
1068         }
1069 }
1070
1071 ///return the local value of parameter
1072 btScalar btConeTwistConstraint::getParam(int num, int axis) const 
1073 {
1074         btScalar retVal = 0;
1075         switch(num)
1076         {
1077                 case BT_CONSTRAINT_ERP :
1078                 case BT_CONSTRAINT_STOP_ERP :
1079                         if((axis >= 0) && (axis < 3)) 
1080                         {
1081                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1082                                 retVal = m_linERP;
1083                         }
1084                         else if((axis >= 3) && (axis < 6)) 
1085                         {
1086                                 retVal = m_biasFactor;
1087                         }
1088                         else
1089                         {
1090                                 btAssertConstrParams(0);
1091                         }
1092                         break;
1093                 case BT_CONSTRAINT_CFM :
1094                 case BT_CONSTRAINT_STOP_CFM :
1095                         if((axis >= 0) && (axis < 3)) 
1096                         {
1097                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1098                                 retVal = m_linCFM;
1099                         }
1100                         else if((axis >= 3) && (axis < 6)) 
1101                         {
1102                                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1103                                 retVal = m_angCFM;
1104                         }
1105                         else
1106                         {
1107                                 btAssertConstrParams(0);
1108                         }
1109                         break;
1110                 default : 
1111                         btAssertConstrParams(0);
1112         }
1113         return retVal;
1114 }
1115
1116
1117 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1118 {
1119         m_rbAFrame = frameA;
1120         m_rbBFrame = frameB;
1121         buildJacobian();
1122         //calculateTransforms();
1123 }
1124
1125  
1126
1127