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