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