Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @ 8393

Last change on this file since 8393 was 8393, checked in by rgrieder, 13 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 5.3 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
16
17#include "btTypedConstraint.h"
18#include "BulletDynamics/Dynamics/btRigidBody.h"
19#include "LinearMath/btSerializer.h"
20
21
22#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
23
24btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
25:btTypedObject(type),
26m_userConstraintType(-1),
27m_userConstraintId(-1),
28m_needsFeedback(false),
29m_rbA(rbA),
30m_rbB(getFixedBody()),
31m_appliedImpulse(btScalar(0.)),
32m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
33m_breakingImpulseThreshold(SIMD_INFINITY),
34m_isEnabled(true)
35{
36}
37
38
39btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
40:btTypedObject(type),
41m_userConstraintType(-1),
42m_userConstraintId(-1),
43m_needsFeedback(false),
44m_rbA(rbA),
45m_rbB(rbB),
46m_appliedImpulse(btScalar(0.)),
47m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
48m_breakingImpulseThreshold(SIMD_INFINITY),
49m_isEnabled(true)
50{
51}
52
53
54
55
56btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
57{
58        if(lowLim > uppLim)
59        {
60                return btScalar(1.0f);
61        }
62        else if(lowLim == uppLim)
63        {
64                return btScalar(0.0f);
65        }
66        btScalar lim_fact = btScalar(1.0f);
67        btScalar delta_max = vel / timeFact;
68        if(delta_max < btScalar(0.0f))
69        {
70                if((pos >= lowLim) && (pos < (lowLim - delta_max)))
71                {
72                        lim_fact = (lowLim - pos) / delta_max;
73                }
74                else if(pos  < lowLim)
75                {
76                        lim_fact = btScalar(0.0f);
77                }
78                else
79                {
80                        lim_fact = btScalar(1.0f);
81                }
82        }
83        else if(delta_max > btScalar(0.0f))
84        {
85                if((pos <= uppLim) && (pos > (uppLim - delta_max)))
86                {
87                        lim_fact = (uppLim - pos) / delta_max;
88                }
89                else if(pos  > uppLim)
90                {
91                        lim_fact = btScalar(0.0f);
92                }
93                else
94                {
95                        lim_fact = btScalar(1.0f);
96                }
97        }
98        else
99        {
100                        lim_fact = btScalar(0.0f);
101        }
102        return lim_fact;
103}
104
105///fills the dataBuffer and returns the struct name (and 0 on failure)
106const char*     btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
107{
108        btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
109
110        tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
111        tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
112        char* name = (char*) serializer->findNameForPointer(this);
113        tcd->m_name = (char*)serializer->getUniquePointer(name);
114        if (tcd->m_name)
115        {
116                serializer->serializeName(name);
117        }
118
119        tcd->m_objectType = m_objectType;
120        tcd->m_needsFeedback = m_needsFeedback;
121        tcd->m_userConstraintId =m_userConstraintId;
122        tcd->m_userConstraintType =m_userConstraintType;
123
124        tcd->m_appliedImpulse = float(m_appliedImpulse);
125        tcd->m_dbgDrawSize = float(m_dbgDrawSize );
126
127        tcd->m_disableCollisionsBetweenLinkedBodies = false;
128
129        int i;
130        for (i=0;i<m_rbA.getNumConstraintRefs();i++)
131                if (m_rbA.getConstraintRef(i) == this)
132                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
133        for (i=0;i<m_rbB.getNumConstraintRefs();i++)
134                if (m_rbB.getConstraintRef(i) == this)
135                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
136
137        return "btTypedConstraintData";
138}
139
140btRigidBody& btTypedConstraint::getFixedBody()
141{
142        static btRigidBody s_fixed(0, 0,0);
143        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
144        return s_fixed;
145}
146
147
148void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
149{
150        m_halfRange = (high - low) / 2.0f;
151        m_center = btNormalizeAngle(low + m_halfRange);
152        m_softness =  _softness;
153        m_biasFactor = _biasFactor;
154        m_relaxationFactor = _relaxationFactor;
155}
156
157void btAngularLimit::test(const btScalar angle)
158{
159        m_correction = 0.0f;
160        m_sign = 0.0f;
161        m_solveLimit = false;
162
163        if (m_halfRange >= 0.0f)
164        {
165                btScalar deviation = btNormalizeAngle(angle - m_center);
166                if (deviation < -m_halfRange)
167                {
168                        m_solveLimit = true;
169                        m_correction = - (deviation + m_halfRange);
170                        m_sign = +1.0f;
171                }
172                else if (deviation > m_halfRange)
173                {
174                        m_solveLimit = true;
175                        m_correction = m_halfRange - deviation;
176                        m_sign = -1.0f;
177                }
178        }
179}
180
181
182btScalar btAngularLimit::getError() const
183{
184        return m_correction * m_sign;
185}
186
187void btAngularLimit::fit(btScalar& angle) const
188{
189        if (m_halfRange > 0.0f)
190        {
191                btScalar relativeAngle = btNormalizeAngle(angle - m_center);
192                if (!btEqual(relativeAngle, m_halfRange))
193                {
194                        if (relativeAngle > 0.0f)
195                        {
196                                angle = getHigh();
197                        }
198                        else
199                        {
200                                angle = getLow();
201                        }
202                }
203        }
204}
205
206btScalar btAngularLimit::getLow() const
207{
208        return btNormalizeAngle(m_center - m_halfRange);
209}
210
211btScalar btAngularLimit::getHigh() const
212{
213        return btNormalizeAngle(m_center + m_halfRange);
214}
Note: See TracBrowser for help on using the repository browser.