src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 /*
  17 Added by Roman Ponomarev (rponom@gmail.com)
  18 April 04, 2008
  19 */
  20
  21
  22
  23 #include "btSliderConstraint.h"
  24 #include "BulletDynamics/Dynamics/btRigidBody.h"
  25 #include "LinearMath/btTransformUtil.h"
  26 #include <new>
  27
  28 #define USE_OFFSET_FOR_CONSTANT_FRAME true
  29
  30 void btSliderConstraint::initParams()
  31 {
  32     m_lowerLinLimit = btScalar(1.0);
  33     m_upperLinLimit = btScalar(-1.0);
  34     m_lowerAngLimit = btScalar(0.);
  35     m_upperAngLimit = btScalar(0.);
  36         m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  37         m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  38         m_dampingDirLin = btScalar(0.);
  39         m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
  40         m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  41         m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  42         m_dampingDirAng = btScalar(0.);
  43         m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
  44         m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  45         m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  46         m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
  47         m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
  48         m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  49         m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  50         m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
  51         m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
  52         m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  53         m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  54         m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
  55         m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
  56         m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
  57         m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
  58         m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
  59         m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
  60
  61         m_poweredLinMotor = false;
  62     m_targetLinMotorVelocity = btScalar(0.);
  63     m_maxLinMotorForce = btScalar(0.);
  64         m_accumulatedLinMotorImpulse = btScalar(0.0);
  65
  66         m_poweredAngMotor = false;
  67     m_targetAngMotorVelocity = btScalar(0.);
  68     m_maxAngMotorForce = btScalar(0.);
  69         m_accumulatedAngMotorImpulse = btScalar(0.0);
  70
  71         m_flags = 0;
  72         m_flags = 0;
  73
  74         m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
  75
  76         calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
  77 }
  78
  79
  80
  81
  82
  83 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
  84         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
  85                 m_useSolveConstraintObsolete(false),
  86                 m_frameInA(frameInA),
  87         m_frameInB(frameInB),
  88                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
  89 {
  90         initParams();
  91 }
  92
  93
  94
  95 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
  96         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
  97                 m_useSolveConstraintObsolete(false),
  98                 m_frameInB(frameInB),
  99                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 100 {
 101         ///not providing rigidbody A means implicitly using worldspace for body A
 102         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
 103 //      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
 104
 105         initParams();
 106 }
 107
 108
 109
 110
 111
 112
 113 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 114 {
 115         if (m_useSolveConstraintObsolete)
 116         {
 117                 info->m_numConstraintRows = 0;
 118                 info->nub = 0;
 119         }
 120         else
 121         {
 122                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
 123                 info->nub = 2;
 124                 //prepare constraint
 125                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 126                 testAngLimits();
 127                 testLinLimits();
 128                 if(getSolveLinLimit() || getPoweredLinMotor())
 129                 {
 130                         info->m_numConstraintRows++; // limit 3rd linear as well
 131                         info->nub--;
 132                 }
 133                 if(getSolveAngLimit() || getPoweredAngMotor())
 134                 {
 135                         info->m_numConstraintRows++; // limit 3rd angular as well
 136                         info->nub--;
 137                 }
 138         }
 139 }
 140
 141 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 142 {
 143
 144         info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
 145         info->nub = 0;
 146 }
 147
 148 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 149 {
 150         getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
 151 }
 152
 153
 154
 155
 156
 157
 158
 159 void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
 160 {
 161         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 162         {
 163                 m_calculatedTransformA = transA * m_frameInA;
 164                 m_calculatedTransformB = transB * m_frameInB;
 165         }
 166         else
 167         {
 168                 m_calculatedTransformA = transB * m_frameInB;
 169                 m_calculatedTransformB = transA * m_frameInA;
 170         }
 171         m_realPivotAInW = m_calculatedTransformA.getOrigin();
 172         m_realPivotBInW = m_calculatedTransformB.getOrigin();
 173         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
 174         if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
 175         {
 176                 m_delta = m_realPivotBInW - m_realPivotAInW;
 177         }
 178         else
 179         {
 180                 m_delta = m_realPivotAInW - m_realPivotBInW;
 181         }
 182         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
 183     btVector3 normalWorld;
 184     int i;
 185     //linear part
 186     for(i = 0; i < 3; i++)
 187     {
 188                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 189                 m_depth[i] = m_delta.dot(normalWorld);
 190     }
 191 }
 192
 193
 194
 195 void btSliderConstraint::testLinLimits(void)
 196 {
 197         m_solveLinLim = false;
 198         m_linPos = m_depth[0];
 199         if(m_lowerLinLimit <= m_upperLinLimit)
 200         {
 201                 if(m_depth[0] > m_upperLinLimit)
 202                 {
 203                         m_depth[0] -= m_upperLinLimit;
 204                         m_solveLinLim = true;
 205                 }
 206                 else if(m_depth[0] < m_lowerLinLimit)
 207                 {
 208                         m_depth[0] -= m_lowerLinLimit;
 209                         m_solveLinLim = true;
 210                 }
 211                 else
 212                 {
 213                         m_depth[0] = btScalar(0.);
 214                 }
 215         }
 216         else
 217         {
 218                 m_depth[0] = btScalar(0.);
 219         }
 220 }
 221
 222
 223
 224 void btSliderConstraint::testAngLimits(void)
 225 {
 226         m_angDepth = btScalar(0.);
 227         m_solveAngLim = false;
 228         if(m_lowerAngLimit <= m_upperAngLimit)
 229         {
 230                 const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
 231                 const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
 232                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
 233 //              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
 234                 btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
 235                 rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
 236                 m_angPos = rot;
 237                 if(rot < m_lowerAngLimit)
 238                 {
 239                         m_angDepth = rot - m_lowerAngLimit;
 240                         m_solveAngLim = true;
 241                 }
 242                 else if(rot > m_upperAngLimit)
 243                 {
 244                         m_angDepth = rot - m_upperAngLimit;
 245                         m_solveAngLim = true;
 246                 }
 247         }
 248 }
 249
 250 btVector3 btSliderConstraint::getAncorInA(void)
 251 {
 252         btVector3 ancorInA;
 253         ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
 254         ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
 255         return ancorInA;
 256 }
 257
 258
 259
 260 btVector3 btSliderConstraint::getAncorInB(void)
 261 {
 262         btVector3 ancorInB;
 263         ancorInB = m_frameInB.getOrigin();
 264         return ancorInB;
 265 }
 266
 267
 268 void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
 269 {
 270         const btTransform& trA = getCalculatedTransformA();
 271         const btTransform& trB = getCalculatedTransformB();
 272
 273         btAssert(!m_useSolveConstraintObsolete);
 274         int i, s = info->rowskip;
 275
 276         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
 277
 278         // difference between frames in WCS
 279         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
 280         // now get weight factors depending on masses
 281         btScalar miA = rbAinvMass;
 282         btScalar miB = rbBinvMass;
 283         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 284         btScalar miS = miA + miB;
 285         btScalar factA, factB;
 286         if(miS > btScalar(0.f))
 287         {
 288                 factA = miB / miS;
 289         }
 290         else
 291         {
 292                 factA = btScalar(0.5f);
 293         }
 294         factB = btScalar(1.0f) - factA;
 295         btVector3 ax1, p, q;
 296         btVector3 ax1A = trA.getBasis().getColumn(0);
 297         btVector3 ax1B = trB.getBasis().getColumn(0);
 298         if(m_useOffsetForConstraintFrame)
 299         {
 300                 // get the desired direction of slider axis
 301                 // as weighted sum of X-orthos of frameA and frameB in WCS
 302                 ax1 = ax1A * factA + ax1B * factB;
 303                 ax1.normalize();
 304                 // construct two orthos to slider axis
 305                 btPlaneSpace1 (ax1, p, q);
 306         }
 307         else
 308         { // old way - use frameA
 309                 ax1 = trA.getBasis().getColumn(0);
 310                 // get 2 orthos to slider axis (Y, Z)
 311                 p = trA.getBasis().getColumn(1);
 312                 q = trA.getBasis().getColumn(2);
 313         }
 314         // make rotations around these orthos equal
 315         // the slider axis should be the only unconstrained
 316         // rotational axis, the angular velocity of the two bodies perpendicular to
 317         // the slider axis should be equal. thus the constraint equations are
 318         //    p*w1 - p*w2 = 0
 319         //    q*w1 - q*w2 = 0
 320         // where p and q are unit vectors normal to the slider axis, and w1 and w2
 321         // are the angular velocity vectors of the two bodies.
 322         info->m_J1angularAxis[0] = p[0];
 323         info->m_J1angularAxis[1] = p[1];
 324         info->m_J1angularAxis[2] = p[2];
 325         info->m_J1angularAxis[s+0] = q[0];
 326         info->m_J1angularAxis[s+1] = q[1];
 327         info->m_J1angularAxis[s+2] = q[2];
 328
 329         info->m_J2angularAxis[0] = -p[0];
 330         info->m_J2angularAxis[1] = -p[1];
 331         info->m_J2angularAxis[2] = -p[2];
 332         info->m_J2angularAxis[s+0] = -q[0];
 333         info->m_J2angularAxis[s+1] = -q[1];
 334         info->m_J2angularAxis[s+2] = -q[2];
 335         // compute the right hand side of the constraint equation. set relative
 336         // body velocities along p and q to bring the slider back into alignment.
 337         // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
 338         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
 339         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
 340         // along u to cover angle erp*theta in one step :
 341         //   |angular_velocity| = angle/time = erp*theta / stepsize
 342         //                      = (erp*fps) * theta
 343         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
 344         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
 345         // ...as ax1 and ax2 are unit length. if theta is smallish,
 346         // theta ~= sin(theta), so
 347         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
 348         // ax1 x ax2 is in the plane space of ax1, so we project the angular
 349         // velocity to p and q to find the right hand side.
 350 //      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
 351         btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
 352         btScalar k = info->fps * currERP;
 353
 354         btVector3 u = ax1A.cross(ax1B);
 355         info->m_constraintError[0] = k * u.dot(p);
 356         info->m_constraintError[s] = k * u.dot(q);
 357         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
 358         {
 359                 info->cfm[0] = m_cfmOrthoAng;
 360                 info->cfm[s] = m_cfmOrthoAng;
 361         }
 362
 363         int nrow = 1; // last filled row
 364         int srow;
 365         btScalar limit_err;
 366         int limit;
 367         int powered;
 368
 369         // next two rows.
 370         // we want: velA + wA x relA == velB + wB x relB ... but this would
 371         // result in three equations, so we project along two orthos to the slider axis
 372
 373         btTransform bodyA_trans = transA;
 374         btTransform bodyB_trans = transB;
 375         nrow++;
 376         int s2 = nrow * s;
 377         nrow++;
 378         int s3 = nrow * s;
 379         btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
 380         if(m_useOffsetForConstraintFrame)
 381         {
 382                 // get vector from bodyB to frameB in WCS
 383                 relB = trB.getOrigin() - bodyB_trans.getOrigin();
 384                 // get its projection to slider axis
 385                 btVector3 projB = ax1 * relB.dot(ax1);
 386                 // get vector directed from bodyB to slider axis (and orthogonal to it)
 387                 btVector3 orthoB = relB - projB;
 388                 // same for bodyA
 389                 relA = trA.getOrigin() - bodyA_trans.getOrigin();
 390                 btVector3 projA = ax1 * relA.dot(ax1);
 391                 btVector3 orthoA = relA - projA;
 392                 // get desired offset between frames A and B along slider axis
 393                 btScalar sliderOffs = m_linPos - m_depth[0];
 394                 // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
 395                 btVector3 totalDist = projA + ax1 * sliderOffs - projB;
 396                 // get offset vectors relA and relB
 397                 relA = orthoA + totalDist * factA;
 398                 relB = orthoB - totalDist * factB;
 399                 // now choose average ortho to slider axis
 400                 p = orthoB * factA + orthoA * factB;
 401                 btScalar len2 = p.length2();
 402                 if(len2 > SIMD_EPSILON)
 403                 {
 404                         p /= btSqrt(len2);
 405                 }
 406                 else
 407                 {
 408                         p = trA.getBasis().getColumn(1);
 409                 }
 410                 // make one more ortho
 411                 q = ax1.cross(p);
 412                 // fill two rows
 413                 tmpA = relA.cross(p);
 414                 tmpB = relB.cross(p);
 415                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
 416                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
 417                 tmpA = relA.cross(q);
 418                 tmpB = relB.cross(q);
 419                 if(hasStaticBody && getSolveAngLimit())
 420                 { // to make constraint between static and dynamic objects more rigid
 421                         // remove wA (or wB) from equation if angular limit is hit
 422                         tmpB *= factB;
 423                         tmpA *= factA;
 424                 }
 425                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
 426                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
 427                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
 428                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
 429         }
 430         else
 431         {       // old way - maybe incorrect if bodies are not on the slider axis
 432                 // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
 433                 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
 434                 btVector3 tmp = c.cross(p);
 435                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
 436                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
 437                 tmp = c.cross(q);
 438                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
 439                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
 440
 441                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
 442                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
 443         }
 444         // compute two elements of right hand side
 445
 446         //      k = info->fps * info->erp * getSoftnessOrthoLin();
 447         currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
 448         k = info->fps * currERP;
 449
 450         btScalar rhs = k * p.dot(ofs);
 451         info->m_constraintError[s2] = rhs;
 452         rhs = k * q.dot(ofs);
 453         info->m_constraintError[s3] = rhs;
 454         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
 455         {
 456                 info->cfm[s2] = m_cfmOrthoLin;
 457                 info->cfm[s3] = m_cfmOrthoLin;
 458         }
 459
 460
 461         // check linear limits
 462         limit_err = btScalar(0.0);
 463         limit = 0;
 464         if(getSolveLinLimit())
 465         {
 466                 limit_err = getLinDepth() *  signFact;
 467                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
 468         }
 469         powered = 0;
 470         if(getPoweredLinMotor())
 471         {
 472                 powered = 1;
 473         }
 474         // if the slider has joint limits or motor, add in the extra row
 475         if (limit || powered)
 476         {
 477                 nrow++;
 478                 srow = nrow * info->rowskip;
 479                 info->m_J1linearAxis[srow+0] = ax1[0];
 480                 info->m_J1linearAxis[srow+1] = ax1[1];
 481                 info->m_J1linearAxis[srow+2] = ax1[2];
 482                 // linear torque decoupling step:
 483                 //
 484                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
 485                 // do not create a torque couple. in other words, the points that the
 486                 // constraint force is applied at must lie along the same ax1 axis.
 487                 // a torque couple will result in limited slider-jointed free
 488                 // bodies from gaining angular momentum.
 489                 if(m_useOffsetForConstraintFrame)
 490                 {
 491                         // this is needed only when bodyA and bodyB are both dynamic.
 492                         if(!hasStaticBody)
 493                         {
 494                                 tmpA = relA.cross(ax1);
 495                                 tmpB = relB.cross(ax1);
 496                                 info->m_J1angularAxis[srow+0] = tmpA[0];
 497                                 info->m_J1angularAxis[srow+1] = tmpA[1];
 498                                 info->m_J1angularAxis[srow+2] = tmpA[2];
 499                                 info->m_J2angularAxis[srow+0] = -tmpB[0];
 500                                 info->m_J2angularAxis[srow+1] = -tmpB[1];
 501                                 info->m_J2angularAxis[srow+2] = -tmpB[2];
 502                         }
 503                 }
 504                 else
 505                 { // The old way. May be incorrect if bodies are not on the slider axis
 506                         btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
 507                         ltd = c.cross(ax1);
 508                         info->m_J1angularAxis[srow+0] = factA*ltd[0];
 509                         info->m_J1angularAxis[srow+1] = factA*ltd[1];
 510                         info->m_J1angularAxis[srow+2] = factA*ltd[2];
 511                         info->m_J2angularAxis[srow+0] = factB*ltd[0];
 512                         info->m_J2angularAxis[srow+1] = factB*ltd[1];
 513                         info->m_J2angularAxis[srow+2] = factB*ltd[2];
 514                 }
 515                 // right-hand part
 516                 btScalar lostop = getLowerLinLimit();
 517                 btScalar histop = getUpperLinLimit();
 518                 if(limit && (lostop == histop))
 519                 {  // the joint motor is ineffective
 520                         powered = 0;
 521                 }
 522                 info->m_constraintError[srow] = 0.;
 523                 info->m_lowerLimit[srow] = 0.;
 524                 info->m_upperLimit[srow] = 0.;
 525                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
 526                 if(powered)
 527                 {
 528                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
 529                         {
 530                                 info->cfm[srow] = m_cfmDirLin;
 531                         }
 532                         btScalar tag_vel = getTargetLinMotorVelocity();
 533                         btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
 534                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
 535                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
 536                         info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
 537                 }
 538                 if(limit)
 539                 {
 540                         k = info->fps * currERP;
 541                         info->m_constraintError[srow] += k * limit_err;
 542                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
 543                         {
 544                                 info->cfm[srow] = m_cfmLimLin;
 545                         }
 546                         if(lostop == histop)
 547                         {       // limited low and high simultaneously
 548                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 549                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 550                         }
 551                         else if(limit == 1)
 552                         { // low limit
 553                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 554                                 info->m_upperLimit[srow] = 0;
 555                         }
 556                         else
 557                         { // high limit
 558                                 info->m_lowerLimit[srow] = 0;
 559                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 560                         }
 561                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
 562                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
 563                         if(bounce > btScalar(0.0))
 564                         {
 565                                 btScalar vel = linVelA.dot(ax1);
 566                                 vel -= linVelB.dot(ax1);
 567                                 vel *= signFact;
 568                                 // only apply bounce if the velocity is incoming, and if the
 569                                 // resulting c[] exceeds what we already have.
 570                                 if(limit == 1)
 571                                 {       // low limit
 572                                         if(vel < 0)
 573                                         {
 574                                                 btScalar newc = -bounce * vel;
 575                                                 if (newc > info->m_constraintError[srow])
 576                                                 {
 577                                                         info->m_constraintError[srow] = newc;
 578                                                 }
 579                                         }
 580                                 }
 581                                 else
 582                                 { // high limit - all those computations are reversed
 583                                         if(vel > 0)
 584                                         {
 585                                                 btScalar newc = -bounce * vel;
 586                                                 if(newc < info->m_constraintError[srow])
 587                                                 {
 588                                                         info->m_constraintError[srow] = newc;
 589                                                 }
 590                                         }
 591                                 }
 592                         }
 593                         info->m_constraintError[srow] *= getSoftnessLimLin();
 594                 } // if(limit)
 595         } // if linear limit
 596         // check angular limits
 597         limit_err = btScalar(0.0);
 598         limit = 0;
 599         if(getSolveAngLimit())
 600         {
 601                 limit_err = getAngDepth();
 602                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 603         }
 604         // if the slider has joint limits, add in the extra row
 605         powered = 0;
 606         if(getPoweredAngMotor())
 607         {
 608                 powered = 1;
 609         }
 610         if(limit || powered)
 611         {
 612                 nrow++;
 613                 srow = nrow * info->rowskip;
 614                 info->m_J1angularAxis[srow+0] = ax1[0];
 615                 info->m_J1angularAxis[srow+1] = ax1[1];
 616                 info->m_J1angularAxis[srow+2] = ax1[2];
 617
 618                 info->m_J2angularAxis[srow+0] = -ax1[0];
 619                 info->m_J2angularAxis[srow+1] = -ax1[1];
 620                 info->m_J2angularAxis[srow+2] = -ax1[2];
 621
 622                 btScalar lostop = getLowerAngLimit();
 623                 btScalar histop = getUpperAngLimit();
 624                 if(limit && (lostop == histop))
 625                 {  // the joint motor is ineffective
 626                         powered = 0;
 627                 }
 628                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
 629                 if(powered)
 630                 {
 631                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
 632                         {
 633                                 info->cfm[srow] = m_cfmDirAng;
 634                         }
 635                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
 636                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
 637                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
 638                         info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
 639                 }
 640                 if(limit)
 641                 {
 642                         k = info->fps * currERP;
 643                         info->m_constraintError[srow] += k * limit_err;
 644                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
 645                         {
 646                                 info->cfm[srow] = m_cfmLimAng;
 647                         }
 648                         if(lostop == histop)
 649                         {
 650                                 // limited low and high simultaneously
 651                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 652                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 653                         }
 654                         else if(limit == 1)
 655                         { // low limit
 656                                 info->m_lowerLimit[srow] = 0;
 657                                 info->m_upperLimit[srow] = SIMD_INFINITY;
 658                         }
 659                         else
 660                         { // high limit
 661                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
 662                                 info->m_upperLimit[srow] = 0;
 663                         }
 664                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
 665                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
 666                         if(bounce > btScalar(0.0))
 667                         {
 668                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
 669                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
 670                                 // only apply bounce if the velocity is incoming, and if the
 671                                 // resulting c[] exceeds what we already have.
 672                                 if(limit == 1)
 673                                 {       // low limit
 674                                         if(vel < 0)
 675                                         {
 676                                                 btScalar newc = -bounce * vel;
 677                                                 if(newc > info->m_constraintError[srow])
 678                                                 {
 679                                                         info->m_constraintError[srow] = newc;
 680                                                 }
 681                                         }
 682                                 }
 683                                 else
 684                                 {       // high limit - all those computations are reversed
 685                                         if(vel > 0)
 686                                         {
 687                                                 btScalar newc = -bounce * vel;
 688                                                 if(newc < info->m_constraintError[srow])
 689                                                 {
 690                                                         info->m_constraintError[srow] = newc;
 691                                                 }
 692                                         }
 693                                 }
 694                         }
 695                         info->m_constraintError[srow] *= getSoftnessLimAng();
 696                 } // if(limit)
 697         } // if angular limit or powered
 698 }
 699
 700
 701 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 702 ///If no axis is provided, it uses the default axis for this constraint.
 703 void btSliderConstraint::setParam(int num, btScalar value, int axis)
 704 {
 705         switch(num)
 706         {
 707         case BT_CONSTRAINT_STOP_ERP :
 708                 if(axis < 1)
 709                 {
 710                         m_softnessLimLin = value;
 711                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
 712                 }
 713                 else if(axis < 3)
 714                 {
 715                         m_softnessOrthoLin = value;
 716                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
 717                 }
 718                 else if(axis == 3)
 719                 {
 720                         m_softnessLimAng = value;
 721                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
 722                 }
 723                 else if(axis < 6)
 724                 {
 725                         m_softnessOrthoAng = value;
 726                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
 727                 }
 728                 else
 729                 {
 730                         btAssertConstrParams(0);
 731                 }
 732                 break;
 733         case BT_CONSTRAINT_CFM :
 734                 if(axis < 1)
 735                 {
 736                         m_cfmDirLin = value;
 737                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
 738                 }
 739                 else if(axis == 3)
 740                 {
 741                         m_cfmDirAng = value;
 742                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
 743                 }
 744                 else
 745                 {
 746                         btAssertConstrParams(0);
 747                 }
 748                 break;
 749         case BT_CONSTRAINT_STOP_CFM :
 750                 if(axis < 1)
 751                 {
 752                         m_cfmLimLin = value;
 753                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
 754                 }
 755                 else if(axis < 3)
 756                 {
 757                         m_cfmOrthoLin = value;
 758                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
 759                 }
 760                 else if(axis == 3)
 761                 {
 762                         m_cfmLimAng = value;
 763                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
 764                 }
 765                 else if(axis < 6)
 766                 {
 767                         m_cfmOrthoAng = value;
 768                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
 769                 }
 770                 else
 771                 {
 772                         btAssertConstrParams(0);
 773                 }
 774                 break;
 775         }
 776 }
 777
 778 ///return the local value of parameter
 779 btScalar btSliderConstraint::getParam(int num, int axis) const
 780 {
 781         btScalar retVal(SIMD_INFINITY);
 782         switch(num)
 783         {
 784         case BT_CONSTRAINT_STOP_ERP :
 785                 if(axis < 1)
 786                 {
 787                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
 788                         retVal = m_softnessLimLin;
 789                 }
 790                 else if(axis < 3)
 791                 {
 792                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
 793                         retVal = m_softnessOrthoLin;
 794                 }
 795                 else if(axis == 3)
 796                 {
 797                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
 798                         retVal = m_softnessLimAng;
 799                 }
 800                 else if(axis < 6)
 801                 {
 802                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
 803                         retVal = m_softnessOrthoAng;
 804                 }
 805                 else
 806                 {
 807                         btAssertConstrParams(0);
 808                 }
 809                 break;
 810         case BT_CONSTRAINT_CFM :
 811                 if(axis < 1)
 812                 {
 813                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
 814                         retVal = m_cfmDirLin;
 815                 }
 816                 else if(axis == 3)
 817                 {
 818                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
 819                         retVal = m_cfmDirAng;
 820                 }
 821                 else
 822                 {
 823                         btAssertConstrParams(0);
 824                 }
 825                 break;
 826         case BT_CONSTRAINT_STOP_CFM :
 827                 if(axis < 1)
 828                 {
 829                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
 830                         retVal = m_cfmLimLin;
 831                 }
 832                 else if(axis < 3)
 833                 {
 834                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
 835                         retVal = m_cfmOrthoLin;
 836                 }
 837                 else if(axis == 3)
 838                 {
 839                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
 840                         retVal = m_cfmLimAng;
 841                 }
 842                 else if(axis < 6)
 843                 {
 844                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
 845                         retVal = m_cfmOrthoAng;
 846                 }
 847                 else
 848                 {
 849                         btAssertConstrParams(0);
 850                 }
 851                 break;
 852         }
 853         return retVal;
 854 }
 855
 856
 857