dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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
  16 #include "btHingeConstraint.h"
  17 #include "BulletDynamics/Dynamics/btRigidBody.h"
  18 #include "LinearMath/btTransformUtil.h"
  19 #include "LinearMath/btMinMax.h"
  20 #include <new>
  21 #include "btSolverBody.h"
  22
  23 //#define HINGE_USE_OBSOLETE_SOLVER false
  24 #define HINGE_USE_OBSOLETE_SOLVER false
  25
  26 #define HINGE_USE_FRAME_OFFSET true
  27
  28 #ifndef __SPU__
  29
  30 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
  31                                                                          const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
  32         : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
  33 #ifdef _BT_USE_CENTER_LIMIT_
  34           m_limit(),
  35 #endif
  36           m_angularOnly(false),
  37           m_enableAngularMotor(false),
  38           m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
  39           m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
  40           m_useReferenceFrameA(useReferenceFrameA),
  41           m_flags(0),
  42           m_normalCFM(0),
  43           m_normalERP(0),
  44           m_stopCFM(0),
  45           m_stopERP(0)
  46 {
  47         m_rbAFrame.getOrigin() = pivotInA;
  48
  49         // since no frame is given, assume this to be zero angle and just pick rb transform axis
  50         btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
  51
  52         btVector3 rbAxisA2;
  53         btScalar projection = axisInA.dot(rbAxisA1);
  54         if (projection >= 1.0f - SIMD_EPSILON)
  55         {
  56                 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
  57                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
  58         }
  59         else if (projection <= -1.0f + SIMD_EPSILON)
  60         {
  61                 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
  62                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
  63         }
  64         else
  65         {
  66                 rbAxisA2 = axisInA.cross(rbAxisA1);
  67                 rbAxisA1 = rbAxisA2.cross(axisInA);
  68         }
  69
  70         m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
  71                                                                    rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
  72                                                                    rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
  73
  74         btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
  75         btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
  76         btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
  77
  78         m_rbBFrame.getOrigin() = pivotInB;
  79         m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
  80                                                                    rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
  81                                                                    rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
  82
  83 #ifndef _BT_USE_CENTER_LIMIT_
  84         //start with free
  85         m_lowerLimit = btScalar(1.0f);
  86         m_upperLimit = btScalar(-1.0f);
  87         m_biasFactor = 0.3f;
  88         m_relaxationFactor = 1.0f;
  89         m_limitSoftness = 0.9f;
  90         m_solveLimit = false;
  91 #endif
  92         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
  93 }
  94
  95 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
  96         : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
  97 #ifdef _BT_USE_CENTER_LIMIT_
  98           m_limit(),
  99 #endif
 100           m_angularOnly(false),
 101           m_enableAngularMotor(false),
 102           m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 103           m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 104           m_useReferenceFrameA(useReferenceFrameA),
 105           m_flags(0),
 106           m_normalCFM(0),
 107           m_normalERP(0),
 108           m_stopCFM(0),
 109           m_stopERP(0)
 110 {
 111         // since no frame is given, assume this to be zero angle and just pick rb transform axis
 112         // fixed axis in worldspace
 113         btVector3 rbAxisA1, rbAxisA2;
 114         btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 115
 116         m_rbAFrame.getOrigin() = pivotInA;
 117         m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
 118                                                                    rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
 119                                                                    rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 120
 121         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 122
 123         btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
 124         btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 125         btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 126
 127         m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
 128         m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
 129                                                                    rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
 130                                                                    rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
 131
 132 #ifndef _BT_USE_CENTER_LIMIT_
 133         //start with free
 134         m_lowerLimit = btScalar(1.0f);
 135         m_upperLimit = btScalar(-1.0f);
 136         m_biasFactor = 0.3f;
 137         m_relaxationFactor = 1.0f;
 138         m_limitSoftness = 0.9f;
 139         m_solveLimit = false;
 140 #endif
 141         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 142 }
 143
 144 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
 145                                                                          const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
 146         : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
 147 #ifdef _BT_USE_CENTER_LIMIT_
 148           m_limit(),
 149 #endif
 150           m_angularOnly(false),
 151           m_enableAngularMotor(false),
 152           m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 153           m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 154           m_useReferenceFrameA(useReferenceFrameA),
 155           m_flags(0),
 156           m_normalCFM(0),
 157           m_normalERP(0),
 158           m_stopCFM(0),
 159           m_stopERP(0)
 160 {
 161 #ifndef _BT_USE_CENTER_LIMIT_
 162         //start with free
 163         m_lowerLimit = btScalar(1.0f);
 164         m_upperLimit = btScalar(-1.0f);
 165         m_biasFactor = 0.3f;
 166         m_relaxationFactor = 1.0f;
 167         m_limitSoftness = 0.9f;
 168         m_solveLimit = false;
 169 #endif
 170         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 171 }
 172
 173 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 174         : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
 175 #ifdef _BT_USE_CENTER_LIMIT_
 176           m_limit(),
 177 #endif
 178           m_angularOnly(false),
 179           m_enableAngularMotor(false),
 180           m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 181           m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 182           m_useReferenceFrameA(useReferenceFrameA),
 183           m_flags(0),
 184           m_normalCFM(0),
 185           m_normalERP(0),
 186           m_stopCFM(0),
 187           m_stopERP(0)
 188 {
 189         ///not providing rigidbody B means implicitly using worldspace for body B
 190
 191         m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
 192 #ifndef _BT_USE_CENTER_LIMIT_
 193         //start with free
 194         m_lowerLimit = btScalar(1.0f);
 195         m_upperLimit = btScalar(-1.0f);
 196         m_biasFactor = 0.3f;
 197         m_relaxationFactor = 1.0f;
 198         m_limitSoftness = 0.9f;
 199         m_solveLimit = false;
 200 #endif
 201         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 202 }
 203
 204 void btHingeConstraint::buildJacobian()
 205 {
 206         if (m_useSolveConstraintObsolete)
 207         {
 208                 m_appliedImpulse = btScalar(0.);
 209                 m_accMotorImpulse = btScalar(0.);
 210
 211                 if (!m_angularOnly)
 212                 {
 213                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
 214                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 215                         btVector3 relPos = pivotBInW - pivotAInW;
 216
 217                         btVector3 normal[3];
 218                         if (relPos.length2() > SIMD_EPSILON)
 219                         {
 220                                 normal[0] = relPos.normalized();
 221                         }
 222                         else
 223                         {
 224                                 normal[0].setValue(btScalar(1.0), 0, 0);
 225                         }
 226
 227                         btPlaneSpace1(normal[0], normal[1], normal[2]);
 228
 229                         for (int i = 0; i < 3; i++)
 230                         {
 231                                 new (&m_jac[i]) btJacobianEntry(
 232                                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 233                                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 234                                         pivotAInW - m_rbA.getCenterOfMassPosition(),
 235                                         pivotBInW - m_rbB.getCenterOfMassPosition(),
 236                                         normal[i],
 237                                         m_rbA.getInvInertiaDiagLocal(),
 238                                         m_rbA.getInvMass(),
 239                                         m_rbB.getInvInertiaDiagLocal(),
 240                                         m_rbB.getInvMass());
 241                         }
 242                 }
 243
 244                 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
 245                 //these two jointAxis require equal angular velocities for both bodies
 246
 247                 //this is unused for now, it's a todo
 248                 btVector3 jointAxis0local;
 249                 btVector3 jointAxis1local;
 250
 251                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
 252
 253                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 254                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
 255                 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 256
 257                 new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
 258                                                                                    m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 259                                                                                    m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 260                                                                                    m_rbA.getInvInertiaDiagLocal(),
 261                                                                                    m_rbB.getInvInertiaDiagLocal());
 262
 263                 new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
 264                                                                                    m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 265                                                                                    m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 266                                                                                    m_rbA.getInvInertiaDiagLocal(),
 267                                                                                    m_rbB.getInvInertiaDiagLocal());
 268
 269                 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
 270                                                                                    m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 271                                                                                    m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 272                                                                                    m_rbA.getInvInertiaDiagLocal(),
 273                                                                                    m_rbB.getInvInertiaDiagLocal());
 274
 275                 // clear accumulator
 276                 m_accLimitImpulse = btScalar(0.);
 277
 278                 // test angular limit
 279                 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 280
 281                 //Compute K = J*W*J' for hinge axis
 282                 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 283                 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
 284                                                    getRigidBodyB().computeAngularImpulseDenominator(axisA));
 285         }
 286 }
 287
 288 #endif  //__SPU__
 289
 290 static inline btScalar btNormalizeAnglePositive(btScalar angle)
 291 {
 292         return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
 293 }
 294
 295 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
 296 {
 297         btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
 298                                                                                                                                 btNormalizeAnglePositive(accAngle)));
 299         return result;
 300 }
 301
 302 static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
 303 {
 304         btScalar tol(0.3);
 305         btScalar result = btShortestAngularDistance(accAngle, curAngle);
 306
 307         if (btFabs(result) > tol)
 308                 return curAngle;
 309         else
 310                 return accAngle + result;
 311
 312         return curAngle;
 313 }
 314
 315 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
 316 {
 317         btScalar hingeAngle = getHingeAngle();
 318         m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
 319         return m_accumulatedAngle;
 320 }
 321 void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
 322 {
 323         m_accumulatedAngle = accAngle;
 324 }
 325
 326 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
 327 {
 328         //update m_accumulatedAngle
 329         btScalar curHingeAngle = getHingeAngle();
 330         m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
 331
 332         btHingeConstraint::getInfo1(info);
 333 }
 334
 335 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 336 {
 337         if (m_useSolveConstraintObsolete)
 338         {
 339                 info->m_numConstraintRows = 0;
 340                 info->nub = 0;
 341         }
 342         else
 343         {
 344                 info->m_numConstraintRows = 5;  // Fixed 3 linear + 2 angular
 345                 info->nub = 1;
 346                 //always add the row, to avoid computation (data is not available yet)
 347                 //prepare constraint
 348                 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 349                 if (getSolveLimit() || getEnableAngularMotor())
 350                 {
 351                         info->m_numConstraintRows++;  // limit 3rd anguar as well
 352                         info->nub--;
 353                 }
 354         }
 355 }
 356
 357 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 358 {
 359         if (m_useSolveConstraintObsolete)
 360         {
 361                 info->m_numConstraintRows = 0;
 362                 info->nub = 0;
 363         }
 364         else
 365         {
 366                 //always add the 'limit' row, to avoid computation (data is not available yet)
 367                 info->m_numConstraintRows = 6;  // Fixed 3 linear + 2 angular
 368                 info->nub = 0;
 369         }
 370 }
 371
 372 void btHingeConstraint::getInfo2(btConstraintInfo2* info)
 373 {
 374         if (m_useOffsetForConstraintFrame)
 375         {
 376                 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 377         }
 378         else
 379         {
 380                 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 381         }
 382 }
 383
 384 void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 385 {
 386         ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
 387         testLimit(transA, transB);
 388
 389         getInfo2Internal(info, transA, transB, angVelA, angVelB);
 390 }
 391
 392 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 393 {
 394         btAssert(!m_useSolveConstraintObsolete);
 395         int i, skip = info->rowskip;
 396         // transforms in world space
 397         btTransform trA = transA * m_rbAFrame;
 398         btTransform trB = transB * m_rbBFrame;
 399         // pivot point
 400         btVector3 pivotAInW = trA.getOrigin();
 401         btVector3 pivotBInW = trB.getOrigin();
 402 #if 0
 403         if (0)
 404         {
 405                 for (i=0;i<6;i++)
 406                 {
 407                         info->m_J1linearAxis[i*skip]=0;
 408                         info->m_J1linearAxis[i*skip+1]=0;
 409                         info->m_J1linearAxis[i*skip+2]=0;
 410
 411                         info->m_J1angularAxis[i*skip]=0;
 412                         info->m_J1angularAxis[i*skip+1]=0;
 413                         info->m_J1angularAxis[i*skip+2]=0;
 414
 415                         info->m_J2linearAxis[i*skip]=0;
 416                         info->m_J2linearAxis[i*skip+1]=0;
 417                         info->m_J2linearAxis[i*skip+2]=0;
 418
 419                         info->m_J2angularAxis[i*skip]=0;
 420                         info->m_J2angularAxis[i*skip+1]=0;
 421                         info->m_J2angularAxis[i*skip+2]=0;
 422
 423                         info->m_constraintError[i*skip]=0.f;
 424                 }
 425         }
 426 #endif  //#if 0
 427         // linear (all fixed)
 428
 429         if (!m_angularOnly)
 430         {
 431                 info->m_J1linearAxis[0] = 1;
 432                 info->m_J1linearAxis[skip + 1] = 1;
 433                 info->m_J1linearAxis[2 * skip + 2] = 1;
 434
 435                 info->m_J2linearAxis[0] = -1;
 436                 info->m_J2linearAxis[skip + 1] = -1;
 437                 info->m_J2linearAxis[2 * skip + 2] = -1;
 438         }
 439
 440         btVector3 a1 = pivotAInW - transA.getOrigin();
 441         {
 442                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
 443                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
 444                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
 445                 btVector3 a1neg = -a1;
 446                 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 447         }
 448         btVector3 a2 = pivotBInW - transB.getOrigin();
 449         {
 450                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 451                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
 452                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
 453                 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 454         }
 455         // linear RHS
 456         btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
 457
 458         btScalar k = info->fps * normalErp;
 459         if (!m_angularOnly)
 460         {
 461                 for (i = 0; i < 3; i++)
 462                 {
 463                         info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
 464                 }
 465         }
 466         // make rotations around X and Y equal
 467         // the hinge axis should be the only unconstrained
 468         // rotational axis, the angular velocity of the two bodies perpendicular to
 469         // the hinge axis should be equal. thus the constraint equations are
 470         //    p*w1 - p*w2 = 0
 471         //    q*w1 - q*w2 = 0
 472         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
 473         // are the angular velocity vectors of the two bodies.
 474         // get hinge axis (Z)
 475         btVector3 ax1 = trA.getBasis().getColumn(2);
 476         // get 2 orthos to hinge axis (X, Y)
 477         btVector3 p = trA.getBasis().getColumn(0);
 478         btVector3 q = trA.getBasis().getColumn(1);
 479         // set the two hinge angular rows
 480         int s3 = 3 * info->rowskip;
 481         int s4 = 4 * info->rowskip;
 482
 483         info->m_J1angularAxis[s3 + 0] = p[0];
 484         info->m_J1angularAxis[s3 + 1] = p[1];
 485         info->m_J1angularAxis[s3 + 2] = p[2];
 486         info->m_J1angularAxis[s4 + 0] = q[0];
 487         info->m_J1angularAxis[s4 + 1] = q[1];
 488         info->m_J1angularAxis[s4 + 2] = q[2];
 489
 490         info->m_J2angularAxis[s3 + 0] = -p[0];
 491         info->m_J2angularAxis[s3 + 1] = -p[1];
 492         info->m_J2angularAxis[s3 + 2] = -p[2];
 493         info->m_J2angularAxis[s4 + 0] = -q[0];
 494         info->m_J2angularAxis[s4 + 1] = -q[1];
 495         info->m_J2angularAxis[s4 + 2] = -q[2];
 496         // compute the right hand side of the constraint equation. set relative
 497         // body velocities along p and q to bring the hinge back into alignment.
 498         // if ax1,ax2 are the unit length hinge axes as computed from body1 and
 499         // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
 500         // if `theta' is the angle between ax1 and ax2, we need an angular velocity
 501         // along u to cover angle erp*theta in one step :
 502         //   |angular_velocity| = angle/time = erp*theta / stepsize
 503         //                      = (erp*fps) * theta
 504         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
 505         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
 506         // ...as ax1 and ax2 are unit length. if theta is smallish,
 507         // theta ~= sin(theta), so
 508         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
 509         // ax1 x ax2 is in the plane space of ax1, so we project the angular
 510         // velocity to p and q to find the right hand side.
 511         btVector3 ax2 = trB.getBasis().getColumn(2);
 512         btVector3 u = ax1.cross(ax2);
 513         info->m_constraintError[s3] = k * u.dot(p);
 514         info->m_constraintError[s4] = k * u.dot(q);
 515         // check angular limits
 516         int nrow = 4;  // last filled row
 517         int srow;
 518         btScalar limit_err = btScalar(0.0);
 519         int limit = 0;
 520         if (getSolveLimit())
 521         {
 522 #ifdef _BT_USE_CENTER_LIMIT_
 523                 limit_err = m_limit.getCorrection() * m_referenceSign;
 524 #else
 525                 limit_err = m_correction * m_referenceSign;
 526 #endif
 527                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 528         }
 529         // if the hinge has joint limits or motor, add in the extra row
 530         bool powered = getEnableAngularMotor();
 531         if (limit || powered)
 532         {
 533                 nrow++;
 534                 srow = nrow * info->rowskip;
 535                 info->m_J1angularAxis[srow + 0] = ax1[0];
 536                 info->m_J1angularAxis[srow + 1] = ax1[1];
 537                 info->m_J1angularAxis[srow + 2] = ax1[2];
 538
 539                 info->m_J2angularAxis[srow + 0] = -ax1[0];
 540                 info->m_J2angularAxis[srow + 1] = -ax1[1];
 541                 info->m_J2angularAxis[srow + 2] = -ax1[2];
 542
 543                 btScalar lostop = getLowerLimit();
 544                 btScalar histop = getUpperLimit();
 545                 if (limit && (lostop == histop))
 546                 {  // the joint motor is ineffective
 547                         powered = false;
 548                 }
 549                 info->m_constraintError[srow] = btScalar(0.0f);
 550                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 551                 if (powered)
 552                 {
 553                         if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 554                         {
 555                                 info->cfm[srow] = m_normalCFM;
 556                         }
 557                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 558                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
 559                         info->m_lowerLimit[srow] = -m_maxMotorImpulse;
 560                         info->m_upperLimit[srow] = m_maxMotorImpulse;
 561                 }
 562                 if (limit)
 563                 {
 564                         k = info->fps * currERP;
 565                         info->m_constraintError[srow] += k * limit_err;
 566                         if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 567                         {
 568                                 info->cfm[srow] = m_stopCFM;
 569                         }
 570                         if (lostop == histop)
 571                         {
 572                                 // limited low and high simultaneously
 573                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 574                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 575                         }
 576                         else if (limit == 1)
 577                         {  // low limit
 578                                 info->m_lowerLimit[srow] = 0;
 579                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 580                         }
 581                         else
 582                         {  // high limit
 583                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 584                                 info->m_upperLimit[srow] = 0;
 585                         }
 586                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
 587 #ifdef _BT_USE_CENTER_LIMIT_
 588                         btScalar bounce = m_limit.getRelaxationFactor();
 589 #else
 590                         btScalar bounce = m_relaxationFactor;
 591 #endif
 592                         if (bounce > btScalar(0.0))
 593                         {
 594                                 btScalar vel = angVelA.dot(ax1);
 595                                 vel -= angVelB.dot(ax1);
 596                                 // only apply bounce if the velocity is incoming, and if the
 597                                 // resulting c[] exceeds what we already have.
 598                                 if (limit == 1)
 599                                 {  // low limit
 600                                         if (vel < 0)
 601                                         {
 602                                                 btScalar newc = -bounce * vel;
 603                                                 if (newc > info->m_constraintError[srow])
 604                                                 {
 605                                                         info->m_constraintError[srow] = newc;
 606                                                 }
 607                                         }
 608                                 }
 609                                 else
 610                                 {  // high limit - all those computations are reversed
 611                                         if (vel > 0)
 612                                         {
 613                                                 btScalar newc = -bounce * vel;
 614                                                 if (newc < info->m_constraintError[srow])
 615                                                 {
 616                                                         info->m_constraintError[srow] = newc;
 617                                                 }
 618                                         }
 619                                 }
 620                         }
 621 #ifdef _BT_USE_CENTER_LIMIT_
 622                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
 623 #else
 624                         info->m_constraintError[srow] *= m_biasFactor;
 625 #endif
 626                 }  // if(limit)
 627         }      // if angular limit or powered
 628 }
 629
 630 void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 631 {
 632         m_rbAFrame = frameA;
 633         m_rbBFrame = frameB;
 634         buildJacobian();
 635 }
 636
 637 void btHingeConstraint::updateRHS(btScalar timeStep)
 638 {
 639         (void)timeStep;
 640 }
 641
 642 btScalar btHingeConstraint::getHingeAngle()
 643 {
 644         return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 645 }
 646
 647 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
 648 {
 649         const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
 650         const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
 651         const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
 652         //      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 653         btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 654         return m_referenceSign * angle;
 655 }
 656
 657 void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
 658 {
 659         // Compute limit information
 660         m_hingeAngle = getHingeAngle(transA, transB);
 661 #ifdef _BT_USE_CENTER_LIMIT_
 662         m_limit.test(m_hingeAngle);
 663 #else
 664         m_correction = btScalar(0.);
 665         m_limitSign = btScalar(0.);
 666         m_solveLimit = false;
 667         if (m_lowerLimit <= m_upperLimit)
 668         {
 669                 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
 670                 if (m_hingeAngle <= m_lowerLimit)
 671                 {
 672                         m_correction = (m_lowerLimit - m_hingeAngle);
 673                         m_limitSign = 1.0f;
 674                         m_solveLimit = true;
 675                 }
 676                 else if (m_hingeAngle >= m_upperLimit)
 677                 {
 678                         m_correction = m_upperLimit - m_hingeAngle;
 679                         m_limitSign = -1.0f;
 680                         m_solveLimit = true;
 681                 }
 682         }
 683 #endif
 684         return;
 685 }
 686
 687 static btVector3 vHinge(0, 0, btScalar(1));
 688
 689 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 690 {
 691         // convert target from body to constraint space
 692         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
 693         qConstraint.normalize();
 694
 695         // extract "pure" hinge component
 696         btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
 697         vNoHinge.normalize();
 698         btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
 699         btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
 700         qHinge.normalize();
 701
 702         // compute angular target, clamped to limits
 703         btScalar targetAngle = qHinge.getAngle();
 704         if (targetAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 705         {
 706                 qHinge = -(qHinge);
 707                 targetAngle = qHinge.getAngle();
 708         }
 709         if (qHinge.getZ() < 0)
 710                 targetAngle = -targetAngle;
 711
 712         setMotorTarget(targetAngle, dt);
 713 }
 714
 715 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 716 {
 717 #ifdef _BT_USE_CENTER_LIMIT_
 718         m_limit.fit(targetAngle);
 719 #else
 720         if (m_lowerLimit < m_upperLimit)
 721         {
 722                 if (targetAngle < m_lowerLimit)
 723                         targetAngle = m_lowerLimit;
 724                 else if (targetAngle > m_upperLimit)
 725                         targetAngle = m_upperLimit;
 726         }
 727 #endif
 728         // compute angular velocity
 729         btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 730         btScalar dAngle = targetAngle - curAngle;
 731         m_motorTargetVelocity = dAngle / dt;
 732 }
 733
 734 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 735 {
 736         btAssert(!m_useSolveConstraintObsolete);
 737         int i, s = info->rowskip;
 738         // transforms in world space
 739         btTransform trA = transA * m_rbAFrame;
 740         btTransform trB = transB * m_rbBFrame;
 741         // pivot point
 742 //      btVector3 pivotAInW = trA.getOrigin();
 743 //      btVector3 pivotBInW = trB.getOrigin();
 744 #if 1
 745         // difference between frames in WCS
 746         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
 747         // now get weight factors depending on masses
 748         btScalar miA = getRigidBodyA().getInvMass();
 749         btScalar miB = getRigidBodyB().getInvMass();
 750         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 751         btScalar miS = miA + miB;
 752         btScalar factA, factB;
 753         if (miS > btScalar(0.f))
 754         {
 755                 factA = miB / miS;
 756         }
 757         else
 758         {
 759                 factA = btScalar(0.5f);
 760         }
 761         factB = btScalar(1.0f) - factA;
 762         // get the desired direction of hinge axis
 763         // as weighted sum of Z-orthos of frameA and frameB in WCS
 764         btVector3 ax1A = trA.getBasis().getColumn(2);
 765         btVector3 ax1B = trB.getBasis().getColumn(2);
 766         btVector3 ax1 = ax1A * factA + ax1B * factB;
 767         if (ax1.length2()<SIMD_EPSILON)
 768         {
 769                 factA=0.f;
 770                 factB=1.f;
 771                 ax1 = ax1A * factA + ax1B * factB;
 772         }
 773         ax1.normalize();
 774         // fill first 3 rows
 775         // we want: velA + wA x relA == velB + wB x relB
 776         btTransform bodyA_trans = transA;
 777         btTransform bodyB_trans = transB;
 778         int s0 = 0;
 779         int s1 = s;
 780         int s2 = s * 2;
 781         int nrow = 2;  // last filled row
 782         btVector3 tmpA, tmpB, relA, relB, p, q;
 783         // get vector from bodyB to frameB in WCS
 784         relB = trB.getOrigin() - bodyB_trans.getOrigin();
 785         // get its projection to hinge axis
 786         btVector3 projB = ax1 * relB.dot(ax1);
 787         // get vector directed from bodyB to hinge axis (and orthogonal to it)
 788         btVector3 orthoB = relB - projB;
 789         // same for bodyA
 790         relA = trA.getOrigin() - bodyA_trans.getOrigin();
 791         btVector3 projA = ax1 * relA.dot(ax1);
 792         btVector3 orthoA = relA - projA;
 793         btVector3 totalDist = projA - projB;
 794         // get offset vectors relA and relB
 795         relA = orthoA + totalDist * factA;
 796         relB = orthoB - totalDist * factB;
 797         // now choose average ortho to hinge axis
 798         p = orthoB * factA + orthoA * factB;
 799         btScalar len2 = p.length2();
 800         if (len2 > SIMD_EPSILON)
 801         {
 802                 p /= btSqrt(len2);
 803         }
 804         else
 805         {
 806                 p = trA.getBasis().getColumn(1);
 807         }
 808         // make one more ortho
 809         q = ax1.cross(p);
 810         // fill three rows
 811         tmpA = relA.cross(p);
 812         tmpB = relB.cross(p);
 813         for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i];
 814         for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i];
 815         tmpA = relA.cross(q);
 816         tmpB = relB.cross(q);
 817         if (hasStaticBody && getSolveLimit())
 818         {  // to make constraint between static and dynamic objects more rigid
 819                 // remove wA (or wB) from equation if angular limit is hit
 820                 tmpB *= factB;
 821                 tmpA *= factA;
 822         }
 823         for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i];
 824         for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i];
 825         tmpA = relA.cross(ax1);
 826         tmpB = relB.cross(ax1);
 827         if (hasStaticBody)
 828         {  // to make constraint between static and dynamic objects more rigid
 829                 // remove wA (or wB) from equation
 830                 tmpB *= factB;
 831                 tmpA *= factA;
 832         }
 833         for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i];
 834         for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i];
 835
 836         btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
 837         btScalar k = info->fps * normalErp;
 838
 839         if (!m_angularOnly)
 840         {
 841                 for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i];
 842                 for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i];
 843                 for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i];
 844
 845                 for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i];
 846                 for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i];
 847                 for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i];
 848
 849                 // compute three elements of right hand side
 850
 851                 btScalar rhs = k * p.dot(ofs);
 852                 info->m_constraintError[s0] = rhs;
 853                 rhs = k * q.dot(ofs);
 854                 info->m_constraintError[s1] = rhs;
 855                 rhs = k * ax1.dot(ofs);
 856                 info->m_constraintError[s2] = rhs;
 857         }
 858         // the hinge axis should be the only unconstrained
 859         // rotational axis, the angular velocity of the two bodies perpendicular to
 860         // the hinge axis should be equal. thus the constraint equations are
 861         //    p*w1 - p*w2 = 0
 862         //    q*w1 - q*w2 = 0
 863         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
 864         // are the angular velocity vectors of the two bodies.
 865         int s3 = 3 * s;
 866         int s4 = 4 * s;
 867         info->m_J1angularAxis[s3 + 0] = p[0];
 868         info->m_J1angularAxis[s3 + 1] = p[1];
 869         info->m_J1angularAxis[s3 + 2] = p[2];
 870         info->m_J1angularAxis[s4 + 0] = q[0];
 871         info->m_J1angularAxis[s4 + 1] = q[1];
 872         info->m_J1angularAxis[s4 + 2] = q[2];
 873
 874         info->m_J2angularAxis[s3 + 0] = -p[0];
 875         info->m_J2angularAxis[s3 + 1] = -p[1];
 876         info->m_J2angularAxis[s3 + 2] = -p[2];
 877         info->m_J2angularAxis[s4 + 0] = -q[0];
 878         info->m_J2angularAxis[s4 + 1] = -q[1];
 879         info->m_J2angularAxis[s4 + 2] = -q[2];
 880         // compute the right hand side of the constraint equation. set relative
 881         // body velocities along p and q to bring the hinge back into alignment.
 882         // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
 883         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
 884         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
 885         // along u to cover angle erp*theta in one step :
 886         //   |angular_velocity| = angle/time = erp*theta / stepsize
 887         //                      = (erp*fps) * theta
 888         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
 889         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
 890         // ...as ax1 and ax2 are unit length. if theta is smallish,
 891         // theta ~= sin(theta), so
 892         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
 893         // ax1 x ax2 is in the plane space of ax1, so we project the angular
 894         // velocity to p and q to find the right hand side.
 895         k = info->fps * normalErp;  //??
 896
 897         btVector3 u = ax1A.cross(ax1B);
 898         info->m_constraintError[s3] = k * u.dot(p);
 899         info->m_constraintError[s4] = k * u.dot(q);
 900 #endif
 901         // check angular limits
 902         nrow = 4;  // last filled row
 903         int srow;
 904         btScalar limit_err = btScalar(0.0);
 905         int limit = 0;
 906         if (getSolveLimit())
 907         {
 908 #ifdef _BT_USE_CENTER_LIMIT_
 909                 limit_err = m_limit.getCorrection() * m_referenceSign;
 910 #else
 911                 limit_err = m_correction * m_referenceSign;
 912 #endif
 913                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 914         }
 915         // if the hinge has joint limits or motor, add in the extra row
 916         bool powered = getEnableAngularMotor();
 917         if (limit || powered)
 918         {
 919                 nrow++;
 920                 srow = nrow * info->rowskip;
 921                 info->m_J1angularAxis[srow + 0] = ax1[0];
 922                 info->m_J1angularAxis[srow + 1] = ax1[1];
 923                 info->m_J1angularAxis[srow + 2] = ax1[2];
 924
 925                 info->m_J2angularAxis[srow + 0] = -ax1[0];
 926                 info->m_J2angularAxis[srow + 1] = -ax1[1];
 927                 info->m_J2angularAxis[srow + 2] = -ax1[2];
 928
 929                 btScalar lostop = getLowerLimit();
 930                 btScalar histop = getUpperLimit();
 931                 if (limit && (lostop == histop))
 932                 {  // the joint motor is ineffective
 933                         powered = false;
 934                 }
 935                 info->m_constraintError[srow] = btScalar(0.0f);
 936                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 937                 if (powered)
 938                 {
 939                         if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 940                         {
 941                                 info->cfm[srow] = m_normalCFM;
 942                         }
 943                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 944                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
 945                         info->m_lowerLimit[srow] = -m_maxMotorImpulse;
 946                         info->m_upperLimit[srow] = m_maxMotorImpulse;
 947                 }
 948                 if (limit)
 949                 {
 950                         k = info->fps * currERP;
 951                         info->m_constraintError[srow] += k * limit_err;
 952                         if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 953                         {
 954                                 info->cfm[srow] = m_stopCFM;
 955                         }
 956                         if (lostop == histop)
 957                         {
 958                                 // limited low and high simultaneously
 959                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 960                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 961                         }
 962                         else if (limit == 1)
 963                         {  // low limit
 964                                 info->m_lowerLimit[srow] = 0;
 965                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 966                         }
 967                         else
 968                         {  // high limit
 969                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 970                                 info->m_upperLimit[srow] = 0;
 971                         }
 972                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
 973 #ifdef _BT_USE_CENTER_LIMIT_
 974                         btScalar bounce = m_limit.getRelaxationFactor();
 975 #else
 976                         btScalar bounce = m_relaxationFactor;
 977 #endif
 978                         if (bounce > btScalar(0.0))
 979                         {
 980                                 btScalar vel = angVelA.dot(ax1);
 981                                 vel -= angVelB.dot(ax1);
 982                                 // only apply bounce if the velocity is incoming, and if the
 983                                 // resulting c[] exceeds what we already have.
 984                                 if (limit == 1)
 985                                 {  // low limit
 986                                         if (vel < 0)
 987                                         {
 988                                                 btScalar newc = -bounce * vel;
 989                                                 if (newc > info->m_constraintError[srow])
 990                                                 {
 991                                                         info->m_constraintError[srow] = newc;
 992                                                 }
 993                                         }
 994                                 }
 995                                 else
 996                                 {  // high limit - all those computations are reversed
 997                                         if (vel > 0)
 998                                         {
 999                                                 btScalar newc = -bounce * vel;
1000                                                 if (newc < info->m_constraintError[srow])
1001                                                 {
1002                                                         info->m_constraintError[srow] = newc;
1003                                                 }
1004                                         }
1005                                 }
1006                         }
1007 #ifdef _BT_USE_CENTER_LIMIT_
1008                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
1009 #else
1010                         info->m_constraintError[srow] *= m_biasFactor;
1011 #endif
1012                 }  // if(limit)
1013         }      // if angular limit or powered
1014 }
1015
1016 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
1017 ///If no axis is provided, it uses the default axis for this constraint.
1018 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1019 {
1020         if ((axis == -1) || (axis == 5))
1021         {
1022                 switch (num)
1023                 {
1024                         case BT_CONSTRAINT_STOP_ERP:
1025                                 m_stopERP = value;
1026                                 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
1027                                 break;
1028                         case BT_CONSTRAINT_STOP_CFM:
1029                                 m_stopCFM = value;
1030                                 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1031                                 break;
1032                         case BT_CONSTRAINT_CFM:
1033                                 m_normalCFM = value;
1034                                 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1035                                 break;
1036                         case BT_CONSTRAINT_ERP:
1037                                 m_normalERP = value;
1038                                 m_flags |= BT_HINGE_FLAGS_ERP_NORM;
1039                                 break;
1040                         default:
1041                                 btAssertConstrParams(0);
1042                 }
1043         }
1044         else
1045         {
1046                 btAssertConstrParams(0);
1047         }
1048 }
1049
1050 ///return the local value of parameter
1051 btScalar btHingeConstraint::getParam(int num, int axis) const
1052 {
1053         btScalar retVal = 0;
1054         if ((axis == -1) || (axis == 5))
1055         {
1056                 switch (num)
1057                 {
1058                         case BT_CONSTRAINT_STOP_ERP:
1059                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1060                                 retVal = m_stopERP;
1061                                 break;
1062                         case BT_CONSTRAINT_STOP_CFM:
1063                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1064                                 retVal = m_stopCFM;
1065                                 break;
1066                         case BT_CONSTRAINT_CFM:
1067                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1068                                 retVal = m_normalCFM;
1069                                 break;
1070                         case BT_CONSTRAINT_ERP:
1071                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
1072                                 retVal = m_normalERP;
1073                                 break;
1074                         default:
1075                                 btAssertConstrParams(0);
1076                 }
1077         }
1078         else
1079         {
1080                 btAssertConstrParams(0);
1081         }
1082         return retVal;
1083 }