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