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