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 /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ 17 18 #ifndef BT_HINGECONSTRAINT_H 19 #define BT_HINGECONSTRAINT_H 20 21 #define _BT_USE_CENTER_LIMIT_ 1 22 23 24 #include "LinearMath/btVector3.h" 25 #include "btJacobianEntry.h" 26 #include "btTypedConstraint.h" 27 28 class btRigidBody; 29 30 #ifdef BT_USE_DOUBLE_PRECISION 31 #define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version 32 #define btHingeConstraintDataName "btHingeConstraintDoubleData2" 33 #else 34 #define btHingeConstraintData btHingeConstraintFloatData 35 #define btHingeConstraintDataName "btHingeConstraintFloatData" 36 #endif //BT_USE_DOUBLE_PRECISION 37 38 39 40 enum btHingeFlags 41 { 42 BT_HINGE_FLAGS_CFM_STOP = 1, 43 BT_HINGE_FLAGS_ERP_STOP = 2, 44 BT_HINGE_FLAGS_CFM_NORM = 4, 45 BT_HINGE_FLAGS_ERP_NORM = 8 46 }; 47 48 49 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space 50 /// axis defines the orientation of the hinge axis 51 ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint 52 { 53 #ifdef IN_PARALLELL_SOLVER 54 public: 55 #endif 56 btJacobianEntry m_jac[3]; //3 orthogonal linear constraints 57 btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor 58 59 btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. 60 btTransform m_rbBFrame; 61 62 btScalar m_motorTargetVelocity; 63 btScalar m_maxMotorImpulse; 64 65 66 #ifdef _BT_USE_CENTER_LIMIT_ 67 btAngularLimit m_limit; 68 #else 69 btScalar m_lowerLimit; 70 btScalar m_upperLimit; 71 btScalar m_limitSign; 72 btScalar m_correction; 73 74 btScalar m_limitSoftness; 75 btScalar m_biasFactor; 76 btScalar m_relaxationFactor; 77 78 bool m_solveLimit; 79 #endif 80 81 btScalar m_kHinge; 82 83 84 btScalar m_accLimitImpulse; 85 btScalar m_hingeAngle; 86 btScalar m_referenceSign; 87 88 bool m_angularOnly; 89 bool m_enableAngularMotor; 90 bool m_useSolveConstraintObsolete; 91 bool m_useOffsetForConstraintFrame; 92 bool m_useReferenceFrameA; 93 94 btScalar m_accMotorImpulse; 95 96 int m_flags; 97 btScalar m_normalCFM; 98 btScalar m_normalERP; 99 btScalar m_stopCFM; 100 btScalar m_stopERP; 101 102 103 public: 104 105 BT_DECLARE_ALIGNED_ALLOCATOR(); 106 107 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false); 108 109 btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false); 110 111 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); 112 113 btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); 114 115 116 virtual void buildJacobian(); 117 118 virtual void getInfo1 (btConstraintInfo1* info); 119 120 void getInfo1NonVirtual(btConstraintInfo1* info); 121 122 virtual void getInfo2 (btConstraintInfo2* info); 123 124 void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); 125 126 void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); 127 void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); 128 129 130 void updateRHS(btScalar timeStep); 131 132 const btRigidBody& getRigidBodyA() const 133 { 134 return m_rbA; 135 } 136 const btRigidBody& getRigidBodyB() const 137 { 138 return m_rbB; 139 } 140 141 btRigidBody& getRigidBodyA() 142 { 143 return m_rbA; 144 } 145 146 btRigidBody& getRigidBodyB() 147 { 148 return m_rbB; 149 } 150 151 btTransform& getFrameOffsetA() 152 { 153 return m_rbAFrame; 154 } 155 156 btTransform& getFrameOffsetB() 157 { 158 return m_rbBFrame; 159 } 160 161 void setFrames(const btTransform& frameA, const btTransform& frameB); 162 163 void setAngularOnly(bool angularOnly) 164 { 165 m_angularOnly = angularOnly; 166 } 167 168 void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) 169 { 170 m_enableAngularMotor = enableMotor; 171 m_motorTargetVelocity = targetVelocity; 172 m_maxMotorImpulse = maxMotorImpulse; 173 } 174 175 // extra motor API, including ability to set a target rotation (as opposed to angular velocity) 176 // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to 177 // maintain a given angular target. 178 void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } 179 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } 180 void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. 181 void setMotorTarget(btScalar targetAngle, btScalar dt); 182 183 184 void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) 185 { 186 #ifdef _BT_USE_CENTER_LIMIT_ 187 m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor); 188 #else 189 m_lowerLimit = btNormalizeAngle(low); 190 m_upperLimit = btNormalizeAngle(high); 191 m_limitSoftness = _softness; 192 m_biasFactor = _biasFactor; 193 m_relaxationFactor = _relaxationFactor; 194 #endif 195 } 196 197 void setAxis(btVector3& axisInA) 198 { 199 btVector3 rbAxisA1, rbAxisA2; 200 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); 201 btVector3 pivotInA = m_rbAFrame.getOrigin(); 202 // m_rbAFrame.getOrigin() = pivotInA; 203 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), 204 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), 205 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 206 207 btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; 208 209 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); 210 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); 211 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); 212 213 m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA)); 214 215 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 216 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 217 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 218 m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis(); 219 220 } 221 222 bool hasLimit() const { 223 #ifdef _BT_USE_CENTER_LIMIT_ 224 return m_limit.getHalfRange() > 0; 225 #else 226 return m_lowerLimit <= m_upperLimit; 227 #endif 228 } 229 230 btScalar getLowerLimit() const 231 { 232 #ifdef _BT_USE_CENTER_LIMIT_ 233 return m_limit.getLow(); 234 #else 235 return m_lowerLimit; 236 #endif 237 } 238 239 btScalar getUpperLimit() const 240 { 241 #ifdef _BT_USE_CENTER_LIMIT_ 242 return m_limit.getHigh(); 243 #else 244 return m_upperLimit; 245 #endif 246 } 247 248 249 ///The getHingeAngle gives the hinge angle in range [-PI,PI] 250 btScalar getHingeAngle(); 251 252 btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); 253 254 void testLimit(const btTransform& transA,const btTransform& transB); 255 256 257 const btTransform& getAFrame() const { return m_rbAFrame; }; 258 const btTransform& getBFrame() const { return m_rbBFrame; }; 259 260 btTransform& getAFrame() { return m_rbAFrame; }; 261 btTransform& getBFrame() { return m_rbBFrame; }; 262 263 inline int getSolveLimit() 264 { 265 #ifdef _BT_USE_CENTER_LIMIT_ 266 return m_limit.isLimit(); 267 #else 268 return m_solveLimit; 269 #endif 270 } 271 272 inline btScalar getLimitSign() 273 { 274 #ifdef _BT_USE_CENTER_LIMIT_ 275 return m_limit.getSign(); 276 #else 277 return m_limitSign; 278 #endif 279 } 280 281 inline bool getAngularOnly() 282 { 283 return m_angularOnly; 284 } 285 inline bool getEnableAngularMotor() 286 { 287 return m_enableAngularMotor; 288 } 289 inline btScalar getMotorTargetVelosity() 290 { 291 return m_motorTargetVelocity; 292 } 293 inline btScalar getMaxMotorImpulse() 294 { 295 return m_maxMotorImpulse; 296 } 297 // access for UseFrameOffset 298 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } 299 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } 300 301 302 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 303 ///If no axis is provided, it uses the default axis for this constraint. 304 virtual void setParam(int num, btScalar value, int axis = -1); 305 ///return the local value of parameter 306 virtual btScalar getParam(int num, int axis = -1) const; 307 308 virtual int calculateSerializeBufferSize() const; 309 310 ///fills the dataBuffer and returns the struct name (and 0 on failure) 311 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; 312 313 314 }; 315 316 317 //only for backward compatibility 318 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION 319 ///this structure is not used, except for loading pre-2.82 .bullet files 320 struct btHingeConstraintDoubleData 321 { 322 btTypedConstraintData m_typeConstraintData; 323 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 324 btTransformDoubleData m_rbBFrame; 325 int m_useReferenceFrameA; 326 int m_angularOnly; 327 int m_enableAngularMotor; 328 float m_motorTargetVelocity; 329 float m_maxMotorImpulse; 330 331 float m_lowerLimit; 332 float m_upperLimit; 333 float m_limitSoftness; 334 float m_biasFactor; 335 float m_relaxationFactor; 336 337 }; 338 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION 339 340 ///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account 341 ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint 342 { 343 protected: 344 btScalar m_accumulatedAngle; 345 public: 346 347 BT_DECLARE_ALIGNED_ALLOCATOR(); 348 349 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false) 350 :btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA ) 351 { 352 m_accumulatedAngle=getHingeAngle(); 353 } 354 355 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false) 356 :btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA) 357 { 358 m_accumulatedAngle=getHingeAngle(); 359 } 360 361 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false) 362 :btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA ) 363 { 364 m_accumulatedAngle=getHingeAngle(); 365 } 366 367 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false) 368 :btHingeConstraint(rbA,rbAFrame, useReferenceFrameA ) 369 { 370 m_accumulatedAngle=getHingeAngle(); 371 } 372 btScalar getAccumulatedHingeAngle(); 373 void setAccumulatedHingeAngle(btScalar accAngle); 374 virtual void getInfo1 (btConstraintInfo1* info); 375 376 }; 377 378 struct btHingeConstraintFloatData 379 { 380 btTypedConstraintData m_typeConstraintData; 381 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 382 btTransformFloatData m_rbBFrame; 383 int m_useReferenceFrameA; 384 int m_angularOnly; 385 386 int m_enableAngularMotor; 387 float m_motorTargetVelocity; 388 float m_maxMotorImpulse; 389 390 float m_lowerLimit; 391 float m_upperLimit; 392 float m_limitSoftness; 393 float m_biasFactor; 394 float m_relaxationFactor; 395 396 }; 397 398 399 400 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 401 struct btHingeConstraintDoubleData2 402 { 403 btTypedConstraintDoubleData m_typeConstraintData; 404 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 405 btTransformDoubleData m_rbBFrame; 406 int m_useReferenceFrameA; 407 int m_angularOnly; 408 int m_enableAngularMotor; 409 double m_motorTargetVelocity; 410 double m_maxMotorImpulse; 411 412 double m_lowerLimit; 413 double m_upperLimit; 414 double m_limitSoftness; 415 double m_biasFactor; 416 double m_relaxationFactor; 417 char m_padding1[4]; 418 419 }; 420 421 422 423 424 SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const 425 { 426 return sizeof(btHingeConstraintData); 427 } 428 429 ///fills the dataBuffer and returns the struct name (and 0 on failure) 430 SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const 431 { 432 btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer; 433 btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer); 434 435 m_rbAFrame.serialize(hingeData->m_rbAFrame); 436 m_rbBFrame.serialize(hingeData->m_rbBFrame); 437 438 hingeData->m_angularOnly = m_angularOnly; 439 hingeData->m_enableAngularMotor = m_enableAngularMotor; 440 hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); 441 hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); 442 hingeData->m_useReferenceFrameA = m_useReferenceFrameA; 443 #ifdef _BT_USE_CENTER_LIMIT_ 444 hingeData->m_lowerLimit = float(m_limit.getLow()); 445 hingeData->m_upperLimit = float(m_limit.getHigh()); 446 hingeData->m_limitSoftness = float(m_limit.getSoftness()); 447 hingeData->m_biasFactor = float(m_limit.getBiasFactor()); 448 hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor()); 449 #else 450 hingeData->m_lowerLimit = float(m_lowerLimit); 451 hingeData->m_upperLimit = float(m_upperLimit); 452 hingeData->m_limitSoftness = float(m_limitSoftness); 453 hingeData->m_biasFactor = float(m_biasFactor); 454 hingeData->m_relaxationFactor = float(m_relaxationFactor); 455 #endif 456 457 return btHingeConstraintDataName; 458 } 459 460 #endif //BT_HINGECONSTRAINT_H 461
