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 (at) 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 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; 430 for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; 431 } 432 else 433 { // old way - maybe incorrect if bodies are not on the slider axis 434 // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 435 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); 436 btVector3 tmp = c.cross(p); 437 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; 438 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; 439 tmp = c.cross(q); 440 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; 441 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; 442 443 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; 444 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; 445 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; 446 for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; 447 } 448 // compute two elements of right hand side 449 450 // k = info->fps * info->erp * getSoftnessOrthoLin(); 451 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; 452 k = info->fps * currERP; 453 454 btScalar rhs = k * p.dot(ofs); 455 info->m_constraintError[s2] = rhs; 456 rhs = k * q.dot(ofs); 457 info->m_constraintError[s3] = rhs; 458 if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) 459 { 460 info->cfm[s2] = m_cfmOrthoLin; 461 info->cfm[s3] = m_cfmOrthoLin; 462 } 463 464 465 // check linear limits 466 limit_err = btScalar(0.0); 467 limit = 0; 468 if(getSolveLinLimit()) 469 { 470 limit_err = getLinDepth() * signFact; 471 limit = (limit_err > btScalar(0.0)) ? 2 : 1; 472 } 473 powered = 0; 474 if(getPoweredLinMotor()) 475 { 476 powered = 1; 477 } 478 // if the slider has joint limits or motor, add in the extra row 479 if (limit || powered) 480 { 481 nrow++; 482 srow = nrow * info->rowskip; 483 info->m_J1linearAxis[srow+0] = ax1[0]; 484 info->m_J1linearAxis[srow+1] = ax1[1]; 485 info->m_J1linearAxis[srow+2] = ax1[2]; 486 info->m_J2linearAxis[srow+0] = -ax1[0]; 487 info->m_J2linearAxis[srow+1] = -ax1[1]; 488 info->m_J2linearAxis[srow+2] = -ax1[2]; 489 // linear torque decoupling step: 490 // 491 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies 492 // do not create a torque couple. in other words, the points that the 493 // constraint force is applied at must lie along the same ax1 axis. 494 // a torque couple will result in limited slider-jointed free 495 // bodies from gaining angular momentum. 496 if(m_useOffsetForConstraintFrame) 497 { 498 // this is needed only when bodyA and bodyB are both dynamic. 499 if(!hasStaticBody) 500 { 501 tmpA = relA.cross(ax1); 502 tmpB = relB.cross(ax1); 503 info->m_J1angularAxis[srow+0] = tmpA[0]; 504 info->m_J1angularAxis[srow+1] = tmpA[1]; 505 info->m_J1angularAxis[srow+2] = tmpA[2]; 506 info->m_J2angularAxis[srow+0] = -tmpB[0]; 507 info->m_J2angularAxis[srow+1] = -tmpB[1]; 508 info->m_J2angularAxis[srow+2] = -tmpB[2]; 509 } 510 } 511 else 512 { // The old way. May be incorrect if bodies are not on the slider axis 513 btVector3 ltd; // Linear Torque Decoupling vector (a torque) 514 ltd = c.cross(ax1); 515 info->m_J1angularAxis[srow+0] = factA*ltd[0]; 516 info->m_J1angularAxis[srow+1] = factA*ltd[1]; 517 info->m_J1angularAxis[srow+2] = factA*ltd[2]; 518 info->m_J2angularAxis[srow+0] = factB*ltd[0]; 519 info->m_J2angularAxis[srow+1] = factB*ltd[1]; 520 info->m_J2angularAxis[srow+2] = factB*ltd[2]; 521 } 522 // right-hand part 523 btScalar lostop = getLowerLinLimit(); 524 btScalar histop = getUpperLinLimit(); 525 if(limit && (lostop == histop)) 526 { // the joint motor is ineffective 527 powered = 0; 528 } 529 info->m_constraintError[srow] = 0.; 530 info->m_lowerLimit[srow] = 0.; 531 info->m_upperLimit[srow] = 0.; 532 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; 533 if(powered) 534 { 535 if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) 536 { 537 info->cfm[srow] = m_cfmDirLin; 538 } 539 btScalar tag_vel = getTargetLinMotorVelocity(); 540 btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); 541 info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); 542 info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps; 543 info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps; 544 } 545 if(limit) 546 { 547 k = info->fps * currERP; 548 info->m_constraintError[srow] += k * limit_err; 549 if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) 550 { 551 info->cfm[srow] = m_cfmLimLin; 552 } 553 if(lostop == histop) 554 { // limited low and high simultaneously 555 info->m_lowerLimit[srow] = -SIMD_INFINITY; 556 info->m_upperLimit[srow] = SIMD_INFINITY; 557 } 558 else if(limit == 1) 559 { // low limit 560 info->m_lowerLimit[srow] = -SIMD_INFINITY; 561 info->m_upperLimit[srow] = 0; 562 } 563 else 564 { // high limit 565 info->m_lowerLimit[srow] = 0; 566 info->m_upperLimit[srow] = SIMD_INFINITY; 567 } 568 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) 569 btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); 570 if(bounce > btScalar(0.0)) 571 { 572 btScalar vel = linVelA.dot(ax1); 573 vel -= linVelB.dot(ax1); 574 vel *= signFact; 575 // only apply bounce if the velocity is incoming, and if the 576 // resulting c[] exceeds what we already have. 577 if(limit == 1) 578 { // low limit 579 if(vel < 0) 580 { 581 btScalar newc = -bounce * vel; 582 if (newc > info->m_constraintError[srow]) 583 { 584 info->m_constraintError[srow] = newc; 585 } 586 } 587 } 588 else 589 { // high limit - all those computations are reversed 590 if(vel > 0) 591 { 592 btScalar newc = -bounce * vel; 593 if(newc < info->m_constraintError[srow]) 594 { 595 info->m_constraintError[srow] = newc; 596 } 597 } 598 } 599 } 600 info->m_constraintError[srow] *= getSoftnessLimLin(); 601 } // if(limit) 602 } // if linear limit 603 // check angular limits 604 limit_err = btScalar(0.0); 605 limit = 0; 606 if(getSolveAngLimit()) 607 { 608 limit_err = getAngDepth(); 609 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 610 } 611 // if the slider has joint limits, add in the extra row 612 powered = 0; 613 if(getPoweredAngMotor()) 614 { 615 powered = 1; 616 } 617 if(limit || powered) 618 { 619 nrow++; 620 srow = nrow * info->rowskip; 621 info->m_J1angularAxis[srow+0] = ax1[0]; 622 info->m_J1angularAxis[srow+1] = ax1[1]; 623 info->m_J1angularAxis[srow+2] = ax1[2]; 624 625 info->m_J2angularAxis[srow+0] = -ax1[0]; 626 info->m_J2angularAxis[srow+1] = -ax1[1]; 627 info->m_J2angularAxis[srow+2] = -ax1[2]; 628 629 btScalar lostop = getLowerAngLimit(); 630 btScalar histop = getUpperAngLimit(); 631 if(limit && (lostop == histop)) 632 { // the joint motor is ineffective 633 powered = 0; 634 } 635 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; 636 if(powered) 637 { 638 if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) 639 { 640 info->cfm[srow] = m_cfmDirAng; 641 } 642 btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); 643 info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); 644 info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps; 645 info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps; 646 } 647 if(limit) 648 { 649 k = info->fps * currERP; 650 info->m_constraintError[srow] += k * limit_err; 651 if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) 652 { 653 info->cfm[srow] = m_cfmLimAng; 654 } 655 if(lostop == histop) 656 { 657 // limited low and high simultaneously 658 info->m_lowerLimit[srow] = -SIMD_INFINITY; 659 info->m_upperLimit[srow] = SIMD_INFINITY; 660 } 661 else if(limit == 1) 662 { // low limit 663 info->m_lowerLimit[srow] = 0; 664 info->m_upperLimit[srow] = SIMD_INFINITY; 665 } 666 else 667 { // high limit 668 info->m_lowerLimit[srow] = -SIMD_INFINITY; 669 info->m_upperLimit[srow] = 0; 670 } 671 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 672 btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); 673 if(bounce > btScalar(0.0)) 674 { 675 btScalar vel = m_rbA.getAngularVelocity().dot(ax1); 676 vel -= m_rbB.getAngularVelocity().dot(ax1); 677 // only apply bounce if the velocity is incoming, and if the 678 // resulting c[] exceeds what we already have. 679 if(limit == 1) 680 { // low limit 681 if(vel < 0) 682 { 683 btScalar newc = -bounce * vel; 684 if(newc > info->m_constraintError[srow]) 685 { 686 info->m_constraintError[srow] = newc; 687 } 688 } 689 } 690 else 691 { // high limit - all those computations are reversed 692 if(vel > 0) 693 { 694 btScalar newc = -bounce * vel; 695 if(newc < info->m_constraintError[srow]) 696 { 697 info->m_constraintError[srow] = newc; 698 } 699 } 700 } 701 } 702 info->m_constraintError[srow] *= getSoftnessLimAng(); 703 } // if(limit) 704 } // if angular limit or powered 705 } 706 707 708 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 709 ///If no axis is provided, it uses the default axis for this constraint. 710 void btSliderConstraint::setParam(int num, btScalar value, int axis) 711 { 712 switch(num) 713 { 714 case BT_CONSTRAINT_STOP_ERP : 715 if(axis < 1) 716 { 717 m_softnessLimLin = value; 718 m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; 719 } 720 else if(axis < 3) 721 { 722 m_softnessOrthoLin = value; 723 m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; 724 } 725 else if(axis == 3) 726 { 727 m_softnessLimAng = value; 728 m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; 729 } 730 else if(axis < 6) 731 { 732 m_softnessOrthoAng = value; 733 m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; 734 } 735 else 736 { 737 btAssertConstrParams(0); 738 } 739 break; 740 case BT_CONSTRAINT_CFM : 741 if(axis < 1) 742 { 743 m_cfmDirLin = value; 744 m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; 745 } 746 else if(axis == 3) 747 { 748 m_cfmDirAng = value; 749 m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; 750 } 751 else 752 { 753 btAssertConstrParams(0); 754 } 755 break; 756 case BT_CONSTRAINT_STOP_CFM : 757 if(axis < 1) 758 { 759 m_cfmLimLin = value; 760 m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; 761 } 762 else if(axis < 3) 763 { 764 m_cfmOrthoLin = value; 765 m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; 766 } 767 else if(axis == 3) 768 { 769 m_cfmLimAng = value; 770 m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; 771 } 772 else if(axis < 6) 773 { 774 m_cfmOrthoAng = value; 775 m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; 776 } 777 else 778 { 779 btAssertConstrParams(0); 780 } 781 break; 782 } 783 } 784 785 ///return the local value of parameter 786 btScalar btSliderConstraint::getParam(int num, int axis) const 787 { 788 btScalar retVal(SIMD_INFINITY); 789 switch(num) 790 { 791 case BT_CONSTRAINT_STOP_ERP : 792 if(axis < 1) 793 { 794 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); 795 retVal = m_softnessLimLin; 796 } 797 else if(axis < 3) 798 { 799 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); 800 retVal = m_softnessOrthoLin; 801 } 802 else if(axis == 3) 803 { 804 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); 805 retVal = m_softnessLimAng; 806 } 807 else if(axis < 6) 808 { 809 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); 810 retVal = m_softnessOrthoAng; 811 } 812 else 813 { 814 btAssertConstrParams(0); 815 } 816 break; 817 case BT_CONSTRAINT_CFM : 818 if(axis < 1) 819 { 820 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); 821 retVal = m_cfmDirLin; 822 } 823 else if(axis == 3) 824 { 825 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); 826 retVal = m_cfmDirAng; 827 } 828 else 829 { 830 btAssertConstrParams(0); 831 } 832 break; 833 case BT_CONSTRAINT_STOP_CFM : 834 if(axis < 1) 835 { 836 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); 837 retVal = m_cfmLimLin; 838 } 839 else if(axis < 3) 840 { 841 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); 842 retVal = m_cfmOrthoLin; 843 } 844 else if(axis == 3) 845 { 846 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); 847 retVal = m_cfmLimAng; 848 } 849 else if(axis < 6) 850 { 851 btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); 852 retVal = m_cfmOrthoAng; 853 } 854 else 855 { 856 btAssertConstrParams(0); 857 } 858 break; 859 } 860 return retVal; 861 } 862 863 864 865
