src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

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