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 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev 17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods 18 19 /* 20 2007-09-09 21 btGeneric6DofConstraint Refactored by Francisco Le?n 22 email: projectileman (at) yahoo.com 23 http://gimpact.sf.net 24 */ 25 26 27 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H 28 #define BT_GENERIC_6DOF_CONSTRAINT_H 29 30 #include "LinearMath/btVector3.h" 31 #include "btJacobianEntry.h" 32 #include "btTypedConstraint.h" 33 34 class btRigidBody; 35 36 37 38 #ifdef BT_USE_DOUBLE_PRECISION 39 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 40 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" 41 #else 42 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintData 43 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData" 44 #endif //BT_USE_DOUBLE_PRECISION 45 46 47 //! Rotation Limit structure for generic joints 48 class btRotationalLimitMotor 49 { 50 public: 51 //! limit_parameters 52 //!@{ 53 btScalar m_loLimit;//!< joint limit 54 btScalar m_hiLimit;//!< joint limit 55 btScalar m_targetVelocity;//!< target motor velocity 56 btScalar m_maxMotorForce;//!< max force on motor 57 btScalar m_maxLimitForce;//!< max force on limit 58 btScalar m_damping;//!< Damping. 59 btScalar m_limitSoftness;//! Relaxation factor 60 btScalar m_normalCFM;//!< Constraint force mixing factor 61 btScalar m_stopERP;//!< Error tolerance factor when joint is at limit 62 btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit 63 btScalar m_bounce;//!< restitution factor 64 bool m_enableMotor; 65 66 //!@} 67 68 //! temp_variables 69 //!@{ 70 btScalar m_currentLimitError;//! How much is violated this limit 71 btScalar m_currentPosition; //! current value of angle 72 int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit 73 btScalar m_accumulatedImpulse; 74 //!@} 75 76 btRotationalLimitMotor() 77 { 78 m_accumulatedImpulse = 0.f; 79 m_targetVelocity = 0; 80 m_maxMotorForce = 0.1f; 81 m_maxLimitForce = 300.0f; 82 m_loLimit = 1.0f; 83 m_hiLimit = -1.0f; 84 m_normalCFM = 0.f; 85 m_stopERP = 0.2f; 86 m_stopCFM = 0.f; 87 m_bounce = 0.0f; 88 m_damping = 1.0f; 89 m_limitSoftness = 0.5f; 90 m_currentLimit = 0; 91 m_currentLimitError = 0; 92 m_enableMotor = false; 93 } 94 95 btRotationalLimitMotor(const btRotationalLimitMotor & limot) 96 { 97 m_targetVelocity = limot.m_targetVelocity; 98 m_maxMotorForce = limot.m_maxMotorForce; 99 m_limitSoftness = limot.m_limitSoftness; 100 m_loLimit = limot.m_loLimit; 101 m_hiLimit = limot.m_hiLimit; 102 m_normalCFM = limot.m_normalCFM; 103 m_stopERP = limot.m_stopERP; 104 m_stopCFM = limot.m_stopCFM; 105 m_bounce = limot.m_bounce; 106 m_currentLimit = limot.m_currentLimit; 107 m_currentLimitError = limot.m_currentLimitError; 108 m_enableMotor = limot.m_enableMotor; 109 } 110 111 112 113 //! Is limited 114 bool isLimited() 115 { 116 if(m_loLimit > m_hiLimit) return false; 117 return true; 118 } 119 120 //! Need apply correction 121 bool needApplyTorques() 122 { 123 if(m_currentLimit == 0 && m_enableMotor == false) return false; 124 return true; 125 } 126 127 //! calculates error 128 /*! 129 calculates m_currentLimit and m_currentLimitError. 130 */ 131 int testLimitValue(btScalar test_value); 132 133 //! apply the correction impulses for two bodies 134 btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); 135 136 }; 137 138 139 140 class btTranslationalLimitMotor 141 { 142 public: 143 btVector3 m_lowerLimit;//!< the constraint lower limits 144 btVector3 m_upperLimit;//!< the constraint upper limits 145 btVector3 m_accumulatedImpulse; 146 //! Linear_Limit_parameters 147 //!@{ 148 btScalar m_limitSoftness;//!< Softness for linear limit 149 btScalar m_damping;//!< Damping for linear limit 150 btScalar m_restitution;//! Bounce parameter for linear limit 151 btVector3 m_normalCFM;//!< Constraint force mixing factor 152 btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit 153 btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit 154 //!@} 155 bool m_enableMotor[3]; 156 btVector3 m_targetVelocity;//!< target motor velocity 157 btVector3 m_maxMotorForce;//!< max force on motor 158 btVector3 m_currentLimitError;//! How much is violated this limit 159 btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames 160 int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit 161 162 btTranslationalLimitMotor() 163 { 164 m_lowerLimit.setValue(0.f,0.f,0.f); 165 m_upperLimit.setValue(0.f,0.f,0.f); 166 m_accumulatedImpulse.setValue(0.f,0.f,0.f); 167 m_normalCFM.setValue(0.f, 0.f, 0.f); 168 m_stopERP.setValue(0.2f, 0.2f, 0.2f); 169 m_stopCFM.setValue(0.f, 0.f, 0.f); 170 171 m_limitSoftness = 0.7f; 172 m_damping = btScalar(1.0f); 173 m_restitution = btScalar(0.5f); 174 for(int i=0; i < 3; i++) 175 { 176 m_enableMotor[i] = false; 177 m_targetVelocity[i] = btScalar(0.f); 178 m_maxMotorForce[i] = btScalar(0.f); 179 } 180 } 181 182 btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) 183 { 184 m_lowerLimit = other.m_lowerLimit; 185 m_upperLimit = other.m_upperLimit; 186 m_accumulatedImpulse = other.m_accumulatedImpulse; 187 188 m_limitSoftness = other.m_limitSoftness ; 189 m_damping = other.m_damping; 190 m_restitution = other.m_restitution; 191 m_normalCFM = other.m_normalCFM; 192 m_stopERP = other.m_stopERP; 193 m_stopCFM = other.m_stopCFM; 194 195 for(int i=0; i < 3; i++) 196 { 197 m_enableMotor[i] = other.m_enableMotor[i]; 198 m_targetVelocity[i] = other.m_targetVelocity[i]; 199 m_maxMotorForce[i] = other.m_maxMotorForce[i]; 200 } 201 } 202 203 //! Test limit 204 /*! 205 - free means upper < lower, 206 - locked means upper == lower 207 - limited means upper > lower 208 - limitIndex: first 3 are linear, next 3 are angular 209 */ 210 inline bool isLimited(int limitIndex) 211 { 212 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); 213 } 214 inline bool needApplyForce(int limitIndex) 215 { 216 if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; 217 return true; 218 } 219 int testLimitValue(int limitIndex, btScalar test_value); 220 221 222 btScalar solveLinearAxis( 223 btScalar timeStep, 224 btScalar jacDiagABInv, 225 btRigidBody& body1,const btVector3 &pointInA, 226 btRigidBody& body2,const btVector3 &pointInB, 227 int limit_index, 228 const btVector3 & axis_normal_on_a, 229 const btVector3 & anchorPos); 230 231 232 }; 233 234 enum bt6DofFlags 235 { 236 BT_6DOF_FLAGS_CFM_NORM = 1, 237 BT_6DOF_FLAGS_CFM_STOP = 2, 238 BT_6DOF_FLAGS_ERP_STOP = 4 239 }; 240 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis 241 242 243 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space 244 /*! 245 btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. 246 currently this limit supports rotational motors<br> 247 <ul> 248 <li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. 249 At this moment translational motors are not supported. May be in the future. </li> 250 251 <li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. 252 This is accessible through btGeneric6DofConstraint.getLimitMotor method, 253 This brings support for limit parameters and motors. </li> 254 255 <li> Angulars limits have these possible ranges: 256 <table border=1 > 257 <tr> 258 <td><b>AXIS</b></td> 259 <td><b>MIN ANGLE</b></td> 260 <td><b>MAX ANGLE</b></td> 261 </tr><tr> 262 <td>X</td> 263 <td>-PI</td> 264 <td>PI</td> 265 </tr><tr> 266 <td>Y</td> 267 <td>-PI/2</td> 268 <td>PI/2</td> 269 </tr><tr> 270 <td>Z</td> 271 <td>-PI</td> 272 <td>PI</td> 273 </tr> 274 </table> 275 </li> 276 </ul> 277 278 */ 279 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint 280 { 281 protected: 282 283 //! relative_frames 284 //!@{ 285 btTransform m_frameInA;//!< the constraint space w.r.t body A 286 btTransform m_frameInB;//!< the constraint space w.r.t body B 287 //!@} 288 289 //! Jacobians 290 //!@{ 291 btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints 292 btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints 293 //!@} 294 295 //! Linear_Limit_parameters 296 //!@{ 297 btTranslationalLimitMotor m_linearLimits; 298 //!@} 299 300 301 //! hinge_parameters 302 //!@{ 303 btRotationalLimitMotor m_angularLimits[3]; 304 //!@} 305 306 307 protected: 308 //! temporal variables 309 //!@{ 310 btScalar m_timeStep; 311 btTransform m_calculatedTransformA; 312 btTransform m_calculatedTransformB; 313 btVector3 m_calculatedAxisAngleDiff; 314 btVector3 m_calculatedAxis[3]; 315 btVector3 m_calculatedLinearDiff; 316 btScalar m_factA; 317 btScalar m_factB; 318 bool m_hasStaticBody; 319 320 btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes 321 322 bool m_useLinearReferenceFrameA; 323 bool m_useOffsetForConstraintFrame; 324 325 int m_flags; 326 327 //!@} 328 329 btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) 330 { 331 btAssert(0); 332 (void) other; 333 return *this; 334 } 335 336 337 int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 338 339 int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 340 341 void buildLinearJacobian( 342 btJacobianEntry & jacLinear,const btVector3 & normalWorld, 343 const btVector3 & pivotAInW,const btVector3 & pivotBInW); 344 345 void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); 346 347 // tests linear limits 348 void calculateLinearInfo(); 349 350 //! calcs the euler angles between the two bodies. 351 void calculateAngleInfo(); 352 353 354 355 public: 356 357 BT_DECLARE_ALIGNED_ALLOCATOR(); 358 359 ///for backwards compatibility during the transition to 'getInfo/getInfo2' 360 bool m_useSolveConstraintObsolete; 361 362 btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); 363 btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); 364 365 //! Calcs global transform of the offsets 366 /*! 367 Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. 368 \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo 369 */ 370 void calculateTransforms(const btTransform& transA,const btTransform& transB); 371 372 void calculateTransforms(); 373 374 //! Gets the global transform of the offset for body A 375 /*! 376 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. 377 */ 378 const btTransform & getCalculatedTransformA() const 379 { 380 return m_calculatedTransformA; 381 } 382 383 //! Gets the global transform of the offset for body B 384 /*! 385 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. 386 */ 387 const btTransform & getCalculatedTransformB() const 388 { 389 return m_calculatedTransformB; 390 } 391 392 const btTransform & getFrameOffsetA() const 393 { 394 return m_frameInA; 395 } 396 397 const btTransform & getFrameOffsetB() const 398 { 399 return m_frameInB; 400 } 401 402 403 btTransform & getFrameOffsetA() 404 { 405 return m_frameInA; 406 } 407 408 btTransform & getFrameOffsetB() 409 { 410 return m_frameInB; 411 } 412 413 414 //! performs Jacobian calculation, and also calculates angle differences and axis 415 virtual void buildJacobian(); 416 417 virtual void getInfo1 (btConstraintInfo1* info); 418 419 void getInfo1NonVirtual (btConstraintInfo1* info); 420 421 virtual void getInfo2 (btConstraintInfo2* info); 422 423 void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 424 425 426 void updateRHS(btScalar timeStep); 427 428 //! Get the rotation axis in global coordinates 429 /*! 430 \pre btGeneric6DofConstraint.buildJacobian must be called previously. 431 */ 432 btVector3 getAxis(int axis_index) const; 433 434 //! Get the relative Euler angle 435 /*! 436 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 437 */ 438 btScalar getAngle(int axis_index) const; 439 440 //! Get the relative position of the constraint pivot 441 /*! 442 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 443 */ 444 btScalar getRelativePivotPosition(int axis_index) const; 445 446 void setFrames(const btTransform & frameA, const btTransform & frameB); 447 448 //! Test angular limit. 449 /*! 450 Calculates angular correction and returns true if limit needs to be corrected. 451 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 452 */ 453 bool testAngularLimitMotor(int axis_index); 454 455 void setLinearLowerLimit(const btVector3& linearLower) 456 { 457 m_linearLimits.m_lowerLimit = linearLower; 458 } 459 460 void getLinearLowerLimit(btVector3& linearLower) 461 { 462 linearLower = m_linearLimits.m_lowerLimit; 463 } 464 465 void setLinearUpperLimit(const btVector3& linearUpper) 466 { 467 m_linearLimits.m_upperLimit = linearUpper; 468 } 469 470 void getLinearUpperLimit(btVector3& linearUpper) 471 { 472 linearUpper = m_linearLimits.m_upperLimit; 473 } 474 475 void setAngularLowerLimit(const btVector3& angularLower) 476 { 477 for(int i = 0; i < 3; i++) 478 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); 479 } 480 481 void getAngularLowerLimit(btVector3& angularLower) 482 { 483 for(int i = 0; i < 3; i++) 484 angularLower[i] = m_angularLimits[i].m_loLimit; 485 } 486 487 void setAngularUpperLimit(const btVector3& angularUpper) 488 { 489 for(int i = 0; i < 3; i++) 490 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); 491 } 492 493 void getAngularUpperLimit(btVector3& angularUpper) 494 { 495 for(int i = 0; i < 3; i++) 496 angularUpper[i] = m_angularLimits[i].m_hiLimit; 497 } 498 499 //! Retrieves the angular limit informacion 500 btRotationalLimitMotor * getRotationalLimitMotor(int index) 501 { 502 return &m_angularLimits[index]; 503 } 504 505 //! Retrieves the limit informacion 506 btTranslationalLimitMotor * getTranslationalLimitMotor() 507 { 508 return &m_linearLimits; 509 } 510 511 //first 3 are linear, next 3 are angular 512 void setLimit(int axis, btScalar lo, btScalar hi) 513 { 514 if(axis<3) 515 { 516 m_linearLimits.m_lowerLimit[axis] = lo; 517 m_linearLimits.m_upperLimit[axis] = hi; 518 } 519 else 520 { 521 lo = btNormalizeAngle(lo); 522 hi = btNormalizeAngle(hi); 523 m_angularLimits[axis-3].m_loLimit = lo; 524 m_angularLimits[axis-3].m_hiLimit = hi; 525 } 526 } 527 528 //! Test limit 529 /*! 530 - free means upper < lower, 531 - locked means upper == lower 532 - limited means upper > lower 533 - limitIndex: first 3 are linear, next 3 are angular 534 */ 535 bool isLimited(int limitIndex) 536 { 537 if(limitIndex<3) 538 { 539 return m_linearLimits.isLimited(limitIndex); 540 541 } 542 return m_angularLimits[limitIndex-3].isLimited(); 543 } 544 545 virtual void calcAnchorPos(void); // overridable 546 547 int get_limit_motor_info2( btRotationalLimitMotor * limot, 548 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, 549 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); 550 551 // access for UseFrameOffset 552 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } 553 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } 554 555 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 556 ///If no axis is provided, it uses the default axis for this constraint. 557 virtual void setParam(int num, btScalar value, int axis = -1); 558 ///return the local value of parameter 559 virtual btScalar getParam(int num, int axis = -1) const; 560 561 void setAxis( const btVector3& axis1, const btVector3& axis2); 562 563 564 virtual int calculateSerializeBufferSize() const; 565 566 ///fills the dataBuffer and returns the struct name (and 0 on failure) 567 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; 568 569 570 }; 571 572 573 struct btGeneric6DofConstraintData 574 { 575 btTypedConstraintData m_typeConstraintData; 576 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 577 btTransformFloatData m_rbBFrame; 578 579 btVector3FloatData m_linearUpperLimit; 580 btVector3FloatData m_linearLowerLimit; 581 582 btVector3FloatData m_angularUpperLimit; 583 btVector3FloatData m_angularLowerLimit; 584 585 int m_useLinearReferenceFrameA; 586 int m_useOffsetForConstraintFrame; 587 }; 588 589 struct btGeneric6DofConstraintDoubleData2 590 { 591 btTypedConstraintDoubleData m_typeConstraintData; 592 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 593 btTransformDoubleData m_rbBFrame; 594 595 btVector3DoubleData m_linearUpperLimit; 596 btVector3DoubleData m_linearLowerLimit; 597 598 btVector3DoubleData m_angularUpperLimit; 599 btVector3DoubleData m_angularLowerLimit; 600 601 int m_useLinearReferenceFrameA; 602 int m_useOffsetForConstraintFrame; 603 }; 604 605 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const 606 { 607 return sizeof(btGeneric6DofConstraintData2); 608 } 609 610 ///fills the dataBuffer and returns the struct name (and 0 on failure) 611 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const 612 { 613 614 btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer; 615 btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); 616 617 m_frameInA.serialize(dof->m_rbAFrame); 618 m_frameInB.serialize(dof->m_rbBFrame); 619 620 621 int i; 622 for (i=0;i<3;i++) 623 { 624 dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; 625 dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; 626 dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i]; 627 dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i]; 628 } 629 630 dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; 631 dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; 632 633 return btGeneric6DofConstraintDataName; 634 } 635 636 637 638 639 640 #endif //BT_GENERIC_6DOF_CONSTRAINT_H 641