00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef BT_TYPED_CONSTRAINT_H
00017 #define BT_TYPED_CONSTRAINT_H
00018
00019
00020 #include "LinearMath/btScalar.h"
00021 #include "btSolverConstraint.h"
00022 #include "BulletDynamics/Dynamics/btRigidBody.h"
00023
00024 class btSerializer;
00025
00026
00027 enum btTypedConstraintType
00028 {
00029 POINT2POINT_CONSTRAINT_TYPE=3,
00030 HINGE_CONSTRAINT_TYPE,
00031 CONETWIST_CONSTRAINT_TYPE,
00032 D6_CONSTRAINT_TYPE,
00033 SLIDER_CONSTRAINT_TYPE,
00034 CONTACT_CONSTRAINT_TYPE,
00035 D6_SPRING_CONSTRAINT_TYPE,
00036 GEAR_CONSTRAINT_TYPE,
00037 MAX_CONSTRAINT_TYPE
00038 };
00039
00040
00041 enum btConstraintParams
00042 {
00043 BT_CONSTRAINT_ERP=1,
00044 BT_CONSTRAINT_STOP_ERP,
00045 BT_CONSTRAINT_CFM,
00046 BT_CONSTRAINT_STOP_CFM
00047 };
00048
00049 #if 1
00050 #define btAssertConstrParams(_par) btAssert(_par)
00051 #else
00052 #define btAssertConstrParams(_par)
00053 #endif
00054
00055
00056 ATTRIBUTE_ALIGNED16(struct) btJointFeedback
00057 {
00058 btVector3 m_appliedForceBodyA;
00059 btVector3 m_appliedTorqueBodyA;
00060 btVector3 m_appliedForceBodyB;
00061 btVector3 m_appliedTorqueBodyB;
00062 };
00063
00064
00066 ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
00067 {
00068 int m_userConstraintType;
00069
00070 union
00071 {
00072 int m_userConstraintId;
00073 void* m_userConstraintPtr;
00074 };
00075
00076 btScalar m_breakingImpulseThreshold;
00077 bool m_isEnabled;
00078 bool m_needsFeedback;
00079 int m_overrideNumSolverIterations;
00080
00081
00082 btTypedConstraint& operator=(btTypedConstraint& other)
00083 {
00084 btAssert(0);
00085 (void) other;
00086 return *this;
00087 }
00088
00089 protected:
00090 btRigidBody& m_rbA;
00091 btRigidBody& m_rbB;
00092 btScalar m_appliedImpulse;
00093 btScalar m_dbgDrawSize;
00094 btJointFeedback* m_jointFeedback;
00095
00097 btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
00098
00099
00100 public:
00101
00102 BT_DECLARE_ALIGNED_ALLOCATOR();
00103
00104 virtual ~btTypedConstraint() {};
00105 btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
00106 btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
00107
00108 struct btConstraintInfo1 {
00109 int m_numConstraintRows,nub;
00110 };
00111
00112 static btRigidBody& getFixedBody();
00113
00114 struct btConstraintInfo2 {
00115
00116
00117 btScalar fps,erp;
00118
00119
00120
00121
00122
00123 btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
00124
00125
00126 int rowskip;
00127
00128
00129
00130
00131 btScalar *m_constraintError,*cfm;
00132
00133
00134 btScalar *m_lowerLimit,*m_upperLimit;
00135
00136
00137
00138
00139
00140 int *findex;
00141
00142 int m_numIterations;
00143
00144
00145 btScalar m_damping;
00146 };
00147
00148 int getOverrideNumSolverIterations() const
00149 {
00150 return m_overrideNumSolverIterations;
00151 }
00152
00155 void setOverrideNumSolverIterations(int overideNumIterations)
00156 {
00157 m_overrideNumSolverIterations = overideNumIterations;
00158 }
00159
00161 virtual void buildJacobian() {};
00162
00164 virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
00165 {
00166 (void)ca;
00167 (void)solverBodyA;
00168 (void)solverBodyB;
00169 (void)timeStep;
00170 }
00171
00173 virtual void getInfo1 (btConstraintInfo1* info)=0;
00174
00176 virtual void getInfo2 (btConstraintInfo2* info)=0;
00177
00179 void internalSetAppliedImpulse(btScalar appliedImpulse)
00180 {
00181 m_appliedImpulse = appliedImpulse;
00182 }
00184 btScalar internalGetAppliedImpulse()
00185 {
00186 return m_appliedImpulse;
00187 }
00188
00189
00190 btScalar getBreakingImpulseThreshold() const
00191 {
00192 return m_breakingImpulseThreshold;
00193 }
00194
00195 void setBreakingImpulseThreshold(btScalar threshold)
00196 {
00197 m_breakingImpulseThreshold = threshold;
00198 }
00199
00200 bool isEnabled() const
00201 {
00202 return m_isEnabled;
00203 }
00204
00205 void setEnabled(bool enabled)
00206 {
00207 m_isEnabled=enabled;
00208 }
00209
00210
00212 virtual void solveConstraintObsolete(btSolverBody& ,btSolverBody& ,btScalar ) {};
00213
00214
00215 const btRigidBody& getRigidBodyA() const
00216 {
00217 return m_rbA;
00218 }
00219 const btRigidBody& getRigidBodyB() const
00220 {
00221 return m_rbB;
00222 }
00223
00224 btRigidBody& getRigidBodyA()
00225 {
00226 return m_rbA;
00227 }
00228 btRigidBody& getRigidBodyB()
00229 {
00230 return m_rbB;
00231 }
00232
00233 int getUserConstraintType() const
00234 {
00235 return m_userConstraintType ;
00236 }
00237
00238 void setUserConstraintType(int userConstraintType)
00239 {
00240 m_userConstraintType = userConstraintType;
00241 };
00242
00243 void setUserConstraintId(int uid)
00244 {
00245 m_userConstraintId = uid;
00246 }
00247
00248 int getUserConstraintId() const
00249 {
00250 return m_userConstraintId;
00251 }
00252
00253 void setUserConstraintPtr(void* ptr)
00254 {
00255 m_userConstraintPtr = ptr;
00256 }
00257
00258 void* getUserConstraintPtr()
00259 {
00260 return m_userConstraintPtr;
00261 }
00262
00263 void setJointFeedback(btJointFeedback* jointFeedback)
00264 {
00265 m_jointFeedback = jointFeedback;
00266 }
00267
00268 const btJointFeedback* getJointFeedback() const
00269 {
00270 return m_jointFeedback;
00271 }
00272
00273 btJointFeedback* getJointFeedback()
00274 {
00275 return m_jointFeedback;
00276 }
00277
00278
00279 int getUid() const
00280 {
00281 return m_userConstraintId;
00282 }
00283
00284 bool needsFeedback() const
00285 {
00286 return m_needsFeedback;
00287 }
00288
00291 void enableFeedback(bool needsFeedback)
00292 {
00293 m_needsFeedback = needsFeedback;
00294 }
00295
00298 btScalar getAppliedImpulse() const
00299 {
00300 btAssert(m_needsFeedback);
00301 return m_appliedImpulse;
00302 }
00303
00304 btTypedConstraintType getConstraintType () const
00305 {
00306 return btTypedConstraintType(m_objectType);
00307 }
00308
00309 void setDbgDrawSize(btScalar dbgDrawSize)
00310 {
00311 m_dbgDrawSize = dbgDrawSize;
00312 }
00313 btScalar getDbgDrawSize()
00314 {
00315 return m_dbgDrawSize;
00316 }
00317
00320 virtual void setParam(int num, btScalar value, int axis = -1) = 0;
00321
00323 virtual btScalar getParam(int num, int axis = -1) const = 0;
00324
00325 virtual int calculateSerializeBufferSize() const;
00326
00328 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
00329
00330 };
00331
00332
00333
00334 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
00335 {
00336 if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
00337 {
00338 return angleInRadians;
00339 }
00340 else if(angleInRadians < angleLowerLimitInRadians)
00341 {
00342 btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
00343 btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
00344 return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
00345 }
00346 else if(angleInRadians > angleUpperLimitInRadians)
00347 {
00348 btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
00349 btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
00350 return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
00351 }
00352 else
00353 {
00354 return angleInRadians;
00355 }
00356 }
00357
00359 struct btTypedConstraintData
00360 {
00361 btRigidBodyData *m_rbA;
00362 btRigidBodyData *m_rbB;
00363 char *m_name;
00364
00365 int m_objectType;
00366 int m_userConstraintType;
00367 int m_userConstraintId;
00368 int m_needsFeedback;
00369
00370 float m_appliedImpulse;
00371 float m_dbgDrawSize;
00372
00373 int m_disableCollisionsBetweenLinkedBodies;
00374 int m_overrideNumSolverIterations;
00375
00376 float m_breakingImpulseThreshold;
00377 int m_isEnabled;
00378
00379 };
00380
00381 SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
00382 {
00383 return sizeof(btTypedConstraintData);
00384 }
00385
00386
00387
00388 class btAngularLimit
00389 {
00390 private:
00391 btScalar
00392 m_center,
00393 m_halfRange,
00394 m_softness,
00395 m_biasFactor,
00396 m_relaxationFactor,
00397 m_correction,
00398 m_sign;
00399
00400 bool
00401 m_solveLimit;
00402
00403 public:
00405 btAngularLimit()
00406 :m_center(0.0f),
00407 m_halfRange(-1.0f),
00408 m_softness(0.9f),
00409 m_biasFactor(0.3f),
00410 m_relaxationFactor(1.0f),
00411 m_correction(0.0f),
00412 m_sign(0.0f),
00413 m_solveLimit(false)
00414 {}
00415
00419 void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
00420
00423 void test(const btScalar angle);
00424
00426 inline btScalar getSoftness() const
00427 {
00428 return m_softness;
00429 }
00430
00432 inline btScalar getBiasFactor() const
00433 {
00434 return m_biasFactor;
00435 }
00436
00438 inline btScalar getRelaxationFactor() const
00439 {
00440 return m_relaxationFactor;
00441 }
00442
00444 inline btScalar getCorrection() const
00445 {
00446 return m_correction;
00447 }
00448
00450 inline btScalar getSign() const
00451 {
00452 return m_sign;
00453 }
00454
00456 inline btScalar getHalfRange() const
00457 {
00458 return m_halfRange;
00459 }
00460
00462 inline bool isLimit() const
00463 {
00464 return m_solveLimit;
00465 }
00466
00469 void fit(btScalar& angle) const;
00470
00472 btScalar getError() const;
00473
00474 btScalar getLow() const;
00475
00476 btScalar getHigh() const;
00477
00478 };
00479
00480
00481
00482 #endif //BT_TYPED_CONSTRAINT_H