dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.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 "b3Generic6DofConstraint.h"
  23 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
  24
  25 #include "Bullet3Common/b3TransformUtil.h"
  26 #include "Bullet3Common/b3TransformUtil.h"
  27 #include <new>
  28
  29 #define D6_USE_OBSOLETE_METHOD false
  30 #define D6_USE_FRAME_OFFSET true
  31
  32 b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
  33         : b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
  34 {
  35         calculateTransforms(bodies);
  36 }
  37
  38 #define GENERIC_D6_DISABLE_WARMSTARTING 1
  39
  40 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
  41 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
  42 {
  43         int i = index % 3;
  44         int j = index / 3;
  45         return mat[i][j];
  46 }
  47
  48 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
  49 bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
  50 bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
  51 {
  52         //      // rot =  cy*cz          -cy*sz           sy
  53         //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
  54         //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
  55         //
  56
  57         b3Scalar fi = btGetMatrixElem(mat, 2);
  58         if (fi < b3Scalar(1.0f))
  59         {
  60                 if (fi > b3Scalar(-1.0f))
  61                 {
  62                         xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
  63                         xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
  64                         xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
  65                         return true;
  66                 }
  67                 else
  68                 {
  69                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
  70                         xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  71                         xyz[1] = -B3_HALF_PI;
  72                         xyz[2] = b3Scalar(0.0);
  73                         return false;
  74                 }
  75         }
  76         else
  77         {
  78                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
  79                 xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  80                 xyz[1] = B3_HALF_PI;
  81                 xyz[2] = 0.0;
  82         }
  83         return false;
  84 }
  85
  86 //////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
  87
  88 int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
  89 {
  90         if (m_loLimit > m_hiLimit)
  91         {
  92                 m_currentLimit = 0;  //Free from violation
  93                 return 0;
  94         }
  95         if (test_value < m_loLimit)
  96         {
  97                 m_currentLimit = 1;  //low limit violation
  98                 m_currentLimitError = test_value - m_loLimit;
  99                 if (m_currentLimitError > B3_PI)
 100                         m_currentLimitError -= B3_2_PI;
 101                 else if (m_currentLimitError < -B3_PI)
 102                         m_currentLimitError += B3_2_PI;
 103                 return 1;
 104         }
 105         else if (test_value > m_hiLimit)
 106         {
 107                 m_currentLimit = 2;  //High limit violation
 108                 m_currentLimitError = test_value - m_hiLimit;
 109                 if (m_currentLimitError > B3_PI)
 110                         m_currentLimitError -= B3_2_PI;
 111                 else if (m_currentLimitError < -B3_PI)
 112                         m_currentLimitError += B3_2_PI;
 113                 return 2;
 114         };
 115
 116         m_currentLimit = 0;  //Free from violation
 117         return 0;
 118 }
 119
 120 //////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
 121
 122 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 123
 124 int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
 125 {
 126         b3Scalar loLimit = m_lowerLimit[limitIndex];
 127         b3Scalar hiLimit = m_upperLimit[limitIndex];
 128         if (loLimit > hiLimit)
 129         {
 130                 m_currentLimit[limitIndex] = 0;  //Free from violation
 131                 m_currentLimitError[limitIndex] = b3Scalar(0.f);
 132                 return 0;
 133         }
 134
 135         if (test_value < loLimit)
 136         {
 137                 m_currentLimit[limitIndex] = 2;  //low limit violation
 138                 m_currentLimitError[limitIndex] = test_value - loLimit;
 139                 return 2;
 140         }
 141         else if (test_value > hiLimit)
 142         {
 143                 m_currentLimit[limitIndex] = 1;  //High limit violation
 144                 m_currentLimitError[limitIndex] = test_value - hiLimit;
 145                 return 1;
 146         };
 147
 148         m_currentLimit[limitIndex] = 0;  //Free from violation
 149         m_currentLimitError[limitIndex] = b3Scalar(0.f);
 150         return 0;
 151 }
 152
 153 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 154
 155 void b3Generic6DofConstraint::calculateAngleInfo()
 156 {
 157         b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
 158         matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 159         // in euler angle mode we do not actually constrain the angular velocity
 160         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
 161         //
 162         //    to get                    constrain w2-w1 along           ...not
 163         //    ------                    ---------------------           ------
 164         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
 165         //    d(angle[1])/dt = 0        ax[1]
 166         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
 167         //
 168         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
 169         // to prove the result for angle[0], write the expression for angle[0] from
 170         // GetInfo1 then take the derivative. to prove this for angle[2] it is
 171         // easier to take the euler rate expression for d(angle[2])/dt with respect
 172         // to the components of w and set that to 0.
 173         b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 174         b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
 175
 176         m_calculatedAxis[1] = axis2.cross(axis0);
 177         m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 178         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 179
 180         m_calculatedAxis[0].normalize();
 181         m_calculatedAxis[1].normalize();
 182         m_calculatedAxis[2].normalize();
 183 }
 184
 185 static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
 186 {
 187         b3Transform tr(body.m_quat, body.m_pos);
 188         return tr;
 189 }
 190
 191 void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
 192 {
 193         b3Transform transA;
 194         b3Transform transB;
 195         transA = getCenterOfMassTransform(bodies[m_rbA]);
 196         transB = getCenterOfMassTransform(bodies[m_rbB]);
 197         calculateTransforms(transA, transB, bodies);
 198 }
 199
 200 void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
 201 {
 202         m_calculatedTransformA = transA * m_frameInA;
 203         m_calculatedTransformB = transB * m_frameInB;
 204         calculateLinearInfo();
 205         calculateAngleInfo();
 206         if (m_useOffsetForConstraintFrame)
 207         {  //  get weight factors depending on masses
 208                 b3Scalar miA = bodies[m_rbA].m_invMass;
 209                 b3Scalar miB = bodies[m_rbB].m_invMass;
 210                 m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
 211                 b3Scalar miS = miA + miB;
 212                 if (miS > b3Scalar(0.f))
 213                 {
 214                         m_factA = miB / miS;
 215                 }
 216                 else
 217                 {
 218                         m_factA = b3Scalar(0.5f);
 219                 }
 220                 m_factB = b3Scalar(1.0f) - m_factA;
 221         }
 222 }
 223
 224 bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 225 {
 226         b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
 227         angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
 228         m_angularLimits[axis_index].m_currentPosition = angle;
 229         //test limits
 230         m_angularLimits[axis_index].testLimitValue(angle);
 231         return m_angularLimits[axis_index].needApplyTorques();
 232 }
 233
 234 void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 235 {
 236         //prepare constraint
 237         calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
 238         info->m_numConstraintRows = 0;
 239         info->nub = 6;
 240         int i;
 241         //test linear limits
 242         for (i = 0; i < 3; i++)
 243         {
 244                 if (m_linearLimits.needApplyForce(i))
 245                 {
 246                         info->m_numConstraintRows++;
 247                         info->nub--;
 248                 }
 249         }
 250         //test angular limits
 251         for (i = 0; i < 3; i++)
 252         {
 253                 if (testAngularLimitMotor(i))
 254                 {
 255                         info->m_numConstraintRows++;
 256                         info->nub--;
 257                 }
 258         }
 259         //      printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
 260 }
 261
 262 void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 263 {
 264         //pre-allocate all 6
 265         info->m_numConstraintRows = 6;
 266         info->nub = 0;
 267 }
 268
 269 void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 270 {
 271         b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
 272         b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
 273         const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
 274         const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
 275         const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
 276         const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
 277
 278         if (m_useOffsetForConstraintFrame)
 279         {  // for stability better to solve angular limits first
 280                 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 281                 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 282         }
 283         else
 284         {  // leave old version for compatibility
 285                 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 286                 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 287         }
 288 }
 289
 290 void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
 291 {
 292         //prepare constraint
 293         calculateTransforms(transA, transB, bodies);
 294
 295         int i;
 296         for (i = 0; i < 3; i++)
 297         {
 298                 testAngularLimitMotor(i);
 299         }
 300
 301         if (m_useOffsetForConstraintFrame)
 302         {  // for stability better to solve angular limits first
 303                 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 304                 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 305         }
 306         else
 307         {  // leave old version for compatibility
 308                 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 309                 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 310         }
 311 }
 312
 313 int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 314 {
 315         //      int row = 0;
 316         //solve linear limits
 317         b3RotationalLimitMotor limot;
 318         for (int i = 0; i < 3; i++)
 319         {
 320                 if (m_linearLimits.needApplyForce(i))
 321                 {  // re-use rotational motor code
 322                         limot.m_bounce = b3Scalar(0.f);
 323                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 324                         limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
 325                         limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
 326                         limot.m_damping = m_linearLimits.m_damping;
 327                         limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
 328                         limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
 329                         limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
 330                         limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
 331                         limot.m_maxLimitForce = b3Scalar(0.f);
 332                         limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
 333                         limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 334                         b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 335                         int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
 336                         limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
 337                         limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
 338                         limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
 339                         if (m_useOffsetForConstraintFrame)
 340                         {
 341                                 int indx1 = (i + 1) % 3;
 342                                 int indx2 = (i + 2) % 3;
 343                                 int rotAllowed = 1;  // rotations around orthos to current axis
 344                                 if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 345                                 {
 346                                         rotAllowed = 0;
 347                                 }
 348                                 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 349                         }
 350                         else
 351                         {
 352                                 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 353                         }
 354                 }
 355         }
 356         return row;
 357 }
 358
 359 int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 360 {
 361         b3Generic6DofConstraint* d6constraint = this;
 362         int row = row_offset;
 363         //solve angular limits
 364         for (int i = 0; i < 3; i++)
 365         {
 366                 if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 367                 {
 368                         b3Vector3 axis = d6constraint->getAxis(i);
 369                         int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
 370                         if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
 371                         {
 372                                 m_angularLimits[i].m_normalCFM = info->cfm[0];
 373                         }
 374                         if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
 375                         {
 376                                 m_angularLimits[i].m_stopCFM = info->cfm[0];
 377                         }
 378                         if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
 379                         {
 380                                 m_angularLimits[i].m_stopERP = info->erp;
 381                         }
 382                         row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
 383                                                                                  transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 384                 }
 385         }
 386
 387         return row;
 388 }
 389
 390 void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
 391 {
 392         (void)timeStep;
 393 }
 394
 395 void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
 396 {
 397         m_frameInA = frameA;
 398         m_frameInB = frameB;
 399
 400         calculateTransforms(bodies);
 401 }
 402
 403 b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
 404 {
 405         return m_calculatedAxis[axis_index];
 406 }
 407
 408 b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
 409 {
 410         return m_calculatedLinearDiff[axisIndex];
 411 }
 412
 413 b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
 414 {
 415         return m_calculatedAxisAngleDiff[axisIndex];
 416 }
 417
 418 void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
 419 {
 420         b3Scalar imA = bodies[m_rbA].m_invMass;
 421         b3Scalar imB = bodies[m_rbB].m_invMass;
 422         b3Scalar weight;
 423         if (imB == b3Scalar(0.0))
 424         {
 425                 weight = b3Scalar(1.0);
 426         }
 427         else
 428         {
 429                 weight = imA / (imA + imB);
 430         }
 431         const b3Vector3& pA = m_calculatedTransformA.getOrigin();
 432         const b3Vector3& pB = m_calculatedTransformB.getOrigin();
 433         m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
 434         return;
 435 }
 436
 437 void b3Generic6DofConstraint::calculateLinearInfo()
 438 {
 439         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 440         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
 441         for (int i = 0; i < 3; i++)
 442         {
 443                 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 444                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 445         }
 446 }
 447
 448 int b3Generic6DofConstraint::get_limit_motor_info2(
 449         b3RotationalLimitMotor* limot,
 450         const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
 451         b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
 452 {
 453         int srow = row * info->rowskip;
 454         bool powered = limot->m_enableMotor;
 455         int limit = limot->m_currentLimit;
 456         if (powered || limit)
 457         {  // if the joint is powered, or has joint limits, add in the extra row
 458                 b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
 459                 b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 460                 if (J1)
 461                 {
 462                         J1[srow + 0] = ax1[0];
 463                         J1[srow + 1] = ax1[1];
 464                         J1[srow + 2] = ax1[2];
 465                 }
 466                 if (J2)
 467                 {
 468                         J2[srow + 0] = -ax1[0];
 469                         J2[srow + 1] = -ax1[1];
 470                         J2[srow + 2] = -ax1[2];
 471                 }
 472                 if ((!rotational))
 473                 {
 474                         if (m_useOffsetForConstraintFrame)
 475                         {
 476                                 b3Vector3 tmpA, tmpB, relA, relB;
 477                                 // get vector from bodyB to frameB in WCS
 478                                 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 479                                 // get its projection to constraint axis
 480                                 b3Vector3 projB = ax1 * relB.dot(ax1);
 481                                 // get vector directed from bodyB to constraint axis (and orthogonal to it)
 482                                 b3Vector3 orthoB = relB - projB;
 483                                 // same for bodyA
 484                                 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
 485                                 b3Vector3 projA = ax1 * relA.dot(ax1);
 486                                 b3Vector3 orthoA = relA - projA;
 487                                 // get desired offset between frames A and B along constraint axis
 488                                 b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
 489                                 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
 490                                 b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
 491                                 // get offset vectors relA and relB
 492                                 relA = orthoA + totalDist * m_factA;
 493                                 relB = orthoB - totalDist * m_factB;
 494                                 tmpA = relA.cross(ax1);
 495                                 tmpB = relB.cross(ax1);
 496                                 if (m_hasStaticBody && (!rotAllowed))
 497                                 {
 498                                         tmpA *= m_factA;
 499                                         tmpB *= m_factB;
 500                                 }
 501                                 int i;
 502                                 for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
 503                                 for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
 504                         }
 505                         else
 506                         {
 507                                 b3Vector3 ltd;  // Linear Torque Decoupling vector
 508                                 b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 509                                 ltd = c.cross(ax1);
 510                                 info->m_J1angularAxis[srow + 0] = ltd[0];
 511                                 info->m_J1angularAxis[srow + 1] = ltd[1];
 512                                 info->m_J1angularAxis[srow + 2] = ltd[2];
 513
 514                                 c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 515                                 ltd = -c.cross(ax1);
 516                                 info->m_J2angularAxis[srow + 0] = ltd[0];
 517                                 info->m_J2angularAxis[srow + 1] = ltd[1];
 518                                 info->m_J2angularAxis[srow + 2] = ltd[2];
 519                         }
 520                 }
 521                 // if we're limited low and high simultaneously, the joint motor is
 522                 // ineffective
 523                 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
 524                 info->m_constraintError[srow] = b3Scalar(0.f);
 525                 if (powered)
 526                 {
 527                         info->cfm[srow] = limot->m_normalCFM;
 528                         if (!limit)
 529                         {
 530                                 b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 531
 532                                 b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
 533                                                                                                    limot->m_loLimit,
 534                                                                                                    limot->m_hiLimit,
 535                                                                                                    tag_vel,
 536                                                                                                    info->fps * limot->m_stopERP);
 537                                 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
 538                                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
 539                                 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 540                         }
 541                 }
 542                 if (limit)
 543                 {
 544                         b3Scalar k = info->fps * limot->m_stopERP;
 545                         if (!rotational)
 546                         {
 547                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
 548                         }
 549                         else
 550                         {
 551                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 552                         }
 553                         info->cfm[srow] = limot->m_stopCFM;
 554                         if (limot->m_loLimit == limot->m_hiLimit)
 555                         {  // limited low and high simultaneously
 556                                 info->m_lowerLimit[srow] = -B3_INFINITY;
 557                                 info->m_upperLimit[srow] = B3_INFINITY;
 558                         }
 559                         else
 560                         {
 561                                 if (limit == 1)
 562                                 {
 563                                         info->m_lowerLimit[srow] = 0;
 564                                         info->m_upperLimit[srow] = B3_INFINITY;
 565                                 }
 566                                 else
 567                                 {
 568                                         info->m_lowerLimit[srow] = -B3_INFINITY;
 569                                         info->m_upperLimit[srow] = 0;
 570                                 }
 571                                 // deal with bounce
 572                                 if (limot->m_bounce > 0)
 573                                 {
 574                                         // calculate joint velocity
 575                                         b3Scalar vel;
 576                                         if (rotational)
 577                                         {
 578                                                 vel = angVelA.dot(ax1);
 579                                                 //make sure that if no body -> angVelB == zero vec
 580                                                 //                        if (body1)
 581                                                 vel -= angVelB.dot(ax1);
 582                                         }
 583                                         else
 584                                         {
 585                                                 vel = linVelA.dot(ax1);
 586                                                 //make sure that if no body -> angVelB == zero vec
 587                                                 //                        if (body1)
 588                                                 vel -= linVelB.dot(ax1);
 589                                         }
 590                                         // only apply bounce if the velocity is incoming, and if the
 591                                         // resulting c[] exceeds what we already have.
 592                                         if (limit == 1)
 593                                         {
 594                                                 if (vel < 0)
 595                                                 {
 596                                                         b3Scalar newc = -limot->m_bounce * vel;
 597                                                         if (newc > info->m_constraintError[srow])
 598                                                                 info->m_constraintError[srow] = newc;
 599                                                 }
 600                                         }
 601                                         else
 602                                         {
 603                                                 if (vel > 0)
 604                                                 {
 605                                                         b3Scalar newc = -limot->m_bounce * vel;
 606                                                         if (newc < info->m_constraintError[srow])
 607                                                                 info->m_constraintError[srow] = newc;
 608                                                 }
 609                                         }
 610                                 }
 611                         }
 612                 }
 613                 return 1;
 614         }
 615         else
 616                 return 0;
 617 }
 618
 619 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 620 ///If no axis is provided, it uses the default axis for this constraint.
 621 void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
 622 {
 623         if ((axis >= 0) && (axis < 3))
 624         {
 625                 switch (num)
 626                 {
 627                         case B3_CONSTRAINT_STOP_ERP:
 628                                 m_linearLimits.m_stopERP[axis] = value;
 629                                 m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 630                                 break;
 631                         case B3_CONSTRAINT_STOP_CFM:
 632                                 m_linearLimits.m_stopCFM[axis] = value;
 633                                 m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 634                                 break;
 635                         case B3_CONSTRAINT_CFM:
 636                                 m_linearLimits.m_normalCFM[axis] = value;
 637                                 m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 638                                 break;
 639                         default:
 640                                 b3AssertConstrParams(0);
 641                 }
 642         }
 643         else if ((axis >= 3) && (axis < 6))
 644         {
 645                 switch (num)
 646                 {
 647                         case B3_CONSTRAINT_STOP_ERP:
 648                                 m_angularLimits[axis - 3].m_stopERP = value;
 649                                 m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 650                                 break;
 651                         case B3_CONSTRAINT_STOP_CFM:
 652                                 m_angularLimits[axis - 3].m_stopCFM = value;
 653                                 m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 654                                 break;
 655                         case B3_CONSTRAINT_CFM:
 656                                 m_angularLimits[axis - 3].m_normalCFM = value;
 657                                 m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 658                                 break;
 659                         default:
 660                                 b3AssertConstrParams(0);
 661                 }
 662         }
 663         else
 664         {
 665                 b3AssertConstrParams(0);
 666         }
 667 }
 668
 669 ///return the local value of parameter
 670 b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
 671 {
 672         b3Scalar retVal = 0;
 673         if ((axis >= 0) && (axis < 3))
 674         {
 675                 switch (num)
 676                 {
 677                         case B3_CONSTRAINT_STOP_ERP:
 678                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 679                                 retVal = m_linearLimits.m_stopERP[axis];
 680                                 break;
 681                         case B3_CONSTRAINT_STOP_CFM:
 682                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 683                                 retVal = m_linearLimits.m_stopCFM[axis];
 684                                 break;
 685                         case B3_CONSTRAINT_CFM:
 686                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 687                                 retVal = m_linearLimits.m_normalCFM[axis];
 688                                 break;
 689                         default:
 690                                 b3AssertConstrParams(0);
 691                 }
 692         }
 693         else if ((axis >= 3) && (axis < 6))
 694         {
 695                 switch (num)
 696                 {
 697                         case B3_CONSTRAINT_STOP_ERP:
 698                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 699                                 retVal = m_angularLimits[axis - 3].m_stopERP;
 700                                 break;
 701                         case B3_CONSTRAINT_STOP_CFM:
 702                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 703                                 retVal = m_angularLimits[axis - 3].m_stopCFM;
 704                                 break;
 705                         case B3_CONSTRAINT_CFM:
 706                                 b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 707                                 retVal = m_angularLimits[axis - 3].m_normalCFM;
 708                                 break;
 709                         default:
 710                                 b3AssertConstrParams(0);
 711                 }
 712         }
 713         else
 714         {
 715                 b3AssertConstrParams(0);
 716         }
 717         return retVal;
 718 }
 719
 720 void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
 721 {
 722         b3Vector3 zAxis = axis1.normalized();
 723         b3Vector3 yAxis = axis2.normalized();
 724         b3Vector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
 725
 726         b3Transform frameInW;
 727         frameInW.setIdentity();
 728         frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
 729                                                                  xAxis[1], yAxis[1], zAxis[1],
 730                                                                  xAxis[2], yAxis[2], zAxis[2]);
 731
 732         // now get constraint frame in local coordinate systems
 733         m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
 734         m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
 735
 736         calculateTransforms(bodies);
 737 }