dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.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 /*
  17 2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
  18 Pros:
  19 - Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
  20 - Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
  21 - Servo motor functionality
  22 - Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
  23 - Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
  24
  25 Cons:
  26 - It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
  27 - At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
  28 */
  29
  30 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
  31 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
  32
  33 /*
  34 2007-09-09
  35 btGeneric6DofConstraint Refactored by Francisco Le?n
  36 email: projectileman@yahoo.com
  37 http://gimpact.sf.net
  38 */
  39
  40 #include "btGeneric6DofSpring2Constraint.h"
  41 #include "BulletDynamics/Dynamics/btRigidBody.h"
  42 #include "LinearMath/btTransformUtil.h"
  43 #include <cmath>
  44 #include <new>
  45
  46 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
  47         : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
  48 {
  49         calculateTransforms();
  50 }
  51
  52 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
  53         : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
  54 {
  55         ///not providing rigidbody A means implicitly using worldspace for body A
  56         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
  57         calculateTransforms();
  58 }
  59
  60 btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
  61 {
  62         int i = index % 3;
  63         int j = index / 3;
  64         return mat[i][j];
  65 }
  66
  67 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
  68
  69 bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
  70 {
  71         // rot =  cy*cz          -cy*sz           sy
  72         //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
  73         //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
  74
  75         btScalar fi = btGetMatrixElem(mat, 2);
  76         if (fi < btScalar(1.0f))
  77         {
  78                 if (fi > btScalar(-1.0f))
  79                 {
  80                         xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
  81                         xyz[1] = btAsin(btGetMatrixElem(mat, 2));
  82                         xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
  83                         return true;
  84                 }
  85                 else
  86                 {
  87                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
  88                         xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  89                         xyz[1] = -SIMD_HALF_PI;
  90                         xyz[2] = btScalar(0.0);
  91                         return false;
  92                 }
  93         }
  94         else
  95         {
  96                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
  97                 xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
  98                 xyz[1] = SIMD_HALF_PI;
  99                 xyz[2] = 0.0;
 100         }
 101         return false;
 102 }
 103
 104 bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz)
 105 {
 106         // rot =  cy*cz          -sz           sy*cz
 107         //        cy*cx*sz+sx*sy  cx*cz        sy*cx*sz-cy*sx
 108         //        cy*sx*sz-cx*sy  sx*cz        sy*sx*sz+cx*cy
 109
 110         btScalar fi = btGetMatrixElem(mat, 1);
 111         if (fi < btScalar(1.0f))
 112         {
 113                 if (fi > btScalar(-1.0f))
 114                 {
 115                         xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
 116                         xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 117                         xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
 118                         return true;
 119                 }
 120                 else
 121                 {
 122                         xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 123                         xyz[1] = btScalar(0.0);
 124                         xyz[2] = SIMD_HALF_PI;
 125                         return false;
 126                 }
 127         }
 128         else
 129         {
 130                 xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 131                 xyz[1] = 0.0;
 132                 xyz[2] = -SIMD_HALF_PI;
 133         }
 134         return false;
 135 }
 136
 137 bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz)
 138 {
 139         // rot =  cy*cz+sy*sx*sz  cz*sy*sx-cy*sz  cx*sy
 140         //        cx*sz           cx*cz           -sx
 141         //        cy*sx*sz-cz*sy  sy*sz+cy*cz*sx  cy*cx
 142
 143         btScalar fi = btGetMatrixElem(mat, 5);
 144         if (fi < btScalar(1.0f))
 145         {
 146                 if (fi > btScalar(-1.0f))
 147                 {
 148                         xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
 149                         xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
 150                         xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 151                         return true;
 152                 }
 153                 else
 154                 {
 155                         xyz[0] = SIMD_HALF_PI;
 156                         xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 157                         xyz[2] = btScalar(0.0);
 158                         return false;
 159                 }
 160         }
 161         else
 162         {
 163                 xyz[0] = -SIMD_HALF_PI;
 164                 xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 165                 xyz[2] = 0.0;
 166         }
 167         return false;
 168 }
 169
 170 bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz)
 171 {
 172         // rot =  cy*cz   sy*sx-cy*cx*sz   cx*sy+cy*sz*sx
 173         //        sz           cz*cx           -cz*sx
 174         //        -cz*sy  cy*sx+cx*sy*sz   cy*cx-sy*sz*sx
 175
 176         btScalar fi = btGetMatrixElem(mat, 3);
 177         if (fi < btScalar(1.0f))
 178         {
 179                 if (fi > btScalar(-1.0f))
 180                 {
 181                         xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
 182                         xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
 183                         xyz[2] = btAsin(btGetMatrixElem(mat, 3));
 184                         return true;
 185                 }
 186                 else
 187                 {
 188                         xyz[0] = btScalar(0.0);
 189                         xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 190                         xyz[2] = -SIMD_HALF_PI;
 191                         return false;
 192                 }
 193         }
 194         else
 195         {
 196                 xyz[0] = btScalar(0.0);
 197                 xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 198                 xyz[2] = SIMD_HALF_PI;
 199         }
 200         return false;
 201 }
 202
 203 bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz)
 204 {
 205         // rot =  cz*cy-sz*sx*sy    -cx*sz   cz*sy+cy*sz*sx
 206         //        cy*sz+cz*sx*sy     cz*cx   sz*sy-cz*xy*sx
 207         //        -cx*sy              sx     cx*cy
 208
 209         btScalar fi = btGetMatrixElem(mat, 7);
 210         if (fi < btScalar(1.0f))
 211         {
 212                 if (fi > btScalar(-1.0f))
 213                 {
 214                         xyz[0] = btAsin(btGetMatrixElem(mat, 7));
 215                         xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 216                         xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
 217                         return true;
 218                 }
 219                 else
 220                 {
 221                         xyz[0] = -SIMD_HALF_PI;
 222                         xyz[1] = btScalar(0.0);
 223                         xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 224                         return false;
 225                 }
 226         }
 227         else
 228         {
 229                 xyz[0] = SIMD_HALF_PI;
 230                 xyz[1] = btScalar(0.0);
 231                 xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 232         }
 233         return false;
 234 }
 235
 236 bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz)
 237 {
 238         // rot =  cz*cy   cz*sy*sx-cx*sz   sz*sx+cz*cx*sy
 239         //        cy*sz   cz*cx+sz*sy*sx   cx*sz*sy-cz*sx
 240         //        -sy          cy*sx         cy*cx
 241
 242         btScalar fi = btGetMatrixElem(mat, 6);
 243         if (fi < btScalar(1.0f))
 244         {
 245                 if (fi > btScalar(-1.0f))
 246                 {
 247                         xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 248                         xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
 249                         xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
 250                         return true;
 251                 }
 252                 else
 253                 {
 254                         xyz[0] = btScalar(0.0);
 255                         xyz[1] = SIMD_HALF_PI;
 256                         xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
 257                         return false;
 258                 }
 259         }
 260         else
 261         {
 262                 xyz[0] = btScalar(0.0);
 263                 xyz[1] = -SIMD_HALF_PI;
 264                 xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
 265         }
 266         return false;
 267 }
 268
 269 void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 270 {
 271         btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
 272         switch (m_rotateOrder)
 273         {
 274                 case RO_XYZ:
 275                         matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 276                         break;
 277                 case RO_XZY:
 278                         matrixToEulerXZY(relative_frame, m_calculatedAxisAngleDiff);
 279                         break;
 280                 case RO_YXZ:
 281                         matrixToEulerYXZ(relative_frame, m_calculatedAxisAngleDiff);
 282                         break;
 283                 case RO_YZX:
 284                         matrixToEulerYZX(relative_frame, m_calculatedAxisAngleDiff);
 285                         break;
 286                 case RO_ZXY:
 287                         matrixToEulerZXY(relative_frame, m_calculatedAxisAngleDiff);
 288                         break;
 289                 case RO_ZYX:
 290                         matrixToEulerZYX(relative_frame, m_calculatedAxisAngleDiff);
 291                         break;
 292                 default:
 293                         btAssert(false);
 294         }
 295         // in euler angle mode we do not actually constrain the angular velocity
 296         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
 297         //
 298         //    to get                    constrain w2-w1 along           ...not
 299         //    ------                    ---------------------           ------
 300         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
 301         //    d(angle[1])/dt = 0        ax[1]
 302         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
 303         //
 304         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
 305         // to prove the result for angle[0], write the expression for angle[0] from
 306         // GetInfo1 then take the derivative. to prove this for angle[2] it is
 307         // easier to take the euler rate expression for d(angle[2])/dt with respect
 308         // to the components of w and set that to 0.
 309         switch (m_rotateOrder)
 310         {
 311                 case RO_XYZ:
 312                 {
 313                         //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
 314                         //The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
 315                         //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
 316                         //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
 317                         // x' = Nperp = N.cross(axis2)
 318                         // y' = N = axis2.cross(axis0)
 319                         // z' = z
 320                         //
 321                         // x" = X
 322                         // y" = y'
 323                         // z" = ??
 324                         //in other words:
 325                         //first rotate around z
 326                         //second rotate around y'= z.cross(X)
 327                         //third rotate around x" = X
 328                         //Original XYZ extrinsic rotation order.
 329                         //Planes: xy and YZ normals: z, X.  Plane intersection (N) is z.cross(X)
 330                         btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 331                         btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
 332                         m_calculatedAxis[1] = axis2.cross(axis0);
 333                         m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 334                         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 335                         break;
 336                 }
 337                 case RO_XZY:
 338                 {
 339                         //planes: xz,ZY normals: y, X
 340                         //first rotate around y
 341                         //second rotate around z'= y.cross(X)
 342                         //third rotate around x" = X
 343                         btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 344                         btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
 345                         m_calculatedAxis[2] = axis0.cross(axis1);
 346                         m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
 347                         m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 348                         break;
 349                 }
 350                 case RO_YXZ:
 351                 {
 352                         //planes: yx,XZ normals: z, Y
 353                         //first rotate around z
 354                         //second rotate around x'= z.cross(Y)
 355                         //third rotate around y" = Y
 356                         btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
 357                         btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
 358                         m_calculatedAxis[0] = axis1.cross(axis2);
 359                         m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
 360                         m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 361                         break;
 362                 }
 363                 case RO_YZX:
 364                 {
 365                         //planes: yz,ZX normals: x, Y
 366                         //first rotate around x
 367                         //second rotate around z'= x.cross(Y)
 368                         //third rotate around y" = Y
 369                         btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
 370                         btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
 371                         m_calculatedAxis[2] = axis0.cross(axis1);
 372                         m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
 373                         m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 374                         break;
 375                 }
 376                 case RO_ZXY:
 377                 {
 378                         //planes: zx,XY normals: y, Z
 379                         //first rotate around y
 380                         //second rotate around x'= y.cross(Z)
 381                         //third rotate around z" = Z
 382                         btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
 383                         btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
 384                         m_calculatedAxis[0] = axis1.cross(axis2);
 385                         m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
 386                         m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 387                         break;
 388                 }
 389                 case RO_ZYX:
 390                 {
 391                         //planes: zy,YX normals: x, Z
 392                         //first rotate around x
 393                         //second rotate around y' = x.cross(Z)
 394                         //third rotate around z" = Z
 395                         btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
 396                         btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
 397                         m_calculatedAxis[1] = axis2.cross(axis0);
 398                         m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 399                         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 400                         break;
 401                 }
 402                 default:
 403                         btAssert(false);
 404         }
 405
 406         m_calculatedAxis[0].normalize();
 407         m_calculatedAxis[1].normalize();
 408         m_calculatedAxis[2].normalize();
 409 }
 410
 411 void btGeneric6DofSpring2Constraint::calculateTransforms()
 412 {
 413         calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 414 }
 415
 416 void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 417 {
 418         m_calculatedTransformA = transA * m_frameInA;
 419         m_calculatedTransformB = transB * m_frameInB;
 420         calculateLinearInfo();
 421         calculateAngleInfo();
 422
 423         btScalar miA = getRigidBodyA().getInvMass();
 424         btScalar miB = getRigidBodyB().getInvMass();
 425         m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 426         btScalar miS = miA + miB;
 427         if (miS > btScalar(0.f))
 428         {
 429                 m_factA = miB / miS;
 430         }
 431         else
 432         {
 433                 m_factA = btScalar(0.5f);
 434         }
 435         m_factB = btScalar(1.0f) - m_factA;
 436 }
 437
 438 void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
 439 {
 440         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
 441         angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
 442         m_angularLimits[axis_index].m_currentPosition = angle;
 443         m_angularLimits[axis_index].testLimitValue(angle);
 444 }
 445
 446 void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info)
 447 {
 448         //prepare constraint
 449         calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 450         info->m_numConstraintRows = 0;
 451         info->nub = 0;
 452         int i;
 453         //test linear limits
 454         for (i = 0; i < 3; i++)
 455         {
 456                 if (m_linearLimits.m_currentLimit[i] == 4)
 457                         info->m_numConstraintRows += 2;
 458                 else if (m_linearLimits.m_currentLimit[i] != 0)
 459                         info->m_numConstraintRows += 1;
 460                 if (m_linearLimits.m_enableMotor[i]) info->m_numConstraintRows += 1;
 461                 if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
 462         }
 463         //test angular limits
 464         for (i = 0; i < 3; i++)
 465         {
 466                 testAngularLimitMotor(i);
 467                 if (m_angularLimits[i].m_currentLimit == 4)
 468                         info->m_numConstraintRows += 2;
 469                 else if (m_angularLimits[i].m_currentLimit != 0)
 470                         info->m_numConstraintRows += 1;
 471                 if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
 472                 if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
 473         }
 474 }
 475
 476 void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info)
 477 {
 478         const btTransform& transA = m_rbA.getCenterOfMassTransform();
 479         const btTransform& transB = m_rbB.getCenterOfMassTransform();
 480         const btVector3& linVelA = m_rbA.getLinearVelocity();
 481         const btVector3& linVelB = m_rbB.getLinearVelocity();
 482         const btVector3& angVelA = m_rbA.getAngularVelocity();
 483         const btVector3& angVelB = m_rbB.getAngularVelocity();
 484
 485         // for stability better to solve angular limits first
 486         int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
 487         setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 488 }
 489
 490 int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 491 {
 492         //solve linear limits
 493         btRotationalLimitMotor2 limot;
 494         for (int i = 0; i < 3; i++)
 495         {
 496                 if (m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
 497                 {  // re-use rotational motor code
 498                         limot.m_bounce = m_linearLimits.m_bounce[i];
 499                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 500                         limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
 501                         limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
 502                         limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
 503                         limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
 504                         limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
 505                         limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
 506                         limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
 507                         limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
 508                         limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
 509                         limot.m_springDamping = m_linearLimits.m_springDamping[i];
 510                         limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
 511                         limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
 512                         limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
 513                         limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
 514                         limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
 515                         limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 516                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 517                         int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
 518                         limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
 519                         limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
 520                         limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
 521                         limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
 522
 523                         //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
 524                         int indx1 = (i + 1) % 3;
 525                         int indx2 = (i + 2) % 3;
 526                         int rotAllowed = 1;  // rotations around orthos to current axis (it is used only when one of the body is static)
 527 #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
 528                         bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
 529                                                                  m_angularLimits[indx1].m_currentLimit == 2 ||
 530                                                                  (m_angularLimits[indx1].m_currentLimit == 3 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
 531                                                                  (m_angularLimits[indx1].m_currentLimit == 4 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
 532                         bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
 533                                                                  m_angularLimits[indx2].m_currentLimit == 2 ||
 534                                                                  (m_angularLimits[indx2].m_currentLimit == 3 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
 535                                                                  (m_angularLimits[indx2].m_currentLimit == 4 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
 536                         if (indx1Violated && indx2Violated)
 537                         {
 538                                 rotAllowed = 0;
 539                         }
 540                         row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 541                 }
 542         }
 543         return row;
 544 }
 545
 546 int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 547 {
 548         int row = row_offset;
 549
 550         //order of rotational constraint rows
 551         int cIdx[] = {0, 1, 2};
 552         switch (m_rotateOrder)
 553         {
 554                 case RO_XYZ:
 555                         cIdx[0] = 0;
 556                         cIdx[1] = 1;
 557                         cIdx[2] = 2;
 558                         break;
 559                 case RO_XZY:
 560                         cIdx[0] = 0;
 561                         cIdx[1] = 2;
 562                         cIdx[2] = 1;
 563                         break;
 564                 case RO_YXZ:
 565                         cIdx[0] = 1;
 566                         cIdx[1] = 0;
 567                         cIdx[2] = 2;
 568                         break;
 569                 case RO_YZX:
 570                         cIdx[0] = 1;
 571                         cIdx[1] = 2;
 572                         cIdx[2] = 0;
 573                         break;
 574                 case RO_ZXY:
 575                         cIdx[0] = 2;
 576                         cIdx[1] = 0;
 577                         cIdx[2] = 1;
 578                         break;
 579                 case RO_ZYX:
 580                         cIdx[0] = 2;
 581                         cIdx[1] = 1;
 582                         cIdx[2] = 0;
 583                         break;
 584                 default:
 585                         btAssert(false);
 586         }
 587
 588         for (int ii = 0; ii < 3; ii++)
 589         {
 590                 int i = cIdx[ii];
 591                 if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
 592                 {
 593                         btVector3 axis = getAxis(i);
 594                         int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
 595                         if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
 596                         {
 597                                 m_angularLimits[i].m_stopCFM = info->cfm[0];
 598                         }
 599                         if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
 600                         {
 601                                 m_angularLimits[i].m_stopERP = info->erp;
 602                         }
 603                         if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
 604                         {
 605                                 m_angularLimits[i].m_motorCFM = info->cfm[0];
 606                         }
 607                         if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
 608                         {
 609                                 m_angularLimits[i].m_motorERP = info->erp;
 610                         }
 611                         row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 612                 }
 613         }
 614
 615         return row;
 616 }
 617
 618 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 619 {
 620         m_frameInA = frameA;
 621         m_frameInB = frameB;
 622         buildJacobian();
 623         calculateTransforms();
 624 }
 625
 626 void btGeneric6DofSpring2Constraint::calculateLinearInfo()
 627 {
 628         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 629         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
 630         for (int i = 0; i < 3; i++)
 631         {
 632                 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 633                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 634         }
 635 }
 636
 637 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
 638 {
 639         btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
 640         btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 641
 642         J1[srow + 0] = ax1[0];
 643         J1[srow + 1] = ax1[1];
 644         J1[srow + 2] = ax1[2];
 645
 646         J2[srow + 0] = -ax1[0];
 647         J2[srow + 1] = -ax1[1];
 648         J2[srow + 2] = -ax1[2];
 649
 650         if (!rotational)
 651         {
 652                 btVector3 tmpA, tmpB, relA, relB;
 653                 // get vector from bodyB to frameB in WCS
 654                 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 655                 // same for bodyA
 656                 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
 657                 tmpA = relA.cross(ax1);
 658                 tmpB = relB.cross(ax1);
 659                 if (m_hasStaticBody && (!rotAllowed))
 660                 {
 661                         tmpA *= m_factA;
 662                         tmpB *= m_factB;
 663                 }
 664                 int i;
 665                 for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
 666                 for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
 667         }
 668 }
 669
 670 int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 671         btRotationalLimitMotor2* limot,
 672         const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
 673         btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
 674 {
 675         int count = 0;
 676         int srow = row * info->rowskip;
 677
 678         if (limot->m_currentLimit == 4)
 679         {
 680                 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
 681
 682                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 683                 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
 684                 if (rotational)
 685                 {
 686                         if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
 687                         {
 688                                 btScalar bounceerror = -limot->m_bounce * vel;
 689                                 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 690                         }
 691                 }
 692                 else
 693                 {
 694                         if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
 695                         {
 696                                 btScalar bounceerror = -limot->m_bounce * vel;
 697                                 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 698                         }
 699                 }
 700                 info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
 701                 info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
 702                 info->cfm[srow] = limot->m_stopCFM;
 703                 srow += info->rowskip;
 704                 ++count;
 705
 706                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 707                 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
 708                 if (rotational)
 709                 {
 710                         if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
 711                         {
 712                                 btScalar bounceerror = -limot->m_bounce * vel;
 713                                 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 714                         }
 715                 }
 716                 else
 717                 {
 718                         if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
 719                         {
 720                                 btScalar bounceerror = -limot->m_bounce * vel;
 721                                 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 722                         }
 723                 }
 724                 info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
 725                 info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
 726                 info->cfm[srow] = limot->m_stopCFM;
 727                 srow += info->rowskip;
 728                 ++count;
 729         }
 730         else if (limot->m_currentLimit == 3)
 731         {
 732                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 733                 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
 734                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 735                 info->m_upperLimit[srow] = SIMD_INFINITY;
 736                 info->cfm[srow] = limot->m_stopCFM;
 737                 srow += info->rowskip;
 738                 ++count;
 739         }
 740
 741         if (limot->m_enableMotor && !limot->m_servoMotor)
 742         {
 743                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 744                 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 745                 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
 746                                                                                    limot->m_loLimit,
 747                                                                                    limot->m_hiLimit,
 748                                                                                    tag_vel,
 749                                                                                    info->fps * limot->m_motorERP);
 750                 info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
 751                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
 752                 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 753                 info->cfm[srow] = limot->m_motorCFM;
 754                 srow += info->rowskip;
 755                 ++count;
 756         }
 757
 758         if (limot->m_enableMotor && limot->m_servoMotor)
 759         {
 760                 btScalar error = limot->m_currentPosition - limot->m_servoTarget;
 761                 btScalar curServoTarget = limot->m_servoTarget;
 762                 if (rotational)
 763                 {
 764                         if (error > SIMD_PI)
 765                         {
 766                                 error -= SIMD_2_PI;
 767                                 curServoTarget += SIMD_2_PI;
 768                         }
 769                         if (error < -SIMD_PI)
 770                         {
 771                                 error += SIMD_2_PI;
 772                                 curServoTarget -= SIMD_2_PI;
 773                         }
 774                 }
 775
 776                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 777                 btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
 778                 btScalar tag_vel = -targetvelocity;
 779                 btScalar mot_fact;
 780                 if (error != 0)
 781                 {
 782                         btScalar lowLimit;
 783                         btScalar hiLimit;
 784                         if (limot->m_loLimit > limot->m_hiLimit)
 785                         {
 786                                 lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
 787                                 hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
 788                         }
 789                         else
 790                         {
 791                                 lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
 792                                 hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
 793                         }
 794                         mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
 795                 }
 796                 else
 797                 {
 798                         mot_fact = 0;
 799                 }
 800                 info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
 801                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
 802                 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 803                 info->cfm[srow] = limot->m_motorCFM;
 804                 srow += info->rowskip;
 805                 ++count;
 806         }
 807
 808         if (limot->m_enableSpring)
 809         {
 810                 btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
 811                 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 812
 813                 //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
 814                 //if(cfm > 0.99999)
 815                 //      cfm = 0.99999;
 816                 //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
 817                 //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
 818                 //info->m_lowerLimit[srow] = -SIMD_INFINITY;
 819                 //info->m_upperLimit[srow] = SIMD_INFINITY;
 820
 821                 btScalar dt = BT_ONE / info->fps;
 822                 btScalar kd = limot->m_springDamping;
 823                 btScalar ks = limot->m_springStiffness;
 824                 btScalar vel;
 825                 if (rotational)
 826                 {
 827                         vel = angVelA.dot(ax1) - angVelB.dot(ax1);
 828                 }
 829                 else
 830                 {
 831                         btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
 832                         btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
 833                         vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
 834                 }
 835                 btScalar cfm = BT_ZERO;
 836                 btScalar mA = BT_ONE / m_rbA.getInvMass();
 837                 btScalar mB = BT_ONE / m_rbB.getInvMass();
 838                 if (rotational)
 839                 {
 840                         btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
 841                         btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
 842                         if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
 843                         if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
 844                 }
 845                 btScalar m;
 846                 if (m_rbA.getInvMass() == 0) m = mB; else
 847                 if (m_rbB.getInvMass() == 0) m = mA; else
 848                         m = mA*mB / (mA + mB);
 849                 btScalar angularfreq = btSqrt(ks / m);
 850
 851                 //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
 852                 if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
 853                 {
 854                         ks = BT_ONE / dt / dt / btScalar(16.0) * m;
 855                 }
 856                 //avoid damping that would blow up the spring
 857                 if (limot->m_springDampingLimited && kd * dt > m)
 858                 {
 859                         kd = m / dt;
 860                 }
 861                 btScalar fs = ks * error * dt;
 862                 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
 863                 btScalar f = (fs + fd);
 864
 865                 // after the spring force affecting the body(es) the new velocity will be
 866                 // vel + f / m * (rotational ? -1 : 1)
 867                 // so in theory this should be set here for m_constraintError
 868                 // (with m_constraintError we set a desired velocity for the affected body(es))
 869                 // however in practice any value is fine as long as it is greater than the "proper" velocity,
 870                 // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
 871                 // so it is much simpler (and more robust) just to simply use inf (with the proper sign)
 872                 // (Even with our best intent the "new" velocity is only an estimation. If we underestimate
 873                 // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
 874                 // matter, because the solver will limit it according the force limit)
 875                 // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
 876                 // will we not request a velocity with the wrong direction ?
 877                 // and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
 878                 // so the sign of the force that is really matters
 879                 if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
 880                         info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
 881                 else
 882                         info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 883
 884                 btScalar minf = f < fd ? f : fd;
 885                 btScalar maxf = f < fd ? fd : f;
 886                 if (!rotational)
 887                 {
 888                         info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
 889                         info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
 890                 }
 891                 else
 892                 {
 893                         info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
 894                         info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
 895                 }
 896
 897                 info->cfm[srow] = cfm;
 898                 srow += info->rowskip;
 899                 ++count;
 900         }
 901
 902         return count;
 903 }
 904
 905 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 906 //If no axis is provided, it uses the default axis for this constraint.
 907 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
 908 {
 909         if ((axis >= 0) && (axis < 3))
 910         {
 911                 switch (num)
 912                 {
 913                         case BT_CONSTRAINT_STOP_ERP:
 914                                 m_linearLimits.m_stopERP[axis] = value;
 915                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 916                                 break;
 917                         case BT_CONSTRAINT_STOP_CFM:
 918                                 m_linearLimits.m_stopCFM[axis] = value;
 919                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 920                                 break;
 921                         case BT_CONSTRAINT_ERP:
 922                                 m_linearLimits.m_motorERP[axis] = value;
 923                                 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 924                                 break;
 925                         case BT_CONSTRAINT_CFM:
 926                                 m_linearLimits.m_motorCFM[axis] = value;
 927                                 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 928                                 break;
 929                         default:
 930                                 btAssertConstrParams(0);
 931                 }
 932         }
 933         else if ((axis >= 3) && (axis < 6))
 934         {
 935                 switch (num)
 936                 {
 937                         case BT_CONSTRAINT_STOP_ERP:
 938                                 m_angularLimits[axis - 3].m_stopERP = value;
 939                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 940                                 break;
 941                         case BT_CONSTRAINT_STOP_CFM:
 942                                 m_angularLimits[axis - 3].m_stopCFM = value;
 943                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 944                                 break;
 945                         case BT_CONSTRAINT_ERP:
 946                                 m_angularLimits[axis - 3].m_motorERP = value;
 947                                 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 948                                 break;
 949                         case BT_CONSTRAINT_CFM:
 950                                 m_angularLimits[axis - 3].m_motorCFM = value;
 951                                 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 952                                 break;
 953                         default:
 954                                 btAssertConstrParams(0);
 955                 }
 956         }
 957         else
 958         {
 959                 btAssertConstrParams(0);
 960         }
 961 }
 962
 963 //return the local value of parameter
 964 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
 965 {
 966         btScalar retVal = 0;
 967         if ((axis >= 0) && (axis < 3))
 968         {
 969                 switch (num)
 970                 {
 971                         case BT_CONSTRAINT_STOP_ERP:
 972                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 973                                 retVal = m_linearLimits.m_stopERP[axis];
 974                                 break;
 975                         case BT_CONSTRAINT_STOP_CFM:
 976                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 977                                 retVal = m_linearLimits.m_stopCFM[axis];
 978                                 break;
 979                         case BT_CONSTRAINT_ERP:
 980                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 981                                 retVal = m_linearLimits.m_motorERP[axis];
 982                                 break;
 983                         case BT_CONSTRAINT_CFM:
 984                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 985                                 retVal = m_linearLimits.m_motorCFM[axis];
 986                                 break;
 987                         default:
 988                                 btAssertConstrParams(0);
 989                 }
 990         }
 991         else if ((axis >= 3) && (axis < 6))
 992         {
 993                 switch (num)
 994                 {
 995                         case BT_CONSTRAINT_STOP_ERP:
 996                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 997                                 retVal = m_angularLimits[axis - 3].m_stopERP;
 998                                 break;
 999                         case BT_CONSTRAINT_STOP_CFM:
1000                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1001                                 retVal = m_angularLimits[axis - 3].m_stopCFM;
1002                                 break;
1003                         case BT_CONSTRAINT_ERP:
1004                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1005                                 retVal = m_angularLimits[axis - 3].m_motorERP;
1006                                 break;
1007                         case BT_CONSTRAINT_CFM:
1008                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1009                                 retVal = m_angularLimits[axis - 3].m_motorCFM;
1010                                 break;
1011                         default:
1012                                 btAssertConstrParams(0);
1013                 }
1014         }
1015         else
1016         {
1017                 btAssertConstrParams(0);
1018         }
1019         return retVal;
1020 }
1021
1022 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1, const btVector3& axis2)
1023 {
1024         btVector3 zAxis = axis1.normalized();
1025         btVector3 yAxis = axis2.normalized();
1026         btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
1027
1028         btTransform frameInW;
1029         frameInW.setIdentity();
1030         frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1031                                                                  xAxis[1], yAxis[1], zAxis[1],
1032                                                                  xAxis[2], yAxis[2], zAxis[2]);
1033
1034         // now get constraint frame in local coordinate systems
1035         m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1036         m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1037
1038         calculateTransforms();
1039 }
1040
1041 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
1042 {
1043         btAssert((index >= 0) && (index < 6));
1044         if (index < 3)
1045                 m_linearLimits.m_bounce[index] = bounce;
1046         else
1047                 m_angularLimits[index - 3].m_bounce = bounce;
1048 }
1049
1050 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
1051 {
1052         btAssert((index >= 0) && (index < 6));
1053         if (index < 3)
1054                 m_linearLimits.m_enableMotor[index] = onOff;
1055         else
1056                 m_angularLimits[index - 3].m_enableMotor = onOff;
1057 }
1058
1059 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
1060 {
1061         btAssert((index >= 0) && (index < 6));
1062         if (index < 3)
1063                 m_linearLimits.m_servoMotor[index] = onOff;
1064         else
1065                 m_angularLimits[index - 3].m_servoMotor = onOff;
1066 }
1067
1068 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
1069 {
1070         btAssert((index >= 0) && (index < 6));
1071         if (index < 3)
1072                 m_linearLimits.m_targetVelocity[index] = velocity;
1073         else
1074                 m_angularLimits[index - 3].m_targetVelocity = velocity;
1075 }
1076
1077 void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
1078 {
1079         btAssert((index >= 0) && (index < 6));
1080         if (index < 3)
1081         {
1082                 m_linearLimits.m_servoTarget[index] = targetOrg;
1083         }
1084         else
1085         {
1086                 //wrap between -PI and PI, see also
1087                 //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1088
1089                 btScalar target = targetOrg + SIMD_PI;
1090                 if (1)
1091                 {
1092                         btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
1093                         // handle boundary cases resulted from floating-point cut off:
1094                         {
1095                                 if (m >= SIMD_2_PI)
1096                                 {
1097                                         target = 0;
1098                                 }
1099                                 else
1100                                 {
1101                                         if (m < 0)
1102                                         {
1103                                                 if (SIMD_2_PI + m == SIMD_2_PI)
1104                                                         target = 0;
1105                                                 else
1106                                                         target = SIMD_2_PI + m;
1107                                         }
1108                                         else
1109                                         {
1110                                                 target = m;
1111                                         }
1112                                 }
1113                         }
1114                         target -= SIMD_PI;
1115                 }
1116
1117                 m_angularLimits[index - 3].m_servoTarget = target;
1118         }
1119 }
1120
1121 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
1122 {
1123         btAssert((index >= 0) && (index < 6));
1124         if (index < 3)
1125                 m_linearLimits.m_maxMotorForce[index] = force;
1126         else
1127                 m_angularLimits[index - 3].m_maxMotorForce = force;
1128 }
1129
1130 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
1131 {
1132         btAssert((index >= 0) && (index < 6));
1133         if (index < 3)
1134                 m_linearLimits.m_enableSpring[index] = onOff;
1135         else
1136                 m_angularLimits[index - 3].m_enableSpring = onOff;
1137 }
1138
1139 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1140 {
1141         btAssert((index >= 0) && (index < 6));
1142         if (index < 3)
1143         {
1144                 m_linearLimits.m_springStiffness[index] = stiffness;
1145                 m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1146         }
1147         else
1148         {
1149                 m_angularLimits[index - 3].m_springStiffness = stiffness;
1150                 m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1151         }
1152 }
1153
1154 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1155 {
1156         btAssert((index >= 0) && (index < 6));
1157         if (index < 3)
1158         {
1159                 m_linearLimits.m_springDamping[index] = damping;
1160                 m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1161         }
1162         else
1163         {
1164                 m_angularLimits[index - 3].m_springDamping = damping;
1165                 m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1166         }
1167 }
1168
1169 void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
1170 {
1171         calculateTransforms();
1172         int i;
1173         for (i = 0; i < 3; i++)
1174                 m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1175         for (i = 0; i < 3; i++)
1176                 m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1177 }
1178
1179 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
1180 {
1181         btAssert((index >= 0) && (index < 6));
1182         calculateTransforms();
1183         if (index < 3)
1184                 m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1185         else
1186                 m_angularLimits[index - 3].m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1187 }
1188
1189 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
1190 {
1191         btAssert((index >= 0) && (index < 6));
1192         if (index < 3)
1193                 m_linearLimits.m_equilibriumPoint[index] = val;
1194         else
1195                 m_angularLimits[index - 3].m_equilibriumPoint = val;
1196 }
1197
1198 //////////////////////////// btRotationalLimitMotor2 ////////////////////////////////////
1199
1200 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
1201 {
1202         //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
1203         if (m_loLimit > m_hiLimit)
1204         {
1205                 m_currentLimit = 0;
1206                 m_currentLimitError = btScalar(0.f);
1207         }
1208         else if (m_loLimit == m_hiLimit)
1209         {
1210                 m_currentLimitError = test_value - m_loLimit;
1211                 m_currentLimit = 3;
1212         }
1213         else
1214         {
1215                 m_currentLimitError = test_value - m_loLimit;
1216                 m_currentLimitErrorHi = test_value - m_hiLimit;
1217                 m_currentLimit = 4;
1218         }
1219 }
1220
1221 //////////////////////////// btTranslationalLimitMotor2 ////////////////////////////////////
1222
1223 void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
1224 {
1225         btScalar loLimit = m_lowerLimit[limitIndex];
1226         btScalar hiLimit = m_upperLimit[limitIndex];
1227         if (loLimit > hiLimit)
1228         {
1229                 m_currentLimitError[limitIndex] = 0;
1230                 m_currentLimit[limitIndex] = 0;
1231         }
1232         else if (loLimit == hiLimit)
1233         {
1234                 m_currentLimitError[limitIndex] = test_value - loLimit;
1235                 m_currentLimit[limitIndex] = 3;
1236         }
1237         else
1238         {
1239                 m_currentLimitError[limitIndex] = test_value - loLimit;
1240                 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1241                 m_currentLimit[limitIndex] = 4;
1242         }
1243 }