← 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/btTypedConstraint.cpp

  • 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/btTypedConstraint.cpp
 
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
#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
 
 
24
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
 
25
:btTypedObject(type),
 
26
m_userConstraintType(-1),
 
27
m_userConstraintId(-1),
 
28
m_needsFeedback(false),
 
29
m_rbA(rbA),
 
30
m_rbB(getFixedBody()),
 
31
m_appliedImpulse(btScalar(0.)),
 
32
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
 
33
m_breakingImpulseThreshold(SIMD_INFINITY),
 
34
m_isEnabled(true)
 
35
{
 
36
}
 
37
 
 
38
 
 
39
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
 
40
:btTypedObject(type),
 
41
m_userConstraintType(-1),
 
42
m_userConstraintId(-1),
 
43
m_needsFeedback(false),
 
44
m_rbA(rbA),
 
45
m_rbB(rbB),
 
46
m_appliedImpulse(btScalar(0.)),
 
47
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
 
48
m_breakingImpulseThreshold(SIMD_INFINITY),
 
49
m_isEnabled(true)
 
50
{
 
51
}
 
52
 
 
53
 
 
54
 
 
55
 
 
56
btScalar 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)
 
106
const 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
 
 
140
btRigidBody& 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
 
 
148
void 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
 
 
157
void 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
 
 
182
btScalar btAngularLimit::getError() const
 
183
{
 
184
        return m_correction * m_sign;
 
185
}
 
186
 
 
187
void 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
 
 
206
btScalar btAngularLimit::getLow() const
 
207
{
 
208
        return btNormalizeAngle(m_center - m_halfRange);
 
209
}
 
210
 
 
211
btScalar btAngularLimit::getHigh() const
 
212
{
 
213
        return btNormalizeAngle(m_center + m_halfRange);
 
214
}