Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/fps/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @ 7395

Last change on this file since 7395 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

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