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