b2Rope.cpp
Go to the documentation of this file.
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 
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;
32  m_k2 = 1.0f;
33  m_k3 = 0.1f;
34 }
35 
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 
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 
172 {
173  int32 count3 = m_count - 2;
174  for (int32 i = 0; i < count3; ++i)
175  {
176  m_as[i] = angle;
177  }
178 }
179 
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 }
d
virtual void DrawSegment(const b2Vec2 &p1, const b2Vec2 &p2, const b2Color &color)=0
Draw a line segment.
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:406
void SetAngle(float32 angle)
Definition: b2Rope.cpp:171
float32 * masses
Definition: b2Rope.h:47
float32 * m_Ls
Definition: b2Rope.h:105
float32 damping
Definition: b2Rope.h:53
#define b2_pi
Definition: b2Settings.h:40
f
float32 m_k2
Definition: b2Rope.h:111
~b2Rope()
Definition: b2Rope.cpp:36
float32 m_damping
Definition: b2Rope.h:109
float32 * m_as
Definition: b2Rope.h:106
float32 LengthSquared() const
Definition: b2Math.h:108
b2Vec2 m_gravity
Definition: b2Rope.h:108
b2Vec2 gravity
Definition: b2Rope.h:50
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:62
A 2D column vector.
Definition: b2Math.h:53
float32 k3
Bending stiffness. Values above 0.5 can make the simulation blow up.
Definition: b2Rope.h:59
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Color for debug drawing. Each value has the range [0,1].
Definition: b2Draw.h:25
signed int int32
Definition: b2Settings.h:31
void Step(float32 timeStep, int32 iterations)
Definition: b2Rope.cpp:105
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:412
Definition: b2Draw.h:35
void Initialize(const b2RopeDef *def)
Definition: b2Rope.cpp:46
float32 k2
Stretching stiffness.
Definition: b2Rope.h:56
void Draw(b2Draw *draw) const
Definition: b2Rope.cpp:251
int32 m_count
Definition: b2Rope.h:98
void b2Distance(b2DistanceOutput *output, b2SimplexCache *cache, const b2DistanceInput *input)
Definition: b2Distance.cpp:444
b2Vec2 * vertices
Definition: b2Rope.h:41
int32 count
Definition: b2Rope.h:44
float32 m_k3
Definition: b2Rope.h:112
#define b2Assert(A)
Definition: b2Settings.h:27
b2Vec2 * m_ps
Definition: b2Rope.h:99
b2Vec2 Skew() const
Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
Definition: b2Math.h:135
void SolveC2()
Definition: b2Rope.cpp:140
void * b2Alloc(int32 size)
Implement this function to use your own memory allocator.
Definition: b2Settings.cpp:27
b2Vec2 * m_p0s
Definition: b2Rope.h:100
#define b2Atan2(y, x)
Definition: b2Math.h:50
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:114
float32 * m_ims
Definition: b2Rope.h:103
void b2Free(void *mem)
If you implement b2Alloc, you should also implement this function.
Definition: b2Settings.cpp:32
b2Rope()
Definition: b2Rope.cpp:22
void SolveC3()
Definition: b2Rope.cpp:180
float float32
Definition: b2Settings.h:35
b2Vec2 * m_vs
Definition: b2Rope.h:101


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40