← Back to branch summary

~ubuntu-branches/ubuntu/saucy/emscripten/saucy-proposed

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Rope/b2Rope.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/box2d/Box2D/Rope/b2Rope.cpp
 
1
/*
 
2
* Copyright (c) 2011 Erin Catto http://box2d.org
 
3
*
 
4
* This software is provided 'as-is', without any express or implied
 
5
* warranty.  In no event will the authors be held liable for any damages
 
6
* 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
 
9
* freely, subject to the following restrictions:
 
10
* 1. The origin of this software must not be misrepresented; you must not
 
11
* claim that you wrote the original software. If you use this software
 
12
* in a product, an acknowledgment in the product documentation would be
 
13
* appreciated but is not required.
 
14
* 2. Altered source versions must be plainly marked as such, and must not be
 
15
* misrepresented as being the original software.
 
16
* 3. This notice may not be removed or altered from any source distribution.
 
17
*/
 
18
 
 
19
#include <Box2D/Rope/b2Rope.h>
 
20
#include <Box2D/Common/b2Draw.h>
 
21
 
 
22
b2Rope::b2Rope()
 
23
{
 
24
        m_count = 0;
 
25
        m_ps = NULL;
 
26
        m_p0s = NULL;
 
27
        m_vs = NULL;
 
28
        m_ims = NULL;
 
29
        m_Ls = NULL;
 
30
        m_as = NULL;
 
31
        m_gravity.SetZero();
 
32
        m_k2 = 1.0f;
 
33
        m_k3 = 0.1f;
 
34
}
 
35
 
 
36
b2Rope::~b2Rope()
 
37
{
 
38
        b2Free(m_ps);
 
39
        b2Free(m_p0s);
 
40
        b2Free(m_vs);
 
41
        b2Free(m_ims);
 
42
        b2Free(m_Ls);
 
43
        b2Free(m_as);
 
44
}
 
45
 
 
46
void b2Rope::Initialize(const b2RopeDef* def)
 
47
{
 
48
        b2Assert(def->count >= 3);
 
49
        m_count = def->count;
 
50
        m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
 
51
        m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
 
52
        m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
 
53
        m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
 
54
 
 
55
        for (int32 i = 0; i < m_count; ++i)
 
56
        {
 
57
                m_ps[i] = def->vertices[i];
 
58
                m_p0s[i] = def->vertices[i];
 
59
                m_vs[i].SetZero();
 
60
 
 
61
                float32 m = def->masses[i];
 
62
                if (m > 0.0f)
 
63
                {
 
64
                        m_ims[i] = 1.0f / m;
 
65
                }
 
66
                else
 
67
                {
 
68
                        m_ims[i] = 0.0f;
 
69
                }
 
70
        }
 
71
 
 
72
        int32 count2 = m_count - 1;
 
73
        int32 count3 = m_count - 2;
 
74
        m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
 
75
        m_as = (float32*)b2Alloc(count3 * sizeof(float32));
 
76
 
 
77
        for (int32 i = 0; i < count2; ++i)
 
78
        {
 
79
                b2Vec2 p1 = m_ps[i];
 
80
                b2Vec2 p2 = m_ps[i+1];
 
81
                m_Ls[i] = b2Distance(p1, p2);
 
82
        }
 
83
 
 
84
        for (int32 i = 0; i < count3; ++i)
 
85
        {
 
86
                b2Vec2 p1 = m_ps[i];
 
87
                b2Vec2 p2 = m_ps[i + 1];
 
88
                b2Vec2 p3 = m_ps[i + 2];
 
89
 
 
90
                b2Vec2 d1 = p2 - p1;
 
91
                b2Vec2 d2 = p3 - p2;
 
92
 
 
93
                float32 a = b2Cross(d1, d2);
 
94
                float32 b = b2Dot(d1, d2);
 
95
 
 
96
                m_as[i] = b2Atan2(a, b);
 
97
        }
 
98
 
 
99
        m_gravity = def->gravity;
 
100
        m_damping = def->damping;
 
101
        m_k2 = def->k2;
 
102
        m_k3 = def->k3;
 
103
}
 
104
 
 
105
void b2Rope::Step(float32 h, int32 iterations)
 
106
{
 
107
        if (h == 0.0)
 
108
        {
 
109
                return;
 
110
        }
 
111
 
 
112
        float32 d = expf(- h * m_damping);
 
113
 
 
114
        for (int32 i = 0; i < m_count; ++i)
 
115
        {
 
116
                m_p0s[i] = m_ps[i];
 
117
                if (m_ims[i] > 0.0f)
 
118
                {
 
119
                        m_vs[i] += h * m_gravity;
 
120
                }
 
121
                m_vs[i] *= d;
 
122
                m_ps[i] += h * m_vs[i];
 
123
 
 
124
        }
 
125
 
 
126
        for (int32 i = 0; i < iterations; ++i)
 
127
        {
 
128
                SolveC2();
 
129
                SolveC3();
 
130
                SolveC2();
 
131
        }
 
132
 
 
133
        float32 inv_h = 1.0f / h;
 
134
        for (int32 i = 0; i < m_count; ++i)
 
135
        {
 
136
                m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]);
 
