dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.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 2007-09-09
  17 Refactored by Francisco Le?n
  18 email: projectileman@yahoo.com
  19 http://gimpact.sf.net
  20 */
  21
  22 #include "btGeneric6DofConstraint.h"
  23 #include "BulletDynamics/Dynamics/btRigidBody.h"
  24 #include "LinearMath/btTransformUtil.h"
  25 #include "LinearMath/btTransformUtil.h"
  26 #include <new>
  27
  28 #define D6_USE_OBSOLETE_METHOD false
  29 #define D6_USE_FRAME_OFFSET true
  30
  31 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
  32         : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
  33 {
  34         calculateTransforms();
  35 }
  36
  37 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
  38         : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
  39           m_frameInB(frameInB),
  40           m_useLinearReferenceFrameA(useLinearReferenceFrameB),
  41           m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
  42           m_flags(0),
  43           m_useSolveConstraintObsolete(false)
  44 {
  45         ///not providing rigidbody A means implicitly using worldspace for body A
  46         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
  47         calculateTransforms();
  48 }
  49
  50 #define GENERIC_D6_DISABLE_WARMSTARTING 1
  51
  52 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
  53 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
  54 {
  55         int i = index % 3;
  56         int j = index / 3;
  57         return mat[i][j];
  58 }
  59
  60 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
  61 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
  62 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
  63 {
  64         //      // rot =  cy*cz          -cy*sz           sy
  65         //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
  66         //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
  67         //
  68
  69         btScalar fi = btGetMatrixElem(mat, 2);
  70         if (fi < btScalar(1.0f))
  71         {
  72                 if (fi > btScalar(-1.0f))
  73                 {
  74                         xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
  75                         xyz[1] = btAsin(btGetMatrixElem(mat, 2));
  76                         xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
  77                         return true;
  78                 }
  79                 else
  80                 {
  81                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
  82                         xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  83                         xyz[1] = -SIMD_HALF_PI;
  84                         xyz[2] = btScalar(0.0);
  85                         return false;
  86                 }
  87         }
  88         else
  89         {
  90                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
  91                 xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  92                 xyz[1] = SIMD_HALF_PI;
  93                 xyz[2] = 0.0;
  94         }
  95         return false;
  96 }
  97
  98 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
  99
 100 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 101 {
 102         if (m_loLimit > m_hiLimit)
 103         {
 104                 m_currentLimit = 0;  //Free from violation
 105                 return 0;
 106         }
 107         if (test_value < m_loLimit)
 108         {
 109                 m_currentLimit = 1;  //low limit violation
 110                 m_currentLimitError = test_value - m_loLimit;
 111                 if (m_currentLimitError > SIMD_PI)
 112                         m_currentLimitError -= SIMD_2_PI;
 113                 else if (m_currentLimitError < -SIMD_PI)
 114                         m_currentLimitError += SIMD_2_PI;
 115                 return 1;
 116         }
 117         else if (test_value > m_hiLimit)
 118         {
 119                 m_currentLimit = 2;  //High limit violation
 120                 m_currentLimitError = test_value - m_hiLimit;
 121                 if (m_currentLimitError > SIMD_PI)
 122                         m_currentLimitError -= SIMD_2_PI;
 123                 else if (m_currentLimitError < -SIMD_PI)
 124                         m_currentLimitError += SIMD_2_PI;
 125                 return 2;
 126         };
 127
 128         m_currentLimit = 0;  //Free from violation
 129         return 0;
 130 }
 131
 132 btScalar btRotationalLimitMotor::solveAngularLimits(
 133         btScalar timeStep, btVector3& axis, btScalar jacDiagABInv,
 134         btRigidBody* body0, btRigidBody* body1)
 135 {
 136         if (needApplyTorques() == false) return 0.0f;
 137
 138         btScalar target_velocity = m_targetVelocity;
 139         btScalar maxMotorForce = m_maxMotorForce;
 140
 141         //current error correction
 142         if (m_currentLimit != 0)
 143         {
 144                 target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
 145                 maxMotorForce = m_maxLimitForce;
 146         }
 147
 148         maxMotorForce *= timeStep;
 149
 150         // current velocity difference
 151
 152         btVector3 angVelA = body0->getAngularVelocity();
 153         btVector3 angVelB = body1->getAngularVelocity();
 154
 155         btVector3 vel_diff;
 156         vel_diff = angVelA - angVelB;
 157
 158         btScalar rel_vel = axis.dot(vel_diff);
 159
 160         // correction velocity
 161         btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
 162
 163         if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON)
 164         {
 165                 return 0.0f;  //no need for applying force
 166         }
 167
 168         // correction impulse
 169         btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
 170
 171         // clip correction impulse
 172         btScalar clippedMotorImpulse;
 173
 174         ///@todo: should clip against accumulated impulse
 175         if (unclippedMotorImpulse > 0.0f)
 176         {
 177                 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
 178         }
 179         else
 180         {
 181                 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
 182         }
 183
 184         // sort with accumulated impulses
 185         btScalar lo = btScalar(-BT_LARGE_FLOAT);
 186         btScalar hi = btScalar(BT_LARGE_FLOAT);
 187
 188         btScalar oldaccumImpulse = m_accumulatedImpulse;
 189         btScalar sum = oldaccumImpulse + clippedMotorImpulse;
 190         m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
 191
 192         clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
 193
 194         btVector3 motorImp = clippedMotorImpulse * axis;
 195
 196         body0->applyTorqueImpulse(motorImp);
 197         body1->applyTorqueImpulse(-motorImp);
 198
 199         return clippedMotorImpulse;
 200 }
 201
 202 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 203
 204 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 205
 206 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
 207 {
 208         btScalar loLimit = m_lowerLimit[limitIndex];
 209         btScalar hiLimit = m_upperLimit[limitIndex];
 210         if (loLimit > hiLimit)
 211         {
 212                 m_currentLimit[limitIndex] = 0;  //Free from violation
 213                 m_currentLimitError[limitIndex] = btScalar(0.f);
 214                 return 0;
 215         }
 216
 217         if (test_value < loLimit)
 218         {
 219                 m_currentLimit[limitIndex] = 2;  //low limit violation
 220                 m_currentLimitError[limitIndex] = test_value - loLimit;
 221                 return 2;
 222         }
 223         else if (test_value > hiLimit)
 224         {
 225                 m_currentLimit[limitIndex] = 1;  //High limit violation
 226                 m_currentLimitError[limitIndex] = test_value - hiLimit;
 227                 return 1;
 228         };
 229
 230         m_currentLimit[limitIndex] = 0;  //Free from violation
 231         m_currentLimitError[limitIndex] = btScalar(0.f);
 232         return 0;
 233 }
 234
 235 btScalar btTranslationalLimitMotor::solveLinearAxis(
 236         btScalar timeStep,
 237         btScalar jacDiagABInv,
 238         btRigidBody& body1, const btVector3& pointInA,
 239         btRigidBody& body2, const btVector3& pointInB,
 240         int limit_index,
 241         const btVector3& axis_normal_on_a,
 242         const btVector3& anchorPos)
 243 {
 244         ///find relative velocity
 245         //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
 246         //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
 247         btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
 248         btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
 249
 250         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 251         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 252         btVector3 vel = vel1 - vel2;
 253
 254         btScalar rel_vel = axis_normal_on_a.dot(vel);
 255
 256         /// apply displacement correction
 257
 258         //positional error (zeroth order error)
 259         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
 260         btScalar lo = btScalar(-BT_LARGE_FLOAT);
 261         btScalar hi = btScalar(BT_LARGE_FLOAT);
 262
 263         btScalar minLimit = m_lowerLimit[limit_index];
 264         btScalar maxLimit = m_upperLimit[limit_index];
 265
 266         //handle the limits
 267         if (minLimit < maxLimit)
 268         {
 269                 {
 270                         if (depth > maxLimit)
 271                         {
 272                                 depth -= maxLimit;
 273                                 lo = btScalar(0.);
 274                         }
 275                         else
 276                         {
 277                                 if (depth < minLimit)
 278                                 {
 279                                         depth -= minLimit;
 280                                         hi = btScalar(0.);
 281                                 }
 282                                 else
 283                                 {
 284                                         return 0.0f;
 285                                 }
 286                         }
 287                 }
 288         }
 289
 290         btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
 291
 292         btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
 293         btScalar sum = oldNormalImpulse + normalImpulse;
 294         m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
 295         normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 296
 297         btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
 298         body1.applyImpulse(impulse_vector, rel_pos1);
 299         body2.applyImpulse(-impulse_vector, rel_pos2);
 300
 301         return normalImpulse;
 302 }
 303
 304 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 305
 306 void btGeneric6DofConstraint::calculateAngleInfo()
 307 {
 308         btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
 309         matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 310         // in euler angle mode we do not actually constrain the angular velocity
 311         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
 312         //
 313         //    to get                    constrain w2-w1 along           ...not
 314         //    ------                    ---------------------           ------
 315         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
 316         //    d(angle[1])/dt = 0        ax[1]
 317         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
 318         //
 319         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
 320         // to prove the result for angle[0], write the expression for angle[0] from
 321         // GetInfo1 then take the derivative. to prove this for angle[2] it is
 322         // easier to take the euler rate expression for d(angle[2])/dt with respect
 323         // to the components of w and set that to 0.
 324         btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 325         btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
 326
 327         m_calculatedAxis[1] = axis2.cross(axis0);
 328         m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 329         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 330
 331         m_calculatedAxis[0].normalize();
 332         m_calculatedAxis[1].normalize();
 333         m_calculatedAxis[2].normalize();
 334 }
 335
 336 void btGeneric6DofConstraint::calculateTransforms()
 337 {
 338         calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 339 }
 340
 341 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 342 {
 343         m_calculatedTransformA = transA * m_frameInA;
 344         m_calculatedTransformB = transB * m_frameInB;
 345         calculateLinearInfo();
 346         calculateAngleInfo();
 347         if (m_useOffsetForConstraintFrame)
 348         {  //  get weight factors depending on masses
 349                 btScalar miA = getRigidBodyA().getInvMass();
 350                 btScalar miB = getRigidBodyB().getInvMass();
 351                 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 352                 btScalar miS = miA + miB;
 353                 if (miS > btScalar(0.f))
 354                 {
 355                         m_factA = miB / miS;
 356                 }
 357                 else
 358                 {
 359                         m_factA = btScalar(0.5f);
 360                 }
 361                 m_factB = btScalar(1.0f) - m_factA;
 362         }
 363 }
 364
 365 void btGeneric6DofConstraint::buildLinearJacobian(
 366         btJacobianEntry& jacLinear, const btVector3& normalWorld,
 367         const btVector3& pivotAInW, const btVector3& pivotBInW)
 368 {
 369         new (&jacLinear) btJacobianEntry(
 370                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 371                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 372                 pivotAInW - m_rbA.getCenterOfMassPosition(),
 373                 pivotBInW - m_rbB.getCenterOfMassPosition(),
 374                 normalWorld,
 375                 m_rbA.getInvInertiaDiagLocal(),
 376                 m_rbA.getInvMass(),
 377                 m_rbB.getInvInertiaDiagLocal(),
 378                 m_rbB.getInvMass());
 379 }
 380
 381 void btGeneric6DofConstraint::buildAngularJacobian(
 382         btJacobianEntry& jacAngular, const btVector3& jointAxisW)
 383 {
 384         new (&jacAngular) btJacobianEntry(jointAxisW,
 385                                                                           m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 386                                                                           m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 387                                                                           m_rbA.getInvInertiaDiagLocal(),
 388                                                                           m_rbB.getInvInertiaDiagLocal());
 389 }
 390
 391 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 392 {
 393         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
 394         angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
 395         m_angularLimits[axis_index].m_currentPosition = angle;
 396         //test limits
 397         m_angularLimits[axis_index].testLimitValue(angle);
 398         return m_angularLimits[axis_index].needApplyTorques();
 399 }
 400
 401 void btGeneric6DofConstraint::buildJacobian()
 402 {
 403 #ifndef __SPU__
 404         if (m_useSolveConstraintObsolete)
 405         {
 406                 // Clear accumulated impulses for the next simulation step
 407                 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 408                 int i;
 409                 for (i = 0; i < 3; i++)
 410                 {
 411                         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
 412                 }
 413                 //calculates transform
 414                 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 415
 416                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
 417                 //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
 418                 calcAnchorPos();
 419                 btVector3 pivotAInW = m_AnchorPos;
 420                 btVector3 pivotBInW = m_AnchorPos;
 421
 422                 // not used here
 423                 //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
 424                 //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 425
 426                 btVector3 normalWorld;
 427                 //linear part
 428                 for (i = 0; i < 3; i++)
 429                 {
 430                         if (m_linearLimits.isLimited(i))
 431                         {
 432                                 if (m_useLinearReferenceFrameA)
 433                                         normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 434                                 else
 435                                         normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
 436
 437                                 buildLinearJacobian(
 438                                         m_jacLinear[i], normalWorld,
 439                                         pivotAInW, pivotBInW);
 440                         }
 441                 }
 442
 443                 // angular part
 444                 for (i = 0; i < 3; i++)
 445                 {
 446                         //calculates error angle
 447                         if (testAngularLimitMotor(i))
 448                         {
 449                                 normalWorld = this->getAxis(i);
 450                                 // Create angular atom
 451                                 buildAngularJacobian(m_jacAng[i], normalWorld);
 452                         }
 453                 }
 454         }
 455 #endif  //__SPU__
 456 }
 457
 458 void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info)
 459 {
 460         if (m_useSolveConstraintObsolete)
 461         {
 462                 info->m_numConstraintRows = 0;
 463                 info->nub = 0;
 464         }
 465         else
 466         {
 467                 //prepare constraint
 468                 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 469                 info->m_numConstraintRows = 0;
 470                 info->nub = 6;
 471                 int i;
 472                 //test linear limits
 473                 for (i = 0; i < 3; i++)
 474                 {
 475                         if (m_linearLimits.needApplyForce(i))
 476                         {
 477                                 info->m_numConstraintRows++;
 478                                 info->nub--;
 479                         }
 480                 }
 481                 //test angular limits
 482                 for (i = 0; i < 3; i++)
 483                 {
 484                         if (testAngularLimitMotor(i))
 485                         {
 486                                 info->m_numConstraintRows++;
 487                                 info->nub--;
 488                         }
 489                 }
 490         }
 491 }
 492
 493 void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 494 {
 495         if (m_useSolveConstraintObsolete)
 496         {
 497                 info->m_numConstraintRows = 0;
 498                 info->nub = 0;
 499         }
 500         else
 501         {
 502                 //pre-allocate all 6
 503                 info->m_numConstraintRows = 6;
 504                 info->nub = 0;
 505         }
 506 }
 507
 508 void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info)
 509 {
 510         btAssert(!m_useSolveConstraintObsolete);
 511
 512         const btTransform& transA = m_rbA.getCenterOfMassTransform();
 513         const btTransform& transB = m_rbB.getCenterOfMassTransform();
 514         const btVector3& linVelA = m_rbA.getLinearVelocity();
 515         const btVector3& linVelB = m_rbB.getLinearVelocity();
 516         const btVector3& angVelA = m_rbA.getAngularVelocity();
 517         const btVector3& angVelB = m_rbB.getAngularVelocity();
 518
 519         if (m_useOffsetForConstraintFrame)
 520         {  // for stability better to solve angular limits first
 521                 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 522                 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 523         }
 524         else
 525         {  // leave old version for compatibility
 526                 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 527                 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 528         }
 529 }
 530
 531 void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 532 {
 533         btAssert(!m_useSolveConstraintObsolete);
 534         //prepare constraint
 535         calculateTransforms(transA, transB);
 536
 537         int i;
 538         for (i = 0; i < 3; i++)
 539         {
 540                 testAngularLimitMotor(i);
 541         }
 542
 543         if (m_useOffsetForConstraintFrame)
 544         {  // for stability better to solve angular limits first
 545                 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 546                 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 547         }
 548         else
 549         {  // leave old version for compatibility
 550                 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 551                 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 552         }
 553 }
 554
 555 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 556 {
 557         //      int row = 0;
 558         //solve linear limits
 559         btRotationalLimitMotor limot;
 560         for (int i = 0; i < 3; i++)
 561         {
 562                 if (m_linearLimits.needApplyForce(i))
 563                 {  // re-use rotational motor code
 564                         limot.m_bounce = btScalar(0.f);
 565                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 566                         limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
 567                         limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
 568                         limot.m_damping = m_linearLimits.m_damping;
 569                         limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
 570                         limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
 571                         limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
 572                         limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
 573                         limot.m_maxLimitForce = btScalar(0.f);
 574                         limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
 575                         limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 576                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 577                         int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
 578                         limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
 579                         limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
 580                         limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
 581                         if (m_useOffsetForConstraintFrame)
 582                         {
 583                                 int indx1 = (i + 1) % 3;
 584                                 int indx2 = (i + 2) % 3;
 585                                 int rotAllowed = 1;  // rotations around orthos to current axis
 586                                 if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 587                                 {
 588                                         rotAllowed = 0;
 589                                 }
 590                                 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 591                         }
 592                         else
 593                         {
 594                                 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 595                         }
 596                 }
 597         }
 598         return row;
 599 }
 600
 601 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 602 {
 603         btGeneric6DofConstraint* d6constraint = this;
 604         int row = row_offset;
 605         //solve angular limits
 606         for (int i = 0; i < 3; i++)
 607         {
 608                 if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 609                 {
 610                         btVector3 axis = d6constraint->getAxis(i);
 611                         int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
 612                         if (!(flags & BT_6DOF_FLAGS_CFM_NORM))
 613                         {
 614                                 m_angularLimits[i].m_normalCFM = info->cfm[0];
 615                         }
 616                         if (!(flags & BT_6DOF_FLAGS_CFM_STOP))
 617                         {
 618                                 m_angularLimits[i].m_stopCFM = info->cfm[0];
 619                         }
 620                         if (!(flags & BT_6DOF_FLAGS_ERP_STOP))
 621                         {
 622                                 m_angularLimits[i].m_stopERP = info->erp;
 623                         }
 624                         row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
 625                                                                                  transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 626                 }
 627         }
 628
 629         return row;
 630 }
 631
 632 void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
 633 {
 634         (void)timeStep;
 635 }
 636
 637 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 638 {
 639         m_frameInA = frameA;
 640         m_frameInB = frameB;
 641         buildJacobian();
 642         calculateTransforms();
 643 }
 644
 645 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 646 {
 647         return m_calculatedAxis[axis_index];
 648 }
 649
 650 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
 651 {
 652         return m_calculatedLinearDiff[axisIndex];
 653 }
 654
 655 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
 656 {
 657         return m_calculatedAxisAngleDiff[axisIndex];
 658 }
 659
 660 void btGeneric6DofConstraint::calcAnchorPos(void)
 661 {
 662         btScalar imA = m_rbA.getInvMass();
 663         btScalar imB = m_rbB.getInvMass();
 664         btScalar weight;
 665         if (imB == btScalar(0.0))
 666         {
 667                 weight = btScalar(1.0);
 668         }
 669         else
 670         {
 671                 weight = imA / (imA + imB);
 672         }
 673         const btVector3& pA = m_calculatedTransformA.getOrigin();
 674         const btVector3& pB = m_calculatedTransformB.getOrigin();
 675         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
 676         return;
 677 }
 678
 679 void btGeneric6DofConstraint::calculateLinearInfo()
 680 {
 681         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 682         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
 683         for (int i = 0; i < 3; i++)
 684         {
 685                 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 686                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 687         }
 688 }
 689
 690 int btGeneric6DofConstraint::get_limit_motor_info2(
 691         btRotationalLimitMotor* limot,
 692         const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
 693         btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
 694 {
 695         int srow = row * info->rowskip;
 696         bool powered = limot->m_enableMotor;
 697         int limit = limot->m_currentLimit;
 698         if (powered || limit)
 699         {  // if the joint is powered, or has joint limits, add in the extra row
 700                 btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
 701                 btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 702                 J1[srow + 0] = ax1[0];
 703                 J1[srow + 1] = ax1[1];
 704                 J1[srow + 2] = ax1[2];
 705
 706                 J2[srow + 0] = -ax1[0];
 707                 J2[srow + 1] = -ax1[1];
 708                 J2[srow + 2] = -ax1[2];
 709
 710                 if ((!rotational))
 711                 {
 712                         if (m_useOffsetForConstraintFrame)
 713                         {
 714                                 btVector3 tmpA, tmpB, relA, relB;
 715                                 // get vector from bodyB to frameB in WCS
 716                                 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 717                                 // get its projection to constraint axis
 718                                 btVector3 projB = ax1 * relB.dot(ax1);
 719                                 // get vector directed from bodyB to constraint axis (and orthogonal to it)
 720                                 btVector3 orthoB = relB - projB;
 721                                 // same for bodyA
 722                                 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
 723                                 btVector3 projA = ax1 * relA.dot(ax1);
 724                                 btVector3 orthoA = relA - projA;
 725                                 // get desired offset between frames A and B along constraint axis
 726                                 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
 727                                 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
 728                                 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
 729                                 // get offset vectors relA and relB
 730                                 relA = orthoA + totalDist * m_factA;
 731                                 relB = orthoB - totalDist * m_factB;
 732                                 tmpA = relA.cross(ax1);
 733                                 tmpB = relB.cross(ax1);
 734                                 if (m_hasStaticBody && (!rotAllowed))
 735                                 {
 736                                         tmpA *= m_factA;
 737                                         tmpB *= m_factB;
 738                                 }
 739                                 int i;
 740                                 for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
 741                                 for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
 742                         }
 743                         else
 744                         {
 745                                 btVector3 ltd;  // Linear Torque Decoupling vector
 746                                 btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 747                                 ltd = c.cross(ax1);
 748                                 info->m_J1angularAxis[srow + 0] = ltd[0];
 749                                 info->m_J1angularAxis[srow + 1] = ltd[1];
 750                                 info->m_J1angularAxis[srow + 2] = ltd[2];
 751
 752                                 c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 753                                 ltd = -c.cross(ax1);
 754                                 info->m_J2angularAxis[srow + 0] = ltd[0];
 755                                 info->m_J2angularAxis[srow + 1] = ltd[1];
 756                                 info->m_J2angularAxis[srow + 2] = ltd[2];
 757                         }
 758                 }
 759                 // if we're limited low and high simultaneously, the joint motor is
 760                 // ineffective
 761                 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
 762                 info->m_constraintError[srow] = btScalar(0.f);
 763                 if (powered)
 764                 {
 765                         info->cfm[srow] = limot->m_normalCFM;
 766                         if (!limit)
 767                         {
 768                                 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 769
 770                                 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
 771                                                                                                    limot->m_loLimit,
 772                                                                                                    limot->m_hiLimit,
 773                                                                                                    tag_vel,
 774                                                                                                    info->fps * limot->m_stopERP);
 775                                 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
 776                                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
 777                                 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 778                         }
 779                 }
 780                 if (limit)
 781                 {
 782                         btScalar k = info->fps * limot->m_stopERP;
 783                         if (!rotational)
 784                         {
 785                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
 786                         }
 787                         else
 788                         {
 789                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 790                         }
 791                         info->cfm[srow] = limot->m_stopCFM;
 792                         if (limot->m_loLimit == limot->m_hiLimit)
 793                         {  // limited low and high simultaneously
 794                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 795                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 796                         }
 797                         else
 798                         {
 799                                 if (limit == 1)
 800                                 {
 801                                         info->m_lowerLimit[srow] = 0;
 802                                         info->m_upperLimit[srow] = SIMD_INFINITY;
 803                                 }
 804                                 else
 805                                 {
 806                                         info->m_lowerLimit[srow] = -SIMD_INFINITY;
 807                                         info->m_upperLimit[srow] = 0;
 808                                 }
 809                                 // deal with bounce
 810                                 if (limot->m_bounce > 0)
 811                                 {
 812                                         // calculate joint velocity
 813                                         btScalar vel;
 814                                         if (rotational)
 815                                         {
 816                                                 vel = angVelA.dot(ax1);
 817                                                 //make sure that if no body -> angVelB == zero vec
 818                                                 //                        if (body1)
 819                                                 vel -= angVelB.dot(ax1);
 820                                         }
 821                                         else
 822                                         {
 823                                                 vel = linVelA.dot(ax1);
 824                                                 //make sure that if no body -> angVelB == zero vec
 825                                                 //                        if (body1)
 826                                                 vel -= linVelB.dot(ax1);
 827                                         }
 828                                         // only apply bounce if the velocity is incoming, and if the
 829                                         // resulting c[] exceeds what we already have.
 830                                         if (limit == 1)
 831                                         {
 832                                                 if (vel < 0)
 833                                                 {
 834                                                         btScalar newc = -limot->m_bounce * vel;
 835                                                         if (newc > info->m_constraintError[srow])
 836                                                                 info->m_constraintError[srow] = newc;
 837                                                 }
 838                                         }
 839                                         else
 840                                         {
 841                                                 if (vel > 0)
 842                                                 {
 843                                                         btScalar newc = -limot->m_bounce * vel;
 844                                                         if (newc < info->m_constraintError[srow])
 845                                                                 info->m_constraintError[srow] = newc;
 846                                                 }
 847                                         }
 848                                 }
 849                         }
 850                 }
 851                 return 1;
 852         }
 853         else
 854                 return 0;
 855 }
 856
 857 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 858 ///If no axis is provided, it uses the default axis for this constraint.
 859 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
 860 {
 861         if ((axis >= 0) && (axis < 3))
 862         {
 863                 switch (num)
 864                 {
 865                         case BT_CONSTRAINT_STOP_ERP:
 866                                 m_linearLimits.m_stopERP[axis] = value;
 867                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 868                                 break;
 869                         case BT_CONSTRAINT_STOP_CFM:
 870                                 m_linearLimits.m_stopCFM[axis] = value;
 871                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 872                                 break;
 873                         case BT_CONSTRAINT_CFM:
 874                                 m_linearLimits.m_normalCFM[axis] = value;
 875                                 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 876                                 break;
 877                         default:
 878                                 btAssertConstrParams(0);
 879                 }
 880         }
 881         else if ((axis >= 3) && (axis < 6))
 882         {
 883                 switch (num)
 884                 {
 885                         case BT_CONSTRAINT_STOP_ERP:
 886                                 m_angularLimits[axis - 3].m_stopERP = value;
 887                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 888                                 break;
 889                         case BT_CONSTRAINT_STOP_CFM:
 890                                 m_angularLimits[axis - 3].m_stopCFM = value;
 891                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 892                                 break;
 893                         case BT_CONSTRAINT_CFM:
 894                                 m_angularLimits[axis - 3].m_normalCFM = value;
 895                                 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 896                                 break;
 897                         default:
 898                                 btAssertConstrParams(0);
 899                 }
 900         }
 901         else
 902         {
 903                 btAssertConstrParams(0);
 904         }
 905 }
 906
 907 ///return the local value of parameter
 908 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 909 {
 910         btScalar retVal = 0;
 911         if ((axis >= 0) && (axis < 3))
 912         {
 913                 switch (num)
 914                 {
 915                         case BT_CONSTRAINT_STOP_ERP:
 916                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 917                                 retVal = m_linearLimits.m_stopERP[axis];
 918                                 break;
 919                         case BT_CONSTRAINT_STOP_CFM:
 920                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 921                                 retVal = m_linearLimits.m_stopCFM[axis];
 922                                 break;
 923                         case BT_CONSTRAINT_CFM:
 924                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 925                                 retVal = m_linearLimits.m_normalCFM[axis];
 926                                 break;
 927                         default:
 928                                 btAssertConstrParams(0);
 929                 }
 930         }
 931         else if ((axis >= 3) && (axis < 6))
 932         {
 933                 switch (num)
 934                 {
 935                         case BT_CONSTRAINT_STOP_ERP:
 936                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 937                                 retVal = m_angularLimits[axis - 3].m_stopERP;
 938                                 break;
 939                         case BT_CONSTRAINT_STOP_CFM:
 940                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 941                                 retVal = m_angularLimits[axis - 3].m_stopCFM;
 942                                 break;
 943                         case BT_CONSTRAINT_CFM:
 944                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 945                                 retVal = m_angularLimits[axis - 3].m_normalCFM;
 946                                 break;
 947                         default:
 948                                 btAssertConstrParams(0);
 949                 }
 950         }
 951         else
 952         {
 953                 btAssertConstrParams(0);
 954         }
 955         return retVal;
 956 }
 957
 958 void btGeneric6DofConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 959 {
 960         btVector3 zAxis = axis1.normalized();
 961         btVector3 yAxis = axis2.normalized();
 962         btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
 963
 964         btTransform frameInW;
 965         frameInW.setIdentity();
 966         frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
 967                                                                  xAxis[1], yAxis[1], zAxis[1],
 968                                                                  xAxis[2], yAxis[2], zAxis[2]);
 969
 970         // now get constraint frame in local coordinate systems
 971         m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 972         m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
 973
 974         calculateTransforms();
 975 }