bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btHingeConstraint.h
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 /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
17
18 #ifndef BT_HINGECONSTRAINT_H
19 #define BT_HINGECONSTRAINT_H
20
21 #define _BT_USE_CENTER_LIMIT_ 1
22
23
24 #include "LinearMath/btVector3.h"
25 #include "btJacobianEntry.h"
26 #include "btTypedConstraint.h"
27
28 class btRigidBody;
29
30 #ifdef BT_USE_DOUBLE_PRECISION
31 #define btHingeConstraintData   btHingeConstraintDoubleData
32 #define btHingeConstraintDataName       "btHingeConstraintDoubleData"
33 #else
34 #define btHingeConstraintData   btHingeConstraintFloatData
35 #define btHingeConstraintDataName       "btHingeConstraintFloatData"
36 #endif //BT_USE_DOUBLE_PRECISION
37
38
39
40 enum btHingeFlags
41 {
42         BT_HINGE_FLAGS_CFM_STOP = 1,
43         BT_HINGE_FLAGS_ERP_STOP = 2,
44         BT_HINGE_FLAGS_CFM_NORM = 4
45 };
46
47
48 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
49 /// axis defines the orientation of the hinge axis
50 ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
51 {
52 #ifdef IN_PARALLELL_SOLVER
53 public:
54 #endif
55         btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
56         btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
57
58         btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
59         btTransform m_rbBFrame;
60
61         btScalar        m_motorTargetVelocity;
62         btScalar        m_maxMotorImpulse;
63
64
65 #ifdef  _BT_USE_CENTER_LIMIT_
66         btAngularLimit  m_limit;
67 #else
68         btScalar        m_lowerLimit;   
69         btScalar        m_upperLimit;   
70         btScalar        m_limitSign;
71         btScalar        m_correction;
72
73         btScalar        m_limitSoftness; 
74         btScalar        m_biasFactor; 
75         btScalar        m_relaxationFactor; 
76
77         bool            m_solveLimit;
78 #endif
79
80         btScalar        m_kHinge;
81
82
83         btScalar        m_accLimitImpulse;
84         btScalar        m_hingeAngle;
85         btScalar        m_referenceSign;
86
87         bool            m_angularOnly;
88         bool            m_enableAngularMotor;
89         bool            m_useSolveConstraintObsolete;
90         bool            m_useOffsetForConstraintFrame;
91         bool            m_useReferenceFrameA;
92
93         btScalar        m_accMotorImpulse;
94
95         int                     m_flags;
96         btScalar        m_normalCFM;
97         btScalar        m_stopCFM;
98         btScalar        m_stopERP;
99
100         
101 public:
102
103         BT_DECLARE_ALIGNED_ALLOCATOR();
104         
105         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
106
107         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
108         
109         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
110
111         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
112
113
114         virtual void    buildJacobian();
115
116         virtual void getInfo1 (btConstraintInfo1* info);
117
118         void getInfo1NonVirtual(btConstraintInfo1* info);
119
120         virtual void getInfo2 (btConstraintInfo2* info);
121
122         void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
123
124         void    getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
125         void    getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
126                 
127
128         void    updateRHS(btScalar      timeStep);
129
130         const btRigidBody& getRigidBodyA() const
131         {
132                 return m_rbA;
133         }
134         const btRigidBody& getRigidBodyB() const
135         {
136                 return m_rbB;
137         }
138
139         btRigidBody& getRigidBodyA()    
140         {               
141                 return m_rbA;   
142         }       
143
144         btRigidBody& getRigidBodyB()    
145         {               
146                 return m_rbB;   
147         }
148
149         btTransform& getFrameOffsetA()
150         {
151         return m_rbAFrame;
152         }
153
154         btTransform& getFrameOffsetB()
155         {
156                 return m_rbBFrame;
157         }
158
159         void setFrames(const btTransform& frameA, const btTransform& frameB);
160         
161         void    setAngularOnly(bool angularOnly)
162         {
163                 m_angularOnly = angularOnly;
164         }
165
166         void    enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
167         {
168                 m_enableAngularMotor  = enableMotor;
169                 m_motorTargetVelocity = targetVelocity;
170                 m_maxMotorImpulse = maxMotorImpulse;
171         }
172
173         // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
174         // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
175         //       maintain a given angular target.
176         void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
177         void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
178         void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
179         void setMotorTarget(btScalar targetAngle, btScalar dt);
180
181
182         void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
183         {
184 #ifdef  _BT_USE_CENTER_LIMIT_
185                 m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
186 #else
187                 m_lowerLimit = btNormalizeAngle(low);
188                 m_upperLimit = btNormalizeAngle(high);
189                 m_limitSoftness =  _softness;
190                 m_biasFactor = _biasFactor;
191                 m_relaxationFactor = _relaxationFactor;
192 #endif
193         }
194
195         void    setAxis(btVector3& axisInA)
196         {
197                 btVector3 rbAxisA1, rbAxisA2;
198                 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
199                 btVector3 pivotInA = m_rbAFrame.getOrigin();
200 //              m_rbAFrame.getOrigin() = pivotInA;
201                 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
202                                                                                 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
203                                                                                 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
204
205                 btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
206
207                 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
208                 btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
209                 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
210
211                 m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
212
213                 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
214                                                                                 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
215                                                                                 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
216                 m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
217
218         }
219
220         btScalar        getLowerLimit() const
221         {
222 #ifdef  _BT_USE_CENTER_LIMIT_
223         return m_limit.getLow();
224 #else
225         return m_lowerLimit;
226 #endif
227         }
228
229         btScalar        getUpperLimit() const
230         {
231 #ifdef  _BT_USE_CENTER_LIMIT_
232         return m_limit.getHigh();
233 #else           
234         return m_upperLimit;
235 #endif
236         }
237
238
239         btScalar getHingeAngle();
240
241         btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
242
243         void testLimit(const btTransform& transA,const btTransform& transB);
244
245
246         const btTransform& getAFrame() const { return m_rbAFrame; };    
247         const btTransform& getBFrame() const { return m_rbBFrame; };
248
249         btTransform& getAFrame() { return m_rbAFrame; };        
250         btTransform& getBFrame() { return m_rbBFrame; };
251
252         inline int getSolveLimit()
253         {
254 #ifdef  _BT_USE_CENTER_LIMIT_
255         return m_limit.isLimit();
256 #else
257         return m_solveLimit;
258 #endif
259         }
260
261         inline btScalar getLimitSign()
262         {
263 #ifdef  _BT_USE_CENTER_LIMIT_
264         return m_limit.getSign();
265 #else
266                 return m_limitSign;
267 #endif
268         }
269
270         inline bool getAngularOnly() 
271         { 
272                 return m_angularOnly; 
273         }
274         inline bool getEnableAngularMotor() 
275         { 
276                 return m_enableAngularMotor; 
277         }
278         inline btScalar getMotorTargetVelosity() 
279         { 
280                 return m_motorTargetVelocity; 
281         }
282         inline btScalar getMaxMotorImpulse() 
283         { 
284                 return m_maxMotorImpulse; 
285         }
286         // access for UseFrameOffset
287         bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
288         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
289
290
291         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
292         ///If no axis is provided, it uses the default axis for this constraint.
293         virtual void    setParam(int num, btScalar value, int axis = -1);
294         ///return the local value of parameter
295         virtual btScalar getParam(int num, int axis = -1) const;
296
297         virtual int     calculateSerializeBufferSize() const;
298
299         ///fills the dataBuffer and returns the struct name (and 0 on failure)
300         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
301
302
303 };
304
305 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
306 struct  btHingeConstraintDoubleData
307 {
308         btTypedConstraintData   m_typeConstraintData;
309         btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
310         btTransformDoubleData m_rbBFrame;
311         int                     m_useReferenceFrameA;
312         int                     m_angularOnly;
313         int                     m_enableAngularMotor;
314         float   m_motorTargetVelocity;
315         float   m_maxMotorImpulse;
316
317         float   m_lowerLimit;
318         float   m_upperLimit;
319         float   m_limitSoftness;
320         float   m_biasFactor;
321         float   m_relaxationFactor;
322
323 };
324 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
325 struct  btHingeConstraintFloatData
326 {
327         btTypedConstraintData   m_typeConstraintData;
328         btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
329         btTransformFloatData m_rbBFrame;
330         int                     m_useReferenceFrameA;
331         int                     m_angularOnly;
332         
333         int                     m_enableAngularMotor;
334         float   m_motorTargetVelocity;
335         float   m_maxMotorImpulse;
336
337         float   m_lowerLimit;
338         float   m_upperLimit;
339         float   m_limitSoftness;
340         float   m_biasFactor;
341         float   m_relaxationFactor;
342
343 };
344
345
346
347 SIMD_FORCE_INLINE       int     btHingeConstraint::calculateSerializeBufferSize() const
348 {
349         return sizeof(btHingeConstraintData);
350 }
351
352         ///fills the dataBuffer and returns the struct name (and 0 on failure)
353 SIMD_FORCE_INLINE       const char*     btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
354 {
355         btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
356         btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
357
358         m_rbAFrame.serialize(hingeData->m_rbAFrame);
359         m_rbBFrame.serialize(hingeData->m_rbBFrame);
360
361         hingeData->m_angularOnly = m_angularOnly;
362         hingeData->m_enableAngularMotor = m_enableAngularMotor;
363         hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
364         hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
365         hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
366 #ifdef  _BT_USE_CENTER_LIMIT_
367         hingeData->m_lowerLimit = float(m_limit.getLow());
368         hingeData->m_upperLimit = float(m_limit.getHigh());
369         hingeData->m_limitSoftness = float(m_limit.getSoftness());
370         hingeData->m_biasFactor = float(m_limit.getBiasFactor());
371         hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
372 #else
373         hingeData->m_lowerLimit = float(m_lowerLimit);
374         hingeData->m_upperLimit = float(m_upperLimit);
375         hingeData->m_limitSoftness = float(m_limitSoftness);
376         hingeData->m_biasFactor = float(m_biasFactor);
377         hingeData->m_relaxationFactor = float(m_relaxationFactor);
378 #endif
379
380         return btHingeConstraintDataName;
381 }
382
383 #endif //BT_HINGECONSTRAINT_H