137
        }
 
138
}
 
139
 
 
140
void b2Rope::SolveC2()
 
141
{
 
142
        int32 count2 = m_count - 1;
 
143
 
 
144
        for (int32 i = 0; i < count2; ++i)
 
145
        {
 
146
                b2Vec2 p1 = m_ps[i];
 
147
                b2Vec2 p2 = m_ps[i + 1];
 
148
 
 
149
                b2Vec2 d = p2 - p1;
 
150
                float32 L = d.Normalize();
 
151
 
 
152
                float32 im1 = m_ims[i];
 
153
                float32 im2 = m_ims[i + 1];
 
154
 
 
155
                if (im1 + im2 == 0.0f)
 
156
                {
 
157
                        continue;
 
158
                }
 
159
 
 
160
                float32 s1 = im1 / (im1 + im2);
 
161
                float32 s2 = im2 / (im1 + im2);
 
162
 
 
163
                p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
 
164
                p2 += m_k2 * s2 * (m_Ls[i] - L) * d;
 
165
 
 
166
                m_ps[i] = p1;
 
167
                m_ps[i + 1] = p2;
 
168
        }
 
169
}
 
170
 
 
171
void b2Rope::SetAngle(float32 angle)
 
172
{
 
173
        int32 count3 = m_count - 2;
 
174
        for (int32 i = 0; i < count3; ++i)
 
175
        {
 
176
                m_as[i] = angle;
 
177
        }
 
178
}
 
179
 
 
180
void b2Rope::SolveC3()
 
181
{
 
182
        int32 count3 = m_count - 2;
 
183
 
 
184
        for (int32 i = 0; i < count3; ++i)
 
185
        {
 
186
                b2Vec2 p1 = m_ps[i];
 
187
                b2Vec2 p2 = m_ps[i + 1];
 
188
                b2Vec2 p3 = m_ps[i + 2];
 
189
 
 
190
                float32 m1 = m_ims[i];
 
191
                float32 m2 = m_ims[i + 1];
 
192
                float32 m3 = m_ims[i + 2];
 
193
 
 
194
                b2Vec2 d1 = p2 - p1;
 
195
                b2Vec2 d2 = p3 - p2;
 
196
 
 
197
                float32 L1sqr = d1.LengthSquared();
 
198
                float32 L2sqr = d2.LengthSquared();
 
199
 
 
200
                if (L1sqr * L2sqr == 0.0f)
 
201
                {
 
202
                        continue;
 
203
                }
 
204
 
 
205
                float32 a = b2Cross(d1, d2);
 
206
                float32 b = b2Dot(d1, d2);
 
207
 
 
208
                float32 angle = b2Atan2(a, b);
 
209
 
 
210
                b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
 
211
                b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
 
212
 
 
213
                b2Vec2 J1 = -Jd1;
 
214
                b2Vec2 J2 = Jd1 - Jd2;
 
215
                b2Vec2 J3 = Jd2;
 
216
 
 
217
                float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3);
 
218
                if (mass == 0.0f)
 
219
                {
 
220
                        continue;
 
221
                }
 
222
 
 
223
                mass = 1.0f / mass;
 
224
 
 
225
                float32 C = angle - m_as[i];
 
226
 
 
227
                while (C > b2_pi)
 
228
                {
 
229
                        angle -= 2 * b2_pi;
 
230
                        C = angle - m_as[i];
 
231
                }
 
232
 
 
233
                while (C < -b2_pi)
 
234
                {
 
235
                        angle += 2.0f * b2_pi;
 
236
                        C = angle - m_as[i];
 
237
                }
 
238
 
 
239
                float32 impulse = - m_k3 * mass * C;
 
240
 
 
241
                p1 += (m1 * impulse) * J1;
 
242
                p2 += (m2 * impulse) * J2;
 
243
                p3 += (m3 * impulse) * J3;
 
244
 
 
245
                m_ps[i] = p1;
 
246
                m_ps[i + 1] = p2;
 
247
                m_ps[i + 2] = p3;
 
248
        }
 
249
}
 
250
 
 
251
void b2Rope::Draw(b2Draw* draw) const
 
252
{
 
253
        b2Color c(0.4f, 0.5f, 0.7f);
 
254
 
 
255
        for (int32 i = 0; i < m_count - 1; ++i)
 
256
        {
 
257
                draw->DrawSegment(m_ps[i], m_ps[i+1], c);
 
258
        }
 
259
}