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