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