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
b2Rope()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
~b2Rope()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
Initialize(const b2RopeDef * def)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
Step(float32 h,int32 iterations)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
SolveC2()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
SetAngle(float32 angle)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
SolveC3()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
Draw(b2Draw * draw) const251 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 }
260