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 BT_SLIDER_CONSTRAINT_H
26 #define BT_SLIDER_CONSTRAINT_H
27
28 #ifdef BT_USE_DOUBLE_PRECISION
29 #define btSliderConstraintData2 btSliderConstraintDoubleData
30 #define btSliderConstraintDataName "btSliderConstraintDoubleData"
31 #else
32 #define btSliderConstraintData2 btSliderConstraintData
33 #define btSliderConstraintDataName "btSliderConstraintData"
34 #endif //BT_USE_DOUBLE_PRECISION
35
36 #include "LinearMath/btVector3.h"
37 #include "btJacobianEntry.h"
38 #include "btTypedConstraint.h"
39
40
41
42 class btRigidBody;
43
44
45
46 #define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
47 #define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
48 #define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
49 #define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
50
51
52 enum btSliderFlags
53 {
54 BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
55 BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
56 BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
57 BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
58 BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
59 BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
60 BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
61 BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
62 BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
63 BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
64 BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
65 BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
66 };
67
68
ATTRIBUTE_ALIGNED16(class)69 ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
70 {
71 protected:
72 ///for backwards compatibility during the transition to 'getInfo/getInfo2'
73 bool m_useSolveConstraintObsolete;
74 bool m_useOffsetForConstraintFrame;
75 btTransform m_frameInA;
76 btTransform m_frameInB;
77 // use frameA fo define limits, if true
78 bool m_useLinearReferenceFrameA;
79 // linear limits
80 btScalar m_lowerLinLimit;
81 btScalar m_upperLinLimit;
82 // angular limits
83 btScalar m_lowerAngLimit;
84 btScalar m_upperAngLimit;
85 // softness, restitution and damping for different cases
86 // DirLin - moving inside linear limits
87 // LimLin - hitting linear limit
88 // DirAng - moving inside angular limits
89 // LimAng - hitting angular limit
90 // OrthoLin, OrthoAng - against constraint axis
91 btScalar m_softnessDirLin;
92 btScalar m_restitutionDirLin;
93 btScalar m_dampingDirLin;
94 btScalar m_cfmDirLin;
95
96 btScalar m_softnessDirAng;
97 btScalar m_restitutionDirAng;
98 btScalar m_dampingDirAng;
99 btScalar m_cfmDirAng;
100
101 btScalar m_softnessLimLin;
102 btScalar m_restitutionLimLin;
103 btScalar m_dampingLimLin;
104 btScalar m_cfmLimLin;
105
106 btScalar m_softnessLimAng;
107 btScalar m_restitutionLimAng;
108 btScalar m_dampingLimAng;
109 btScalar m_cfmLimAng;
110
111 btScalar m_softnessOrthoLin;
112 btScalar m_restitutionOrthoLin;
113 btScalar m_dampingOrthoLin;
114 btScalar m_cfmOrthoLin;
115
116 btScalar m_softnessOrthoAng;
117 btScalar m_restitutionOrthoAng;
118 btScalar m_dampingOrthoAng;
119 btScalar m_cfmOrthoAng;
120
121 // for interlal use
122 bool m_solveLinLim;
123 bool m_solveAngLim;
124
125 int m_flags;
126
127 btJacobianEntry m_jacLin[3];
128 btScalar m_jacLinDiagABInv[3];
129
130 btJacobianEntry m_jacAng[3];
131
132 btScalar m_timeStep;
133 btTransform m_calculatedTransformA;
134 btTransform m_calculatedTransformB;
135
136 btVector3 m_sliderAxis;
137 btVector3 m_realPivotAInW;
138 btVector3 m_realPivotBInW;
139 btVector3 m_projPivotInW;
140 btVector3 m_delta;
141 btVector3 m_depth;
142 btVector3 m_relPosA;
143 btVector3 m_relPosB;
144
145 btScalar m_linPos;
146 btScalar m_angPos;
147
148 btScalar m_angDepth;
149 btScalar m_kAngle;
150
151 bool m_poweredLinMotor;
152 btScalar m_targetLinMotorVelocity;
153 btScalar m_maxLinMotorForce;
154 btScalar m_accumulatedLinMotorImpulse;
155
156 bool m_poweredAngMotor;
157 btScalar m_targetAngMotorVelocity;
158 btScalar m_maxAngMotorForce;
159 btScalar m_accumulatedAngMotorImpulse;
160
161 //------------------------
162 void initParams();
163 public:
164 BT_DECLARE_ALIGNED_ALLOCATOR();
165
166 // constructors
167 btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
168 btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
169
170 // overrides
171
172 virtual void getInfo1 (btConstraintInfo1* info);
173
174 void getInfo1NonVirtual(btConstraintInfo1* info);
175
176 virtual void getInfo2 (btConstraintInfo2* info);
177
178 void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
179
180
181 // access
182 const btRigidBody& getRigidBodyA() const { return m_rbA; }
183 const btRigidBody& getRigidBodyB() const { return m_rbB; }
184 const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
185 const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
186 const btTransform & getFrameOffsetA() const { return m_frameInA; }
187 const btTransform & getFrameOffsetB() const { return m_frameInB; }
188 btTransform & getFrameOffsetA() { return m_frameInA; }
189 btTransform & getFrameOffsetB() { return m_frameInB; }
190 btScalar getLowerLinLimit() { return m_lowerLinLimit; }
191 void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
192 btScalar getUpperLinLimit() { return m_upperLinLimit; }
193 void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
194 btScalar getLowerAngLimit() { return m_lowerAngLimit; }
195 void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
196 btScalar getUpperAngLimit() { return m_upperAngLimit; }
197 void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
198 bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
199 btScalar getSoftnessDirLin() { return m_softnessDirLin; }
200 btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
201 btScalar getDampingDirLin() { return m_dampingDirLin ; }
202 btScalar getSoftnessDirAng() { return m_softnessDirAng; }
203 btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
204 btScalar getDampingDirAng() { return m_dampingDirAng; }
205 btScalar getSoftnessLimLin() { return m_softnessLimLin; }
206 btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
207 btScalar getDampingLimLin() { return m_dampingLimLin; }
208 btScalar getSoftnessLimAng() { return m_softnessLimAng; }
209 btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
210 btScalar getDampingLimAng() { return m_dampingLimAng; }
211 btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
212 btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
213 btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
214 btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
215 btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
216 btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
217 void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
218 void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
219 void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
220 void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
221 void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
222 void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
223 void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
224 void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
225 void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
226 void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
227 void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
228 void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
229 void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
230 void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
231 void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
232 void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
233 void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
234 void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
235 void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
236 bool getPoweredLinMotor() { return m_poweredLinMotor; }
237 void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
238 btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
239 void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
240 btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
241 void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
242 bool getPoweredAngMotor() { return m_poweredAngMotor; }
243 void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
244 btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
245 void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
246 btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
247
248 btScalar getLinearPos() const { return m_linPos; }
249 btScalar getAngularPos() const { return m_angPos; }
250
251
252
253 // access for ODE solver
254 bool getSolveLinLimit() { return m_solveLinLim; }
255 btScalar getLinDepth() { return m_depth[0]; }
256 bool getSolveAngLimit() { return m_solveAngLim; }
257 btScalar getAngDepth() { return m_angDepth; }
258 // shared code used by ODE solver
259 void calculateTransforms(const btTransform& transA,const btTransform& transB);
260 void testLinLimits();
261 void testAngLimits();
262 // access for PE Solver
263 btVector3 getAncorInA();
264 btVector3 getAncorInB();
265 // access for UseFrameOffset
266 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
267 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
268
269 void setFrames(const btTransform& frameA, const btTransform& frameB)
270 {
271 m_frameInA=frameA;
272 m_frameInB=frameB;
273 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
274 buildJacobian();
275 }
276
277
278 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
279 ///If no axis is provided, it uses the default axis for this constraint.
280 virtual void setParam(int num, btScalar value, int axis = -1);
281 ///return the local value of parameter
282 virtual btScalar getParam(int num, int axis = -1) const;
283
284 virtual int calculateSerializeBufferSize() const;
285
286 ///fills the dataBuffer and returns the struct name (and 0 on failure)
287 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
288
289
290 };
291
292
293 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
294
295
296 struct btSliderConstraintData
297 {
298 btTypedConstraintData m_typeConstraintData;
299 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
300 btTransformFloatData m_rbBFrame;
301
302 float m_linearUpperLimit;
303 float m_linearLowerLimit;
304
305 float m_angularUpperLimit;
306 float m_angularLowerLimit;
307
308 int m_useLinearReferenceFrameA;
309 int m_useOffsetForConstraintFrame;
310
311 };
312
313
314 struct btSliderConstraintDoubleData
315 {
316 btTypedConstraintDoubleData m_typeConstraintData;
317 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
318 btTransformDoubleData m_rbBFrame;
319
320 double m_linearUpperLimit;
321 double m_linearLowerLimit;
322
323 double m_angularUpperLimit;
324 double m_angularLowerLimit;
325
326 int m_useLinearReferenceFrameA;
327 int m_useOffsetForConstraintFrame;
328
329 };
330
calculateSerializeBufferSize()331 SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
332 {
333 return sizeof(btSliderConstraintData2);
334 }
335
336 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer)337 SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
338 {
339
340 btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
341 btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
342
343 m_frameInA.serialize(sliderData->m_rbAFrame);
344 m_frameInB.serialize(sliderData->m_rbBFrame);
345
346 sliderData->m_linearUpperLimit = m_upperLinLimit;
347 sliderData->m_linearLowerLimit = m_lowerLinLimit;
348
349 sliderData->m_angularUpperLimit = m_upperAngLimit;
350 sliderData->m_angularLowerLimit = m_lowerAngLimit;
351
352 sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
353 sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
354
355 return btSliderConstraintDataName;
356 }
357
358
359
360 #endif //BT_SLIDER_CONSTRAINT_H
361
362