← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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
        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
 
104
 
 
105
        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
 
106
        
 
107
        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
108
 
 
109
        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
110
 
 
111
 
 
112
        virtual void    buildJacobian();
 
113
 
 
114
        virtual void getInfo1 (btConstraintInfo1* info);
 
115
 
 
116
        void getInfo1NonVirtual(btConstraintInfo1* info);
 
117
 
 
118
        virtual void getInfo2 (btConstraintInfo2* info);
 
119
 
 
120
        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
 
121
 
 
122
        void    getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
 
123
        void    getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
 
124
                
 
125
 
 
126
        void    updateRHS(btScalar      timeStep);
 
127
 
 
128
        const btRigidBody& getRigidBodyA() const
 
129
        {
 
130
                return m_rbA;
 
131
        }
 
132
        const btRigidBody& getRigidBodyB() const
 
133
        {
 
134
                return m_rbB;
 
135
        }
 
136
 
 
137
        btRigidBody& getRigidBodyA()    
 
138
        {               
 
139
                return m_rbA;   
 
140
        }       
 
141
 
 
142
        btRigidBody& getRigidBodyB()    
 
143
        {               
 
144
                return m_rbB;   
 
145
        }
 
146
 
 
147
        btTransform& getFrameOffsetA()
 
148
        {
 
149
        return m_rbAFrame;
 
150
        }
 
151
 
 
152
        btTransform& getFrameOffsetB()
 
153
        {
 
154
                return m_rbBFrame;
 
155
        }
 
156
 
 
157
        void setFrames(const btTransform& frameA, const btTransform& frameB);
 
158
        
 
159
        void    setAngularOnly(bool angularOnly)
 
160
        {
 
161
                m_angularOnly = angularOnly;
 
162
        }
 
163
 
 
164
        void    enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
 
165
        {
 
166
                m_enableAngularMotor  = enableMotor;
 
167
                m_motorTargetVelocity = targetVelocity;
 
168
                m_maxMotorImpulse = maxMotorImpulse;
 
169
        }
 
170
 
 
171
        // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
 
172
        // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
 
173
        //       maintain a given angular target.
 
174
        void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
 
175
        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
 
176
        void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
 
177
        void setMotorTarget(btScalar targetAngle, btScalar dt);
 
178
 
 
179
 
 
180
        void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 
181
        {
 
182
#ifdef  _BT_USE_CENTER_LIMIT_
 
183
                m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
 
184
#else
 
185
                m_lowerLimit = btNormalizeAngle(low);
 
186
                m_upperLimit = btNormalizeAngle(high);
 
187
                m_limitSoftness =  _softness;
 
188
                m_biasFactor = _biasFactor;
 
189
                m_relaxationFactor = _relaxationFactor;
 
190
#endif
 
191
        }
 
192
 
 
193
        void    setAxis(btVector3& axisInA)
 
194
        {
 
195
                btVector3 rbAxisA1, rbAxisA2;
 
196
                btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 
197
                btVector3 pivotInA = m_rbAFrame.getOrigin();
 
198
//              m_rbAFrame.getOrigin() = pivotInA;
 
199
                m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
 
200
                                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
 
201
                                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
202
 
 
203
                btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
204
 
 
205
                btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
 
206
                btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
 
207
                btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
208
 
 
209
                m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
 
210
 
 
211
                m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
 
212
                                                                                rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
 
213
                                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 
214
                m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
 
215
 
 
216
        }
 
217
 
 
218
        btScalar        getLowerLimit() const
 
219
        {
 
220
#ifdef  _BT_USE_CENTER_LIMIT_
 
221
        return m_limit.getLow();
 
222
#else
 
223
        return m_lowerLimit;
 
224
#endif
 
225
        }
 
226
 
 
227
        btScalar        getUpperLimit() const
 
228
        {
 
229
#ifdef  _BT_USE_CENTER_LIMIT_
 
230
        return m_limit.getHigh();
 
231
#else           
 
232
        return m_upperLimit;
 
233
#endif
 
234
        }
 
235
 
 
236
 
 
237
        btScalar getHingeAngle();
 
238
 
 
239
        btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
 
240
 
 
241
        void testLimit(const btTransform& transA,const btTransform& transB);
 
242
 
 
243
 
 
244
        const btTransform& getAFrame() const { return m_rbAFrame; };    
 
245
        const btTransform& getBFrame() const { return m_rbBFrame; };
 
246
 
 
247
        btTransform& getAFrame() { return m_rbAFrame; };        
 
248
        btTransform& getBFrame() { return m_rbBFrame; };
 
249
 
 
250
        inline int getSolveLimit()
 
251
        {
 
252
#ifdef  _BT_USE_CENTER_LIMIT_
 
253
        return m_limit.isLimit();
 
254
#else
 
255
        return m_solveLimit;
 
256
#endif
 
257
        }
 
