src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

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