[1963] | 1 | /* |
---|
| 2 | Bullet Continuous Collision Detection and Physics Library |
---|
[8351] | 3 | Copyright (c) 2003-2010 Erwin Coumans http://continuousphysics.com/Bullet/ |
---|
[1963] | 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 | |
---|
[8393] | 16 | #ifndef BT_TYPED_CONSTRAINT_H |
---|
| 17 | #define BT_TYPED_CONSTRAINT_H |
---|
[1963] | 18 | |
---|
| 19 | class btRigidBody; |
---|
| 20 | #include "LinearMath/btScalar.h" |
---|
[2882] | 21 | #include "btSolverConstraint.h" |
---|
[1963] | 22 | |
---|
[8351] | 23 | class btSerializer; |
---|
[2882] | 24 | |
---|
[8393] | 25 | //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility |
---|
[1963] | 26 | enum btTypedConstraintType |
---|
| 27 | { |
---|
[8393] | 28 | POINT2POINT_CONSTRAINT_TYPE=3, |
---|
[1963] | 29 | HINGE_CONSTRAINT_TYPE, |
---|
| 30 | CONETWIST_CONSTRAINT_TYPE, |
---|
| 31 | D6_CONSTRAINT_TYPE, |
---|
[8351] | 32 | SLIDER_CONSTRAINT_TYPE, |
---|
[8393] | 33 | CONTACT_CONSTRAINT_TYPE, |
---|
| 34 | D6_SPRING_CONSTRAINT_TYPE, |
---|
| 35 | MAX_CONSTRAINT_TYPE |
---|
[1963] | 36 | }; |
---|
| 37 | |
---|
[8351] | 38 | |
---|
| 39 | enum btConstraintParams |
---|
| 40 | { |
---|
| 41 | BT_CONSTRAINT_ERP=1, |
---|
| 42 | BT_CONSTRAINT_STOP_ERP, |
---|
| 43 | BT_CONSTRAINT_CFM, |
---|
| 44 | BT_CONSTRAINT_STOP_CFM |
---|
| 45 | }; |
---|
| 46 | |
---|
| 47 | #if 1 |
---|
| 48 | #define btAssertConstrParams(_par) btAssert(_par) |
---|
| 49 | #else |
---|
| 50 | #define btAssertConstrParams(_par) |
---|
| 51 | #endif |
---|
| 52 | |
---|
| 53 | |
---|
[1963] | 54 | ///TypedConstraint is the baseclass for Bullet constraints and vehicles |
---|
[8351] | 55 | class btTypedConstraint : public btTypedObject |
---|
[1963] | 56 | { |
---|
| 57 | int m_userConstraintType; |
---|
| 58 | |
---|
[8351] | 59 | union |
---|
| 60 | { |
---|
| 61 | int m_userConstraintId; |
---|
| 62 | void* m_userConstraintPtr; |
---|
| 63 | }; |
---|
[1963] | 64 | |
---|
[8393] | 65 | btScalar m_breakingImpulseThreshold; |
---|
| 66 | bool m_isEnabled; |
---|
| 67 | |
---|
| 68 | |
---|
[8351] | 69 | bool m_needsFeedback; |
---|
| 70 | |
---|
[1963] | 71 | btTypedConstraint& operator=(btTypedConstraint& other) |
---|
| 72 | { |
---|
| 73 | btAssert(0); |
---|
| 74 | (void) other; |
---|
| 75 | return *this; |
---|
| 76 | } |
---|
| 77 | |
---|
| 78 | protected: |
---|
| 79 | btRigidBody& m_rbA; |
---|
| 80 | btRigidBody& m_rbB; |
---|
| 81 | btScalar m_appliedImpulse; |
---|
[2882] | 82 | btScalar m_dbgDrawSize; |
---|
[1963] | 83 | |
---|
[8351] | 84 | ///internal method used by the constraint solver, don't use them directly |
---|
| 85 | btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); |
---|
| 86 | |
---|
| 87 | static btRigidBody& getFixedBody(); |
---|
[1963] | 88 | |
---|
| 89 | public: |
---|
| 90 | |
---|
| 91 | virtual ~btTypedConstraint() {}; |
---|
| 92 | btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); |
---|
| 93 | btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); |
---|
| 94 | |
---|
[2882] | 95 | struct btConstraintInfo1 { |
---|
| 96 | int m_numConstraintRows,nub; |
---|
| 97 | }; |
---|
| 98 | |
---|
| 99 | struct btConstraintInfo2 { |
---|
| 100 | // integrator parameters: frames per second (1/stepsize), default error |
---|
| 101 | // reduction parameter (0..1). |
---|
| 102 | btScalar fps,erp; |
---|
| 103 | |
---|
| 104 | // for the first and second body, pointers to two (linear and angular) |
---|
| 105 | // n*3 jacobian sub matrices, stored by rows. these matrices will have |
---|
| 106 | // been initialized to 0 on entry. if the second body is zero then the |
---|
| 107 | // J2xx pointers may be 0. |
---|
| 108 | btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; |
---|
| 109 | |
---|
| 110 | // elements to jump from one row to the next in J's |
---|
| 111 | int rowskip; |
---|
| 112 | |
---|
| 113 | // right hand sides of the equation J*v = c + cfm * lambda. cfm is the |
---|
| 114 | // "constraint force mixing" vector. c is set to zero on entry, cfm is |
---|
| 115 | // set to a constant value (typically very small or zero) value on entry. |
---|
| 116 | btScalar *m_constraintError,*cfm; |
---|
| 117 | |
---|
| 118 | // lo and hi limits for variables (set to -/+ infinity on entry). |
---|
| 119 | btScalar *m_lowerLimit,*m_upperLimit; |
---|
| 120 | |
---|
| 121 | // findex vector for variables. see the LCP solver interface for a |
---|
| 122 | // description of what this does. this is set to -1 on entry. |
---|
| 123 | // note that the returned indexes are relative to the first index of |
---|
| 124 | // the constraint. |
---|
| 125 | int *findex; |
---|
[8351] | 126 | // number of solver iterations |
---|
| 127 | int m_numIterations; |
---|
| 128 | |
---|
| 129 | //damping of the velocity |
---|
| 130 | btScalar m_damping; |
---|
[2882] | 131 | }; |
---|
| 132 | |
---|
[8351] | 133 | ///internal method used by the constraint solver, don't use them directly |
---|
| 134 | virtual void buildJacobian() {}; |
---|
[2882] | 135 | |
---|
[8351] | 136 | ///internal method used by the constraint solver, don't use them directly |
---|
[2882] | 137 | virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) |
---|
| 138 | { |
---|
[8351] | 139 | (void)ca; |
---|
| 140 | (void)solverBodyA; |
---|
| 141 | (void)solverBodyB; |
---|
| 142 | (void)timeStep; |
---|
[2882] | 143 | } |
---|
[8351] | 144 | |
---|
| 145 | ///internal method used by the constraint solver, don't use them directly |
---|
[2882] | 146 | virtual void getInfo1 (btConstraintInfo1* info)=0; |
---|
[1963] | 147 | |
---|
[8351] | 148 | ///internal method used by the constraint solver, don't use them directly |
---|
[2882] | 149 | virtual void getInfo2 (btConstraintInfo2* info)=0; |
---|
| 150 | |
---|
[8351] | 151 | ///internal method used by the constraint solver, don't use them directly |
---|
| 152 | void internalSetAppliedImpulse(btScalar appliedImpulse) |
---|
| 153 | { |
---|
| 154 | m_appliedImpulse = appliedImpulse; |
---|
| 155 | } |
---|
| 156 | ///internal method used by the constraint solver, don't use them directly |
---|
| 157 | btScalar internalGetAppliedImpulse() |
---|
| 158 | { |
---|
| 159 | return m_appliedImpulse; |
---|
| 160 | } |
---|
[2882] | 161 | |
---|
[8393] | 162 | |
---|
| 163 | btScalar getBreakingImpulseThreshold() const |
---|
| 164 | { |
---|
| 165 | return m_breakingImpulseThreshold; |
---|
| 166 | } |
---|
| 167 | |
---|
| 168 | void setBreakingImpulseThreshold(btScalar threshold) |
---|
| 169 | { |
---|
| 170 | m_breakingImpulseThreshold = threshold; |
---|
| 171 | } |
---|
| 172 | |
---|
| 173 | bool isEnabled() const |
---|
| 174 | { |
---|
| 175 | return m_isEnabled; |
---|
| 176 | } |
---|
| 177 | |
---|
| 178 | void setEnabled(bool enabled) |
---|
| 179 | { |
---|
| 180 | m_isEnabled=enabled; |
---|
| 181 | } |
---|
| 182 | |
---|
| 183 | |
---|
[8351] | 184 | ///internal method used by the constraint solver, don't use them directly |
---|
| 185 | virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; |
---|
| 186 | |
---|
[2882] | 187 | |
---|
[1963] | 188 | const btRigidBody& getRigidBodyA() const |
---|
| 189 | { |
---|
| 190 | return m_rbA; |
---|
| 191 | } |
---|
| 192 | const btRigidBody& getRigidBodyB() const |
---|
| 193 | { |
---|
| 194 | return m_rbB; |
---|
| 195 | } |
---|
| 196 | |
---|
| 197 | btRigidBody& getRigidBodyA() |
---|
| 198 | { |
---|
| 199 | return m_rbA; |
---|
| 200 | } |
---|
| 201 | btRigidBody& getRigidBodyB() |
---|
| 202 | { |
---|
| 203 | return m_rbB; |
---|
| 204 | } |
---|
| 205 | |
---|
| 206 | int getUserConstraintType() const |
---|
| 207 | { |
---|
| 208 | return m_userConstraintType ; |
---|
| 209 | } |
---|
| 210 | |
---|
| 211 | void setUserConstraintType(int userConstraintType) |
---|
| 212 | { |
---|
| 213 | m_userConstraintType = userConstraintType; |
---|
| 214 | }; |
---|
| 215 | |
---|
| 216 | void setUserConstraintId(int uid) |
---|
| 217 | { |
---|
| 218 | m_userConstraintId = uid; |
---|
| 219 | } |
---|
[2882] | 220 | |
---|
[1963] | 221 | int getUserConstraintId() const |
---|
| 222 | { |
---|
| 223 | return m_userConstraintId; |
---|
| 224 | } |
---|
| 225 | |
---|
[8351] | 226 | void setUserConstraintPtr(void* ptr) |
---|
| 227 | { |
---|
| 228 | m_userConstraintPtr = ptr; |
---|
| 229 | } |
---|
| 230 | |
---|
| 231 | void* getUserConstraintPtr() |
---|
| 232 | { |
---|
| 233 | return m_userConstraintPtr; |
---|
| 234 | } |
---|
| 235 | |
---|
[1963] | 236 | int getUid() const |
---|
| 237 | { |
---|
| 238 | return m_userConstraintId; |
---|
| 239 | } |
---|
| 240 | |
---|
[8351] | 241 | bool needsFeedback() const |
---|
| 242 | { |
---|
| 243 | return m_needsFeedback; |
---|
| 244 | } |
---|
| 245 | |
---|
| 246 | ///enableFeedback will allow to read the applied linear and angular impulse |
---|
| 247 | ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information |
---|
| 248 | void enableFeedback(bool needsFeedback) |
---|
| 249 | { |
---|
| 250 | m_needsFeedback = needsFeedback; |
---|
| 251 | } |
---|
| 252 | |
---|
| 253 | ///getAppliedImpulse is an estimated total applied impulse. |
---|
| 254 | ///This feedback could be used to determine breaking constraints or playing sounds. |
---|
[1963] | 255 | btScalar getAppliedImpulse() const |
---|
| 256 | { |
---|
[8351] | 257 | btAssert(m_needsFeedback); |
---|
[1963] | 258 | return m_appliedImpulse; |
---|
| 259 | } |
---|
| 260 | |
---|
| 261 | btTypedConstraintType getConstraintType () const |
---|
| 262 | { |
---|
[8351] | 263 | return btTypedConstraintType(m_objectType); |
---|
[1963] | 264 | } |
---|
[2882] | 265 | |
---|
| 266 | void setDbgDrawSize(btScalar dbgDrawSize) |
---|
| 267 | { |
---|
| 268 | m_dbgDrawSize = dbgDrawSize; |
---|
| 269 | } |
---|
| 270 | btScalar getDbgDrawSize() |
---|
| 271 | { |
---|
| 272 | return m_dbgDrawSize; |
---|
| 273 | } |
---|
[8351] | 274 | |
---|
| 275 | ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). |
---|
| 276 | ///If no axis is provided, it uses the default axis for this constraint. |
---|
| 277 | virtual void setParam(int num, btScalar value, int axis = -1) = 0; |
---|
| 278 | |
---|
| 279 | ///return the local value of parameter |
---|
| 280 | virtual btScalar getParam(int num, int axis = -1) const = 0; |
---|
[2882] | 281 | |
---|
[8351] | 282 | virtual int calculateSerializeBufferSize() const; |
---|
| 283 | |
---|
| 284 | ///fills the dataBuffer and returns the struct name (and 0 on failure) |
---|
| 285 | virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; |
---|
| 286 | |
---|
[1963] | 287 | }; |
---|
| 288 | |
---|
[8351] | 289 | // returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits |
---|
| 290 | // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) |
---|
| 291 | SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) |
---|
| 292 | { |
---|
| 293 | if(angleLowerLimitInRadians >= angleUpperLimitInRadians) |
---|
| 294 | { |
---|
| 295 | return angleInRadians; |
---|
| 296 | } |
---|
| 297 | else if(angleInRadians < angleLowerLimitInRadians) |
---|
| 298 | { |
---|
| 299 | btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians)); |
---|
| 300 | btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians)); |
---|
| 301 | return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI); |
---|
| 302 | } |
---|
| 303 | else if(angleInRadians > angleUpperLimitInRadians) |
---|
| 304 | { |
---|
| 305 | btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians)); |
---|
| 306 | btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); |
---|
| 307 | return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; |
---|
| 308 | } |
---|
| 309 | else |
---|
| 310 | { |
---|
| 311 | return angleInRadians; |
---|
| 312 | } |
---|
| 313 | } |
---|
| 314 | |
---|
| 315 | ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 |
---|
| 316 | struct btTypedConstraintData |
---|
| 317 | { |
---|
| 318 | btRigidBodyData *m_rbA; |
---|
| 319 | btRigidBodyData *m_rbB; |
---|
| 320 | char *m_name; |
---|
| 321 | |
---|
| 322 | int m_objectType; |
---|
| 323 | int m_userConstraintType; |
---|
| 324 | int m_userConstraintId; |
---|
| 325 | int m_needsFeedback; |
---|
| 326 | |
---|
| 327 | float m_appliedImpulse; |
---|
| 328 | float m_dbgDrawSize; |
---|
| 329 | |
---|
| 330 | int m_disableCollisionsBetweenLinkedBodies; |
---|
| 331 | char m_pad4[4]; |
---|
| 332 | |
---|
| 333 | }; |
---|
| 334 | |
---|
| 335 | SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const |
---|
| 336 | { |
---|
| 337 | return sizeof(btTypedConstraintData); |
---|
| 338 | } |
---|
| 339 | |
---|
| 340 | |
---|
| 341 | |
---|
[8393] | 342 | class btAngularLimit |
---|
| 343 | { |
---|
| 344 | private: |
---|
| 345 | btScalar |
---|
| 346 | m_center, |
---|
| 347 | m_halfRange, |
---|
| 348 | m_softness, |
---|
| 349 | m_biasFactor, |
---|
| 350 | m_relaxationFactor, |
---|
| 351 | m_correction, |
---|
| 352 | m_sign; |
---|
[8351] | 353 | |
---|
[8393] | 354 | bool |
---|
| 355 | m_solveLimit; |
---|
| 356 | |
---|
| 357 | public: |
---|
| 358 | /// Default constructor initializes limit as inactive, allowing free constraint movement |
---|
| 359 | btAngularLimit() |
---|
| 360 | :m_center(0.0f), |
---|
| 361 | m_halfRange(-1.0f), |
---|
| 362 | m_softness(0.9f), |
---|
| 363 | m_biasFactor(0.3f), |
---|
| 364 | m_relaxationFactor(1.0f), |
---|
| 365 | m_correction(0.0f), |
---|
| 366 | m_sign(0.0f), |
---|
| 367 | m_solveLimit(false) |
---|
| 368 | {} |
---|
| 369 | |
---|
| 370 | /// Sets all limit's parameters. |
---|
| 371 | /// When low > high limit becomes inactive. |
---|
| 372 | /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit |
---|
| 373 | void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f); |
---|
| 374 | |
---|
| 375 | /// Checks conastaint angle against limit. If limit is active and the angle violates the limit |
---|
| 376 | /// correction is calculated. |
---|
| 377 | void test(const btScalar angle); |
---|
| 378 | |
---|
| 379 | /// Returns limit's softness |
---|
| 380 | inline btScalar getSoftness() const |
---|
| 381 | { |
---|
| 382 | return m_softness; |
---|
| 383 | } |
---|
| 384 | |
---|
| 385 | /// Returns limit's bias factor |
---|
| 386 | inline btScalar getBiasFactor() const |
---|
| 387 | { |
---|
| 388 | return m_biasFactor; |
---|
| 389 | } |
---|
| 390 | |
---|
| 391 | /// Returns limit's relaxation factor |
---|
| 392 | inline btScalar getRelaxationFactor() const |
---|
| 393 | { |
---|
| 394 | return m_relaxationFactor; |
---|
| 395 | } |
---|
| 396 | |
---|
| 397 | /// Returns correction value evaluated when test() was invoked |
---|
| 398 | inline btScalar getCorrection() const |
---|
| 399 | { |
---|
| 400 | return m_correction; |
---|
| 401 | } |
---|
| 402 | |
---|
| 403 | /// Returns sign value evaluated when test() was invoked |
---|
| 404 | inline btScalar getSign() const |
---|
| 405 | { |
---|
| 406 | return m_sign; |
---|
| 407 | } |
---|
| 408 | |
---|
| 409 | /// Gives half of the distance between min and max limit angle |
---|
| 410 | inline btScalar getHalfRange() const |
---|
| 411 | { |
---|
| 412 | return m_halfRange; |
---|
| 413 | } |
---|
| 414 | |
---|
| 415 | /// Returns true when the last test() invocation recognized limit violation |
---|
| 416 | inline bool isLimit() const |
---|
| 417 | { |
---|
| 418 | return m_solveLimit; |
---|
| 419 | } |
---|
| 420 | |
---|
| 421 | /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle |
---|
| 422 | /// returned is modified so it equals to the limit closest to given angle. |
---|
| 423 | void fit(btScalar& angle) const; |
---|
| 424 | |
---|
| 425 | /// Returns correction value multiplied by sign value |
---|
| 426 | btScalar getError() const; |
---|
| 427 | |
---|
| 428 | btScalar getLow() const; |
---|
| 429 | |
---|
| 430 | btScalar getHigh() const; |
---|
| 431 | |
---|
| 432 | }; |
---|
| 433 | |
---|
| 434 | |
---|
| 435 | |
---|
| 436 | #endif //BT_TYPED_CONSTRAINT_H |
---|