258
 
 
259
        inline btScalar getLimitSign()
 
260
        {
 
261
#ifdef  _BT_USE_CENTER_LIMIT_
 
262
        return m_limit.getSign();
 
263
#else
 
264
                return m_limitSign;
 
265
#endif
 
266
        }
 
267
 
 
268
        inline bool getAngularOnly() 
 
269
        { 
 
270
                return m_angularOnly; 
 
271
        }
 
272
        inline bool getEnableAngularMotor() 
 
273
        { 
 
274
                return m_enableAngularMotor; 
 
275
        }
 
276
        inline btScalar getMotorTargetVelosity() 
 
277
        { 
 
278
                return m_motorTargetVelocity; 
 
279
        }
 
280
        inline btScalar getMaxMotorImpulse() 
 
281
        { 
 
282
                return m_maxMotorImpulse; 
 
283
        }
 
284
        // access for UseFrameOffset
 
285
        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 
286
        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
287
 
 
288
 
 
289
        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 
290
        ///If no axis is provided, it uses the default axis for this constraint.
 
291
        virtual void    setParam(int num, btScalar value, int axis = -1);
 
292
        ///return the local value of parameter
 
293
        virtual btScalar getParam(int num, int axis = -1) const;
 
294
 
 
295
        virtual int     calculateSerializeBufferSize() const;
 
296
 
 
297
        ///fills the dataBuffer and returns the struct name (and 0 on failure)
 
298
        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 
299
 
 
300
 
 
301
};
 
302
 
 
303
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
304
struct  btHingeConstraintDoubleData
 
305
{
 
306
        btTypedConstraintData   m_typeConstraintData;
 
307
        btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 
308
        btTransformDoubleData m_rbBFrame;
 
309
        int                     m_useReferenceFrameA;
 
310
        int                     m_angularOnly;
 
311
        int                     m_enableAngularMotor;
 
312
        float   m_motorTargetVelocity;
 
313
        float   m_maxMotorImpulse;
 
314
 
 
315
        float   m_lowerLimit;
 
316
        float   m_upperLimit;
 
317
        float   m_limitSoftness;
 
318
        float   m_biasFactor;
 
319
        float   m_relaxationFactor;
 
320
 
 
321
};
 
322
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
323
struct  btHingeConstraintFloatData
 
324
{
 
325
        btTypedConstraintData   m_typeConstraintData;
 
326
        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 
327
        btTransformFloatData m_rbBFrame;
 
328
        int                     m_useReferenceFrameA;
 
329
        int                     m_angularOnly;
 
330
        
 
331
        int                     m_enableAngularMotor;
 
332
        float   m_motorTargetVelocity;
 
333
        float   m_maxMotorImpulse;
 
334
 
 
335
        float   m_lowerLimit;
 
336
        float   m_upperLimit;
 
337
        float   m_limitSoftness;
 
338
        float   m_biasFactor;
 
339
        float   m_relaxationFactor;
 
340
 
 
341
};
 
342
 
 
343
 
 
344
 
 
345
SIMD_FORCE_INLINE       int     btHingeConstraint::calculateSerializeBufferSize() const
 
346
{
 
347
        return sizeof(btHingeConstraintData);
 
348
}
 
349
 
 
350
        ///fills the dataBuffer and returns the struct name (and 0 on failure)
 
351
SIMD_FORCE_INLINE       const char*     btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 
352
{
 
353
        btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
 
354
        btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
 
355
 
 
356
        m_rbAFrame.serialize(hingeData->m_rbAFrame);
 
357
        m_rbBFrame.serialize(hingeData->m_rbBFrame);
 
358
 
 
359
        hingeData->m_angularOnly = m_angularOnly;
 
360
        hingeData->m_enableAngularMotor = m_enableAngularMotor;
 
361
        hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
 
362
        hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
 
363
        hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
 
364
#ifdef  _BT_USE_CENTER_LIMIT_
 
365
        hingeData->m_lowerLimit = float(m_limit.getLow());
 
366
        hingeData->m_upperLimit = float(m_limit.getHigh());
 
367
        hingeData->m_limitSoftness = float(m_limit.getSoftness());
 
368
        hingeData->m_biasFactor = float(m_limit.getBiasFactor());
 
369
        hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
 
370
#else
 
371
        hingeData->m_lowerLimit = float(m_lowerLimit);
 
372
        hingeData->m_upperLimit = float(m_upperLimit);
 
373
        hingeData->m_limitSoftness = float(m_limitSoftness);
 
374
        hingeData->m_biasFactor = float(m_biasFactor);
 
375
        hingeData->m_relaxationFactor = float(m_relaxationFactor);
 
376
#endif
 
377
 
 
378
        return btHingeConstraintDataName;
 
379
}
 
380
 
 
381
#endif //BT_HINGECONSTRAINT_H