Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/particles/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @ 2900

Last change on this file since 2900 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

  • Property svn:eol-style set to native
File size: 11.9 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15/*
162007-09-09
17btGeneric6DofConstraint Refactored by Francisco Le?n
18email: projectileman@yahoo.com
19http://gimpact.sf.net
20*/
21
22
23#ifndef GENERIC_6DOF_CONSTRAINT_H
24#define GENERIC_6DOF_CONSTRAINT_H
25
26#include "LinearMath/btVector3.h"
27#include "btJacobianEntry.h"
28#include "btTypedConstraint.h"
29
30class btRigidBody;
31
32
33//! Rotation Limit structure for generic joints
34class btRotationalLimitMotor
35{
36public:
37    //! limit_parameters
38    //!@{
39    btScalar m_loLimit;//!< joint limit
40    btScalar m_hiLimit;//!< joint limit
41    btScalar m_targetVelocity;//!< target motor velocity
42    btScalar m_maxMotorForce;//!< max force on motor
43    btScalar m_maxLimitForce;//!< max force on limit
44    btScalar m_damping;//!< Damping.
45    btScalar m_limitSoftness;//! Relaxation factor
46    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
47    btScalar m_bounce;//!< restitution factor
48    bool m_enableMotor;
49
50    //!@}
51
52    //! temp_variables
53    //!@{
54    btScalar m_currentLimitError;//!  How much is violated this limit
55    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
56    btScalar m_accumulatedImpulse;
57    //!@}
58
59    btRotationalLimitMotor()
60    {
61        m_accumulatedImpulse = 0.f;
62        m_targetVelocity = 0;
63        m_maxMotorForce = 0.1f;
64        m_maxLimitForce = 300.0f;
65        m_loLimit = -SIMD_INFINITY;
66        m_hiLimit = SIMD_INFINITY;
67        m_ERP = 0.5f;
68        m_bounce = 0.0f;
69        m_damping = 1.0f;
70        m_limitSoftness = 0.5f;
71        m_currentLimit = 0;
72        m_currentLimitError = 0;
73        m_enableMotor = false;
74    }
75
76    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
77    {
78        m_targetVelocity = limot.m_targetVelocity;
79        m_maxMotorForce = limot.m_maxMotorForce;
80        m_limitSoftness = limot.m_limitSoftness;
81        m_loLimit = limot.m_loLimit;
82        m_hiLimit = limot.m_hiLimit;
83        m_ERP = limot.m_ERP;
84        m_bounce = limot.m_bounce;
85        m_currentLimit = limot.m_currentLimit;
86        m_currentLimitError = limot.m_currentLimitError;
87        m_enableMotor = limot.m_enableMotor;
88    }
89
90
91
92        //! Is limited
93    bool isLimited()
94    {
95        if(m_loLimit>=m_hiLimit) return false;
96        return true;
97    }
98
99        //! Need apply correction
100    bool needApplyTorques()
101    {
102        if(m_currentLimit == 0 && m_enableMotor == false) return false;
103        return true;
104    }
105
106        //! calculates  error
107        /*!
108        calculates m_currentLimit and m_currentLimitError.
109        */
110        int testLimitValue(btScalar test_value);
111
112        //! apply the correction impulses for two bodies
113    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
114
115
116};
117
118
119
120class btTranslationalLimitMotor
121{
122public:
123        btVector3 m_lowerLimit;//!< the constraint lower limits
124    btVector3 m_upperLimit;//!< the constraint upper limits
125    btVector3 m_accumulatedImpulse;
126    //! Linear_Limit_parameters
127    //!@{
128    btScalar    m_limitSoftness;//!< Softness for linear limit
129    btScalar    m_damping;//!< Damping for linear limit
130    btScalar    m_restitution;//! Bounce parameter for linear limit
131    //!@}
132
133    btTranslationalLimitMotor()
134    {
135        m_lowerLimit.setValue(0.f,0.f,0.f);
136        m_upperLimit.setValue(0.f,0.f,0.f);
137        m_accumulatedImpulse.setValue(0.f,0.f,0.f);
138
139        m_limitSoftness = 0.7f;
140        m_damping = btScalar(1.0f);
141        m_restitution = btScalar(0.5f);
142    }
143
144    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
145    {
146        m_lowerLimit = other.m_lowerLimit;
147        m_upperLimit = other.m_upperLimit;
148        m_accumulatedImpulse = other.m_accumulatedImpulse;
149
150        m_limitSoftness = other.m_limitSoftness ;
151        m_damping = other.m_damping;
152        m_restitution = other.m_restitution;
153    }
154
155    //! Test limit
156        /*!
157    - free means upper < lower,
158    - locked means upper == lower
159    - limited means upper > lower
160    - limitIndex: first 3 are linear, next 3 are angular
161    */
162    inline bool isLimited(int limitIndex)
163    {
164       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
165    }
166
167
168    btScalar solveLinearAxis(
169        btScalar timeStep,
170        btScalar jacDiagABInv,
171        btRigidBody& body1,const btVector3 &pointInA,
172        btRigidBody& body2,const btVector3 &pointInB,
173        int limit_index,
174        const btVector3 & axis_normal_on_a,
175                const btVector3 & anchorPos);
176
177
178};
179
180/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
181/*!
182btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
183currently this limit supports rotational motors<br>
184<ul>
185<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
186At this moment translational motors are not supported. May be in the future. </li>
187
188<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
189This is accessible through btGeneric6DofConstraint.getLimitMotor method,
190This brings support for limit parameters and motors. </li>
191
192<li> Angulars limits have these possible ranges:
193<table border=1 >
194<tr
195
196        <td><b>AXIS</b></td>
197        <td><b>MIN ANGLE</b></td>
198        <td><b>MAX ANGLE</b></td>
199        <td>X</td>
200                <td>-PI</td>
201                <td>PI</td>
202        <td>Y</td>
203                <td>-PI/2</td>
204                <td>PI/2</td>
205        <td>Z</td>
206                <td>-PI/2</td>
207                <td>PI/2</td>
208</tr>
209</table>
210</li>
211</ul>
212
213*/
214class btGeneric6DofConstraint : public btTypedConstraint
215{
216protected:
217
218        //! relative_frames
219    //!@{
220        btTransform     m_frameInA;//!< the constraint space w.r.t body A
221    btTransform m_frameInB;//!< the constraint space w.r.t body B
222    //!@}
223
224    //! Jacobians
225    //!@{
226    btJacobianEntry     m_jacLinear[3];//!< 3 orthogonal linear constraints
227    btJacobianEntry     m_jacAng[3];//!< 3 orthogonal angular constraints
228    //!@}
229
230        //! Linear_Limit_parameters
231    //!@{
232    btTranslationalLimitMotor m_linearLimits;
233    //!@}
234
235
236    //! hinge_parameters
237    //!@{
238    btRotationalLimitMotor m_angularLimits[3];
239        //!@}
240
241
242protected:
243    //! temporal variables
244    //!@{
245    btScalar m_timeStep;
246    btTransform m_calculatedTransformA;
247    btTransform m_calculatedTransformB;
248    btVector3 m_calculatedAxisAngleDiff;
249    btVector3 m_calculatedAxis[3];
250   
251        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
252
253    bool        m_useLinearReferenceFrameA;
254   
255    //!@}
256
257    btGeneric6DofConstraint&    operator=(btGeneric6DofConstraint&      other)
258    {
259        btAssert(0);
260        (void) other;
261        return *this;
262    }
263
264
265
266    void buildLinearJacobian(
267        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
268        const btVector3 & pivotAInW,const btVector3 & pivotBInW);
269
270    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
271
272
273        //! calcs the euler angles between the two bodies.
274    void calculateAngleInfo();
275
276
277
278public:
279    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
280
281    btGeneric6DofConstraint();
282
283        //! Calcs global transform of the offsets
284        /*!
285        Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
286        \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
287        */
288    void calculateTransforms();
289
290        //! Gets the global transform of the offset for body A
291    /*!
292    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
293    */
294    const btTransform & getCalculatedTransformA() const
295    {
296        return m_calculatedTransformA;
297    }
298
299    //! Gets the global transform of the offset for body B
300    /*!
301    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
302    */
303    const btTransform & getCalculatedTransformB() const
304    {
305        return m_calculatedTransformB;
306    }
307
308    const btTransform & getFrameOffsetA() const
309    {
310        return m_frameInA;
311    }
312
313    const btTransform & getFrameOffsetB() const
314    {
315        return m_frameInB;
316    }
317
318
319    btTransform & getFrameOffsetA()
320    {
321        return m_frameInA;
322    }
323
324    btTransform & getFrameOffsetB()
325    {
326        return m_frameInB;
327    }
328
329
330        //! performs Jacobian calculation, and also calculates angle differences and axis
331    virtual void        buildJacobian();
332
333    virtual     void    solveConstraint(btScalar        timeStep);
334
335    void        updateRHS(btScalar      timeStep);
336
337        //! Get the rotation axis in global coordinates
338        /*!
339        \pre btGeneric6DofConstraint.buildJacobian must be called previously.
340        */
341    btVector3 getAxis(int axis_index) const;
342
343    //! Get the relative Euler angle
344    /*!
345        \pre btGeneric6DofConstraint.buildJacobian must be called previously.
346        */
347    btScalar getAngle(int axis_index) const;
348
349        //! Test angular limit.
350        /*!
351        Calculates angular correction and returns true if limit needs to be corrected.
352        \pre btGeneric6DofConstraint.buildJacobian must be called previously.
353        */
354    bool testAngularLimitMotor(int axis_index);
355
356    void        setLinearLowerLimit(const btVector3& linearLower)
357    {
358        m_linearLimits.m_lowerLimit = linearLower;
359    }
360
361    void        setLinearUpperLimit(const btVector3& linearUpper)
362    {
363        m_linearLimits.m_upperLimit = linearUpper;
364    }
365
366    void        setAngularLowerLimit(const btVector3& angularLower)
367    {
368        m_angularLimits[0].m_loLimit = angularLower.getX();
369        m_angularLimits[1].m_loLimit = angularLower.getY();
370        m_angularLimits[2].m_loLimit = angularLower.getZ();
371    }
372
373    void        setAngularUpperLimit(const btVector3& angularUpper)
374    {
375        m_angularLimits[0].m_hiLimit = angularUpper.getX();
376        m_angularLimits[1].m_hiLimit = angularUpper.getY();
377        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
378    }
379
380        //! Retrieves the angular limit informacion
381    btRotationalLimitMotor * getRotationalLimitMotor(int index)
382    {
383        return &m_angularLimits[index];
384    }
385
386    //! Retrieves the  limit informacion
387    btTranslationalLimitMotor * getTranslationalLimitMotor()
388    {
389        return &m_linearLimits;
390    }
391
392    //first 3 are linear, next 3 are angular
393    void setLimit(int axis, btScalar lo, btScalar hi)
394    {
395        if(axis<3)
396        {
397                m_linearLimits.m_lowerLimit[axis] = lo;
398                m_linearLimits.m_upperLimit[axis] = hi;
399        }
400        else
401        {
402                m_angularLimits[axis-3].m_loLimit = lo;
403                m_angularLimits[axis-3].m_hiLimit = hi;
404        }
405    }
406
407        //! Test limit
408        /*!
409    - free means upper < lower,
410    - locked means upper == lower
411    - limited means upper > lower
412    - limitIndex: first 3 are linear, next 3 are angular
413    */
414    bool        isLimited(int limitIndex)
415    {
416        if(limitIndex<3)
417        {
418                        return m_linearLimits.isLimited(limitIndex);
419
420        }
421        return m_angularLimits[limitIndex-3].isLimited();
422    }
423
424    const btRigidBody& getRigidBodyA() const
425    {
426        return m_rbA;
427    }
428    const btRigidBody& getRigidBodyB() const
429    {
430        return m_rbB;
431    }
432
433        virtual void calcAnchorPos(void); // overridable
434
435};
436
437#endif //GENERIC_6DOF_CONSTRAINT_H
Note: See TracBrowser for help on using the repository browser.