Minor fixes in Bullet/constraint solving
[blender-staging.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSliderConstraint.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 /*
17 Added by Roman Ponomarev (rponom@gmail.com)
18 April 04, 2008
19
20 TODO:
21  - add clamping od accumulated impulse to improve stability
22  - add conversion for ODE constraint solver
23 */
24
25 #ifndef SLIDER_CONSTRAINT_H
26 #define SLIDER_CONSTRAINT_H
27
28
29
30 #include "LinearMath/btVector3.h"
31 #include "btJacobianEntry.h"
32 #include "btTypedConstraint.h"
33
34
35
36 class btRigidBody;
37
38
39
40 #define SLIDER_CONSTRAINT_DEF_SOFTNESS          (btScalar(1.0))
41 #define SLIDER_CONSTRAINT_DEF_DAMPING           (btScalar(1.0))
42 #define SLIDER_CONSTRAINT_DEF_RESTITUTION       (btScalar(0.7))
43
44
45
46 class btSliderConstraint : public btTypedConstraint
47 {
48 protected:
49         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
50         bool            m_useSolveConstraintObsolete;
51         btTransform     m_frameInA;
52     btTransform m_frameInB;
53         // use frameA fo define limits, if true
54         bool m_useLinearReferenceFrameA;
55         // linear limits
56         btScalar m_lowerLinLimit;
57         btScalar m_upperLinLimit;
58         // angular limits
59         btScalar m_lowerAngLimit;
60         btScalar m_upperAngLimit;
61         // softness, restitution and damping for different cases
62         // DirLin - moving inside linear limits
63         // LimLin - hitting linear limit
64         // DirAng - moving inside angular limits
65         // LimAng - hitting angular limit
66         // OrthoLin, OrthoAng - against constraint axis
67         btScalar m_softnessDirLin;
68         btScalar m_restitutionDirLin;
69         btScalar m_dampingDirLin;
70         btScalar m_softnessDirAng;
71         btScalar m_restitutionDirAng;
72         btScalar m_dampingDirAng;
73         btScalar m_softnessLimLin;
74         btScalar m_restitutionLimLin;
75         btScalar m_dampingLimLin;
76         btScalar m_softnessLimAng;
77         btScalar m_restitutionLimAng;
78         btScalar m_dampingLimAng;
79         btScalar m_softnessOrthoLin;
80         btScalar m_restitutionOrthoLin;
81         btScalar m_dampingOrthoLin;
82         btScalar m_softnessOrthoAng;
83         btScalar m_restitutionOrthoAng;
84         btScalar m_dampingOrthoAng;
85         
86         // for interlal use
87         bool m_solveLinLim;
88         bool m_solveAngLim;
89
90         btJacobianEntry m_jacLin[3];
91         btScalar                m_jacLinDiagABInv[3];
92
93     btJacobianEntry     m_jacAng[3];
94
95         btScalar m_timeStep;
96     btTransform m_calculatedTransformA;
97     btTransform m_calculatedTransformB;
98
99         btVector3 m_sliderAxis;
100         btVector3 m_realPivotAInW;
101         btVector3 m_realPivotBInW;
102         btVector3 m_projPivotInW;
103         btVector3 m_delta;
104         btVector3 m_depth;
105         btVector3 m_relPosA;
106         btVector3 m_relPosB;
107
108         btScalar m_linPos;
109         btScalar m_angPos;
110
111         btScalar m_angDepth;
112         btScalar m_kAngle;
113
114         bool     m_poweredLinMotor;
115     btScalar m_targetLinMotorVelocity;
116     btScalar m_maxLinMotorForce;
117     btScalar m_accumulatedLinMotorImpulse;
118         
119         bool     m_poweredAngMotor;
120     btScalar m_targetAngMotorVelocity;
121     btScalar m_maxAngMotorForce;
122     btScalar m_accumulatedAngMotorImpulse;
123
124         //------------------------    
125         void initParams();
126 public:
127         // constructors
128     btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
129     btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
130     btSliderConstraint();
131         // overrides
132     virtual void        buildJacobian();
133     virtual void getInfo1 (btConstraintInfo1* info);
134         
135         virtual void getInfo2 (btConstraintInfo2* info);
136
137     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
138         
139
140         // access
141     const btRigidBody& getRigidBodyA() const { return m_rbA; }
142     const btRigidBody& getRigidBodyB() const { return m_rbB; }
143     const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
144     const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
145     const btTransform & getFrameOffsetA() const { return m_frameInA; }
146     const btTransform & getFrameOffsetB() const { return m_frameInB; }
147     btTransform & getFrameOffsetA() { return m_frameInA; }
148     btTransform & getFrameOffsetB() { return m_frameInB; }
149     btScalar getLowerLinLimit() { return m_lowerLinLimit; }
150     void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
151     btScalar getUpperLinLimit() { return m_upperLinLimit; }
152     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
153     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
154     void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
155     btScalar getUpperAngLimit() { return m_upperAngLimit; }
156     void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
157         bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
158         btScalar getSoftnessDirLin() { return m_softnessDirLin; }
159         btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
160         btScalar getDampingDirLin() { return m_dampingDirLin ; }
161         btScalar getSoftnessDirAng() { return m_softnessDirAng; }
162         btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
163         btScalar getDampingDirAng() { return m_dampingDirAng; }
164         btScalar getSoftnessLimLin() { return m_softnessLimLin; }
165         btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
166         btScalar getDampingLimLin() { return m_dampingLimLin; }
167         btScalar getSoftnessLimAng() { return m_softnessLimAng; }
168         btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
169         btScalar getDampingLimAng() { return m_dampingLimAng; }
170         btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
171         btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
172         btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
173         btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
174         btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
175         btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
176         void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
177         void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
178         void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
179         void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
180         void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
181         void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
182         void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
183         void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
184         void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
185         void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
186         void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
187         void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
188         void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
189         void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
190         void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
191         void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
192         void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
193         void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
194         void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
195         bool getPoweredLinMotor() { return m_poweredLinMotor; }
196         void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
197         btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
198         void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
199         btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
200         void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
201         bool getPoweredAngMotor() { return m_poweredAngMotor; }
202         void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
203         btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
204         void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
205         btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
206         btScalar getLinearPos() { return m_linPos; }
207         
208
209         // access for ODE solver
210         bool getSolveLinLimit() { return m_solveLinLim; }
211         btScalar getLinDepth() { return m_depth[0]; }
212         bool getSolveAngLimit() { return m_solveAngLim; }
213         btScalar getAngDepth() { return m_angDepth; }
214         // internal
215     void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
216     void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
217         // shared code used by ODE solver
218         void    calculateTransforms(void);
219         void    testLinLimits(void);
220         void    testLinLimits2(btConstraintInfo2* info);
221         void    testAngLimits(void);
222         // access for PE Solver
223         btVector3 getAncorInA(void);
224         btVector3 getAncorInB(void);
225 };
226
227
228
229 #endif //SLIDER_CONSTRAINT_H
230