1 | /* |
---|
2 | Bullet Continuous Collision Detection and Physics Library |
---|
3 | Copyright (c) 2003-2010 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 | #ifndef BT_TYPED_CONSTRAINT_H |
---|
17 | #define BT_TYPED_CONSTRAINT_H |
---|
18 | |
---|
19 | class btRigidBody; |
---|
20 | #include "LinearMath/btScalar.h" |
---|
21 | #include "btSolverConstraint.h" |
---|
22 | |
---|
23 | class btSerializer; |
---|
24 | |
---|
25 | //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility |
---|
26 | enum btTypedConstraintType |
---|
27 | { |
---|
28 | POINT2POINT_CONSTRAINT_TYPE=3, |
---|
29 | HINGE_CONSTRAINT_TYPE, |
---|
30 | CONETWIST_CONSTRAINT_TYPE, |
---|
31 | D6_CONSTRAINT_TYPE, |
---|
32 | SLIDER_CONSTRAINT_TYPE, |
---|
33 | CONTACT_CONSTRAINT_TYPE, |
---|
34 | D6_SPRING_CONSTRAINT_TYPE, |
---|
35 | MAX_CONSTRAINT_TYPE |
---|
36 | }; |
---|
37 | |
---|
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 | |
---|
54 | ///TypedConstraint is the baseclass for Bullet constraints and vehicles |
---|
55 | class btTypedConstraint : public btTypedObject |
---|
56 | { |
---|
57 | int m_userConstraintType; |
---|
58 | |
---|
59 | union |
---|
60 | { |
---|
61 | int m_userConstraintId; |
---|
62 | void* m_userConstraintPtr; |
---|
63 | }; |
---|
64 | |
---|
65 | btScalar m_breakingImpulseThreshold; |
---|
66 | bool m_isEnabled; |
---|
67 | |
---|
68 | |
---|
69 | bool m_needsFeedback; |
---|
70 | |
---|
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; |
---|
82 | btScalar m_dbgDrawSize; |
---|
83 | |
---|
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(); |
---|
88 | |
---|
89 | public: |
---|
90 | |
---|
91 | virtual ~btTypedConstraint() {}; |
---|
92 | btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); |
---|
93 | btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); |
---|
94 | |
---|
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; |
---|
126 | // number of solver iterations |
---|
127 | int m_numIterations; |
---|
128 | |
---|
129 | //damping of the velocity |
---|
130 | btScalar m_damping; |
---|
131 | }; |
---|
132 | |
---|
133 | ///internal method used by the constraint solver, don't use them directly |
---|
134 | virtual void buildJacobian() {}; |
---|
135 | |
---|
136 | ///internal method used by the constraint solver, don't use them directly |
---|
137 | virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) |
---|
138 | { |
---|
139 | (void)ca; |
---|
140 | (void)solverBodyA; |
---|
141 | (void)solverBodyB; |
---|
142 | (void)timeStep; |
---|
143 | } |
---|
144 | |
---|
145 | ///internal method used by the constraint solver, don't use them directly |
---|
146 | virtual void getInfo1 (btConstraintInfo1* info)=0; |
---|
147 | |
---|
148 | ///internal method used by the constraint solver, don't use them directly |
---|
149 | virtual void getInfo2 (btConstraintInfo2* info)=0; |
---|
150 | |
---|
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 | } |
---|
161 | |
---|
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 | |
---|
184 | ///internal method used by the constraint solver, don't use them directly |
---|
185 | virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; |
---|
186 | |
---|
187 | |
---|
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 | } |
---|
220 | |
---|
221 | int getUserConstraintId() const |
---|
222 | { |
---|
223 | return m_userConstraintId; |
---|
224 | } |
---|
225 | |
---|
226 | void setUserConstraintPtr(void* ptr) |
---|
227 | { |
---|
228 | m_userConstraintPtr = ptr; |
---|
229 | } |
---|
230 | |
---|
231 | void* getUserConstraintPtr() |
---|
232 | { |
---|
233 | return m_userConstraintPtr; |
---|
234 | } |
---|
235 | |
---|
236 | int getUid() const |
---|
237 | { |
---|
238 | return m_userConstraintId; |
---|
239 | } |
---|
240 | |
---|
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. |
---|
255 | btScalar getAppliedImpulse() const |
---|
256 | { |
---|
257 | btAssert(m_needsFeedback); |
---|
258 | return m_appliedImpulse; |
---|
259 | } |
---|
260 | |
---|
261 | btTypedConstraintType getConstraintType () const |
---|
262 | { |
---|
263 | return btTypedConstraintType(m_objectType); |
---|
264 | } |
---|
265 | |
---|
266 | void setDbgDrawSize(btScalar dbgDrawSize) |
---|
267 | { |
---|
268 | m_dbgDrawSize = dbgDrawSize; |
---|
269 | } |
---|
270 | btScalar getDbgDrawSize() |
---|
271 | { |
---|
272 | return m_dbgDrawSize; |
---|
273 | } |
---|
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; |
---|
281 | |
---|
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 | |
---|
287 | }; |
---|
288 | |
---|
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 | |
---|
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; |
---|
353 | |
---|
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 |
---|