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 TODO: 21 - add clamping od accumulated impulse to improve stability 22 - add conversion for ODE constraint solver 23 */ 24 25 #ifndef BT_SLIDER_CONSTRAINT_H 26 #define BT_SLIDER_CONSTRAINT_H 27 28 #ifdef BT_USE_DOUBLE_PRECISION 29 #define btSliderConstraintData2 btSliderConstraintDoubleData 30 #define btSliderConstraintDataName "btSliderConstraintDoubleData" 31 #else 32 #define btSliderConstraintData2 btSliderConstraintData 33 #define btSliderConstraintDataName "btSliderConstraintData" 34 #endif //BT_USE_DOUBLE_PRECISION 35 36 #include "LinearMath/btVector3.h" 37 #include "btJacobianEntry.h" 38 #include "btTypedConstraint.h" 39 40 41 42 class btRigidBody; 43 44 45 46 #define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) 47 #define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) 48 #define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) 49 #define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f)) 50 51 52 enum btSliderFlags 53 { 54 BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0), 55 BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1), 56 BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2), 57 BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3), 58 BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4), 59 BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5), 60 BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6), 61 BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7), 62 BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8), 63 BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9), 64 BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10), 65 BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11) 66 }; 67 68 69 ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint 70 { 71 protected: 72 ///for backwards compatibility during the transition to 'getInfo/getInfo2' 73 bool m_useSolveConstraintObsolete; 74 bool m_useOffsetForConstraintFrame; 75 btTransform m_frameInA; 76 btTransform m_frameInB; 77 // use frameA fo define limits, if true 78 bool m_useLinearReferenceFrameA; 79 // linear limits 80 btScalar m_lowerLinLimit; 81 btScalar m_upperLinLimit; 82 // angular limits 83 btScalar m_lowerAngLimit; 84 btScalar m_upperAngLimit; 85 // softness, restitution and damping for different cases 86 // DirLin - moving inside linear limits 87 // LimLin - hitting linear limit 88 // DirAng - moving inside angular limits 89 // LimAng - hitting angular limit 90 // OrthoLin, OrthoAng - against constraint axis 91 btScalar m_softnessDirLin; 92 btScalar m_restitutionDirLin; 93 btScalar m_dampingDirLin; 94 btScalar m_cfmDirLin; 95 96 btScalar m_softnessDirAng; 97 btScalar m_restitutionDirAng; 98 btScalar m_dampingDirAng; 99 btScalar m_cfmDirAng; 100 101 btScalar m_softnessLimLin; 102 btScalar m_restitutionLimLin; 103 btScalar m_dampingLimLin; 104 btScalar m_cfmLimLin; 105 106 btScalar m_softnessLimAng; 107 btScalar m_restitutionLimAng; 108 btScalar m_dampingLimAng; 109 btScalar m_cfmLimAng; 110 111 btScalar m_softnessOrthoLin; 112 btScalar m_restitutionOrthoLin; 113 btScalar m_dampingOrthoLin; 114 btScalar m_cfmOrthoLin; 115 116 btScalar m_softnessOrthoAng; 117 btScalar m_restitutionOrthoAng; 118 btScalar m_dampingOrthoAng; 119 btScalar m_cfmOrthoAng; 120 121 // for interlal use 122 bool m_solveLinLim; 123 bool m_solveAngLim; 124 125 int m_flags; 126 127 btJacobianEntry m_jacLin[3]; 128 btScalar m_jacLinDiagABInv[3]; 129 130 btJacobianEntry m_jacAng[3]; 131 132 btScalar m_timeStep; 133 btTransform m_calculatedTransformA; 134 btTransform m_calculatedTransformB; 135 136 btVector3 m_sliderAxis; 137 btVector3 m_realPivotAInW; 138 btVector3 m_realPivotBInW; 139 btVector3 m_projPivotInW; 140 btVector3 m_delta; 141 btVector3 m_depth; 142 btVector3 m_relPosA; 143 btVector3 m_relPosB; 144 145 btScalar m_linPos; 146 btScalar m_angPos; 147 148 btScalar m_angDepth; 149 btScalar m_kAngle; 150 151 bool m_poweredLinMotor; 152 btScalar m_targetLinMotorVelocity; 153 btScalar m_maxLinMotorForce; 154 btScalar m_accumulatedLinMotorImpulse; 155 156 bool m_poweredAngMotor; 157 btScalar m_targetAngMotorVelocity; 158 btScalar m_maxAngMotorForce; 159 btScalar m_accumulatedAngMotorImpulse; 160 161 //------------------------ 162 void initParams(); 163 public: 164 BT_DECLARE_ALIGNED_ALLOCATOR(); 165 166 // constructors 167 btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); 168 btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); 169 170 // overrides 171 172 virtual void getInfo1 (btConstraintInfo1* info); 173 174 void getInfo1NonVirtual(btConstraintInfo1* info); 175 176 virtual void getInfo2 (btConstraintInfo2* info); 177 178 void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); 179 180 181 // access 182 const btRigidBody& getRigidBodyA() const { return m_rbA; } 183 const btRigidBody& getRigidBodyB() const { return m_rbB; } 184 const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } 185 const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } 186 const btTransform & getFrameOffsetA() const { return m_frameInA; } 187 const btTransform & getFrameOffsetB() const { return m_frameInB; } 188 btTransform & getFrameOffsetA() { return m_frameInA; } 189 btTransform & getFrameOffsetB() { return m_frameInB; } 190 btScalar getLowerLinLimit() { return m_lowerLinLimit; } 191 void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } 192 btScalar getUpperLinLimit() { return m_upperLinLimit; } 193 void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } 194 btScalar getLowerAngLimit() { return m_lowerAngLimit; } 195 void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } 196 btScalar getUpperAngLimit() { return m_upperAngLimit; } 197 void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } 198 bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } 199 btScalar getSoftnessDirLin() { return m_softnessDirLin; } 200 btScalar getRestitutionDirLin() { return m_restitutionDirLin; } 201 btScalar getDampingDirLin() { return m_dampingDirLin ; } 202 btScalar getSoftnessDirAng() { return m_softnessDirAng; } 203 btScalar getRestitutionDirAng() { return m_restitutionDirAng; } 204 btScalar getDampingDirAng() { return m_dampingDirAng; } 205 btScalar getSoftnessLimLin() { return m_softnessLimLin; } 206 btScalar getRestitutionLimLin() { return m_restitutionLimLin; } 207 btScalar getDampingLimLin() { return m_dampingLimLin; } 208 btScalar getSoftnessLimAng() { return m_softnessLimAng; } 209 btScalar getRestitutionLimAng() { return m_restitutionLimAng; } 210 btScalar getDampingLimAng() { return m_dampingLimAng; } 211 btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } 212 btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } 213 btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } 214 btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } 215 btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } 216 btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } 217 void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } 218 void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } 219 void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } 220 void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } 221 void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } 222 void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } 223 void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } 224 void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } 225 void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } 226 void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } 227 void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } 228 void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } 229 void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } 230 void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } 231 void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } 232 void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } 233 void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } 234 void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } 235 void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } 236 bool getPoweredLinMotor() { return m_poweredLinMotor; } 237 void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } 238 btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } 239 void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } 240 btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } 241 void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } 242 bool getPoweredAngMotor() { return m_poweredAngMotor; } 243 void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } 244 btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } 245 void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } 246 btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } 247 248 btScalar getLinearPos() const { return m_linPos; } 249 btScalar getAngularPos() const { return m_angPos; } 250 251 252 253 // access for ODE solver 254 bool getSolveLinLimit() { return m_solveLinLim; } 255 btScalar getLinDepth() { return m_depth[0]; } 256 bool getSolveAngLimit() { return m_solveAngLim; } 257 btScalar getAngDepth() { return m_angDepth; } 258 // shared code used by ODE solver 259 void calculateTransforms(const btTransform& transA,const btTransform& transB); 260 void testLinLimits(); 261 void testAngLimits(); 262 // access for PE Solver 263 btVector3 getAncorInA(); 264 btVector3 getAncorInB(); 265 // access for UseFrameOffset 266 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } 267 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } 268 269 void setFrames(const btTransform& frameA, const btTransform& frameB) 270 { 271 m_frameInA=frameA; 272 m_frameInB=frameB; 273 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 274 buildJacobian(); 275 } 276 277 278 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 279 ///If no axis is provided, it uses the default axis for this constraint. 280 virtual void setParam(int num, btScalar value, int axis = -1); 281 ///return the local value of parameter 282 virtual btScalar getParam(int num, int axis = -1) const; 283 284 virtual int calculateSerializeBufferSize() const; 285 286 ///fills the dataBuffer and returns the struct name (and 0 on failure) 287 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; 288 289 290 }; 291 292 293 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 294 295 296 struct btSliderConstraintData 297 { 298 btTypedConstraintData m_typeConstraintData; 299 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 300 btTransformFloatData m_rbBFrame; 301 302 float m_linearUpperLimit; 303 float m_linearLowerLimit; 304 305 float m_angularUpperLimit; 306 float m_angularLowerLimit; 307 308 int m_useLinearReferenceFrameA; 309 int m_useOffsetForConstraintFrame; 310 311 }; 312 313 314 struct btSliderConstraintDoubleData 315 { 316 btTypedConstraintDoubleData m_typeConstraintData; 317 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 318 btTransformDoubleData m_rbBFrame; 319 320 double m_linearUpperLimit; 321 double m_linearLowerLimit; 322 323 double m_angularUpperLimit; 324 double m_angularLowerLimit; 325 326 int m_useLinearReferenceFrameA; 327 int m_useOffsetForConstraintFrame; 328 329 }; 330 331 SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const 332 { 333 return sizeof(btSliderConstraintData2); 334 } 335 336 ///fills the dataBuffer and returns the struct name (and 0 on failure) 337 SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const 338 { 339 340 btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer; 341 btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); 342 343 m_frameInA.serialize(sliderData->m_rbAFrame); 344 m_frameInB.serialize(sliderData->m_rbBFrame); 345 346 sliderData->m_linearUpperLimit = m_upperLinLimit; 347 sliderData->m_linearLowerLimit = m_lowerLinLimit; 348 349 sliderData->m_angularUpperLimit = m_upperAngLimit; 350 sliderData->m_angularLowerLimit = m_lowerAngLimit; 351 352 sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA; 353 sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame; 354 355 return btSliderConstraintDataName; 356 } 357 358 359 360 #endif //BT_SLIDER_CONSTRAINT_H 361 362
