1 // MIT License
2
3 // Copyright (c) 2019 Erin Catto
4
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11
12 // The above copyright notice and this permission notice shall be included in all
13 // copies or substantial portions of the Software.
14
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 // SOFTWARE.
22
23 #include "box2d/b2_draw.h"
24 #include "box2d/b2_rope.h"
25
26 #include <stdio.h>
27
28 struct b2RopeStretch
29 {
30 int32 i1, i2;
31 float invMass1, invMass2;
32 float L;
33 float lambda;
34 float spring;
35 float damper;
36 };
37
38 struct b2RopeBend
39 {
40 int32 i1, i2, i3;
41 float invMass1, invMass2, invMass3;
42 float invEffectiveMass;
43 float lambda;
44 float L1, L2;
45 float alpha1, alpha2;
46 float spring;
47 float damper;
48 };
49
b2Rope()50 b2Rope::b2Rope()
51 {
52 m_position.SetZero();
53 m_count = 0;
54 m_stretchCount = 0;
55 m_bendCount = 0;
56 m_stretchConstraints = nullptr;
57 m_bendConstraints = nullptr;
58 m_bindPositions = nullptr;
59 m_ps = nullptr;
60 m_p0s = nullptr;
61 m_vs = nullptr;
62 m_invMasses = nullptr;
63 m_gravity.SetZero();
64 }
65
~b2Rope()66 b2Rope::~b2Rope()
67 {
68 b2Free(m_stretchConstraints);
69 b2Free(m_bendConstraints);
70 b2Free(m_bindPositions);
71 b2Free(m_ps);
72 b2Free(m_p0s);
73 b2Free(m_vs);
74 b2Free(m_invMasses);
75 }
76
Create(const b2RopeDef & def)77 void b2Rope::Create(const b2RopeDef& def)
78 {
79 b2Assert(def.count >= 3);
80 m_position = def.position;
81 m_count = def.count;
82 m_bindPositions = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
83 m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
84 m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
85 m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
86 m_invMasses = (float*)b2Alloc(m_count * sizeof(float));
87
88 for (int32 i = 0; i < m_count; ++i)
89 {
90 m_bindPositions[i] = def.vertices[i];
91 m_ps[i] = def.vertices[i] + m_position;
92 m_p0s[i] = def.vertices[i] + m_position;
93 m_vs[i].SetZero();
94
95 float m = def.masses[i];
96 if (m > 0.0f)
97 {
98 m_invMasses[i] = 1.0f / m;
99 }
100 else
101 {
102 m_invMasses[i] = 0.0f;
103 }
104 }
105
106 m_stretchCount = m_count - 1;
107 m_bendCount = m_count - 2;
108
109 m_stretchConstraints = (b2RopeStretch*)b2Alloc(m_stretchCount * sizeof(b2RopeStretch));
110 m_bendConstraints = (b2RopeBend*)b2Alloc(m_bendCount * sizeof(b2RopeBend));
111
112 for (int32 i = 0; i < m_stretchCount; ++i)
113 {
114 b2RopeStretch& c = m_stretchConstraints[i];
115
116 b2Vec2 p1 = m_ps[i];
117 b2Vec2 p2 = m_ps[i+1];
118
119 c.i1 = i;
120 c.i2 = i + 1;
121 c.L = b2Distance(p1, p2);
122 c.invMass1 = m_invMasses[i];
123 c.invMass2 = m_invMasses[i + 1];
124 c.lambda = 0.0f;
125 c.damper = 0.0f;
126 c.spring = 0.0f;
127 }
128
129 for (int32 i = 0; i < m_bendCount; ++i)
130 {
131 b2RopeBend& c = m_bendConstraints[i];
132
133 b2Vec2 p1 = m_ps[i];
134 b2Vec2 p2 = m_ps[i + 1];
135 b2Vec2 p3 = m_ps[i + 2];
136
137 c.i1 = i;
138 c.i2 = i + 1;
139 c.i3 = i + 2;
140 c.invMass1 = m_invMasses[i];
141 c.invMass2 = m_invMasses[i + 1];
142 c.invMass3 = m_invMasses[i + 2];
143 c.invEffectiveMass = 0.0f;
144 c.L1 = b2Distance(p1, p2);
145 c.L2 = b2Distance(p2, p3);
146 c.lambda = 0.0f;
147
148 // Pre-compute effective mass (TODO use flattened config)
149 b2Vec2 e1 = p2 - p1;
150 b2Vec2 e2 = p3 - p2;
151 float L1sqr = e1.LengthSquared();
152 float L2sqr = e2.LengthSquared();
153
154 if (L1sqr * L2sqr == 0.0f)
155 {
156 continue;
157 }
158
159 b2Vec2 Jd1 = (-1.0f / L1sqr) * e1.Skew();
160 b2Vec2 Jd2 = (1.0f / L2sqr) * e2.Skew();
161
162 b2Vec2 J1 = -Jd1;
163 b2Vec2 J2 = Jd1 - Jd2;
164 b2Vec2 J3 = Jd2;
165
166 c.invEffectiveMass = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
167
168 b2Vec2 r = p3 - p1;
169
170 float rr = r.LengthSquared();
171 if (rr == 0.0f)
172 {
173 continue;
174 }
175
176 // a1 = h2 / (h1 + h2)
177 // a2 = h1 / (h1 + h2)
178 c.alpha1 = b2Dot(e2, r) / rr;
179 c.alpha2 = b2Dot(e1, r) / rr;
180 }
181
182 m_gravity = def.gravity;
183
184 SetTuning(def.tuning);
185 }
186
SetTuning(const b2RopeTuning & tuning)187 void b2Rope::SetTuning(const b2RopeTuning& tuning)
188 {
189 m_tuning = tuning;
190
191 // Pre-compute spring and damper values based on tuning
192
193 const float bendOmega = 2.0f * b2_pi * m_tuning.bendHertz;
194
195 for (int32 i = 0; i < m_bendCount; ++i)
196 {
197 b2RopeBend& c = m_bendConstraints[i];
198
199 float L1sqr = c.L1 * c.L1;
200 float L2sqr = c.L2 * c.L2;
201
202 if (L1sqr * L2sqr == 0.0f)
203 {
204 c.spring = 0.0f;
205 c.damper = 0.0f;
206 continue;
207 }
208
209 // Flatten the triangle formed by the two edges
210 float J2 = 1.0f / c.L1 + 1.0f / c.L2;
211 float sum = c.invMass1 / L1sqr + c.invMass2 * J2 * J2 + c.invMass3 / L2sqr;
212 if (sum == 0.0f)
213 {
214 c.spring = 0.0f;
215 c.damper = 0.0f;
216 continue;
217 }
218
219 float mass = 1.0f / sum;
220
221 c.spring = mass * bendOmega * bendOmega;
222 c.damper = 2.0f * mass * m_tuning.bendDamping * bendOmega;
223 }
224
225 const float stretchOmega = 2.0f * b2_pi * m_tuning.stretchHertz;
226
227 for (int32 i = 0; i < m_stretchCount; ++i)
228 {
229 b2RopeStretch& c = m_stretchConstraints[i];
230
231 float sum = c.invMass1 + c.invMass2;
232 if (sum == 0.0f)
233 {
234 continue;
235 }
236
237 float mass = 1.0f / sum;
238
239 c.spring = mass * stretchOmega * stretchOmega;
240 c.damper = 2.0f * mass * m_tuning.stretchDamping * stretchOmega;
241 }
242 }
243
Step(float dt,int32 iterations,const b2Vec2 & position)244 void b2Rope::Step(float dt, int32 iterations, const b2Vec2& position)
245 {
246 if (dt == 0.0)
247 {
248 return;
249 }
250
251 const float inv_dt = 1.0f / dt;
252 float d = expf(- dt * m_tuning.damping);
253
254 // Apply gravity and damping
255 for (int32 i = 0; i < m_count; ++i)
256 {
257 if (m_invMasses[i] > 0.0f)
258 {
259 m_vs[i] *= d;
260 m_vs[i] += dt * m_gravity;
261 }
262 else
263 {
264 m_vs[i] = inv_dt * (m_bindPositions[i] + position - m_p0s[i]);
265 }
266 }
267
268 // Apply bending spring
269 if (m_tuning.bendingModel == b2_springAngleBendingModel)
270 {
271 ApplyBendForces(dt);
272 }
273
274 for (int32 i = 0; i < m_bendCount; ++i)
275 {
276 m_bendConstraints[i].lambda = 0.0f;
277 }
278
279 for (int32 i = 0; i < m_stretchCount; ++i)
280 {
281 m_stretchConstraints[i].lambda = 0.0f;
282 }
283
284 // Update position
285 for (int32 i = 0; i < m_count; ++i)
286 {
287 m_ps[i] += dt * m_vs[i];
288 }
289
290 // Solve constraints
291 for (int32 i = 0; i < iterations; ++i)
292 {
293 if (m_tuning.bendingModel == b2_pbdAngleBendingModel)
294 {
295 SolveBend_PBD_Angle();
296 }
297 else if (m_tuning.bendingModel == b2_xpbdAngleBendingModel)
298 {
299 SolveBend_XPBD_Angle(dt);
300 }
301 else if (m_tuning.bendingModel == b2_pbdDistanceBendingModel)
302 {
303 SolveBend_PBD_Distance();
304 }
305 else if (m_tuning.bendingModel == b2_pbdHeightBendingModel)
306 {
307 SolveBend_PBD_Height();
308 }
309 else if (m_tuning.bendingModel == b2_pbdTriangleBendingModel)
310 {
311 SolveBend_PBD_Triangle();
312 }
313
314 if (m_tuning.stretchingModel == b2_pbdStretchingModel)
315 {
316 SolveStretch_PBD();
317 }
318 else if (m_tuning.stretchingModel == b2_xpbdStretchingModel)
319 {
320 SolveStretch_XPBD(dt);
321 }
322 }
323
324 // Constrain velocity
325 for (int32 i = 0; i < m_count; ++i)
326 {
327 m_vs[i] = inv_dt * (m_ps[i] - m_p0s[i]);
328 m_p0s[i] = m_ps[i];
329 }
330 }
331
Reset(const b2Vec2 & position)332 void b2Rope::Reset(const b2Vec2& position)
333 {
334 m_position = position;
335
336 for (int32 i = 0; i < m_count; ++i)
337 {
338 m_ps[i] = m_bindPositions[i] + m_position;
339 m_p0s[i] = m_bindPositions[i] + m_position;
340 m_vs[i].SetZero();
341 }
342
343 for (int32 i = 0; i < m_bendCount; ++i)
344 {
345 m_bendConstraints[i].lambda = 0.0f;
346 }
347
348 for (int32 i = 0; i < m_stretchCount; ++i)
349 {
350 m_stretchConstraints[i].lambda = 0.0f;
351 }
352 }
353
SolveStretch_PBD()354 void b2Rope::SolveStretch_PBD()
355 {
356 const float stiffness = m_tuning.stretchStiffness;
357
358 for (int32 i = 0; i < m_stretchCount; ++i)
359 {
360 const b2RopeStretch& c = m_stretchConstraints[i];
361
362 b2Vec2 p1 = m_ps[c.i1];
363 b2Vec2 p2 = m_ps[c.i2];
364
365 b2Vec2 d = p2 - p1;
366 float L = d.Normalize();
367
368 float sum = c.invMass1 + c.invMass2;
369 if (sum == 0.0f)
370 {
371 continue;
372 }
373
374 float s1 = c.invMass1 / sum;
375 float s2 = c.invMass2 / sum;
376
377 p1 -= stiffness * s1 * (c.L - L) * d;
378 p2 += stiffness * s2 * (c.L - L) * d;
379
380 m_ps[c.i1] = p1;
381 m_ps[c.i2] = p2;
382 }
383 }
384
SolveStretch_XPBD(float dt)385 void b2Rope::SolveStretch_XPBD(float dt)
386 {
387 b2Assert(dt > 0.0f);
388
389 for (int32 i = 0; i < m_stretchCount; ++i)
390 {
391 b2RopeStretch& c = m_stretchConstraints[i];
392
393 b2Vec2 p1 = m_ps[c.i1];
394 b2Vec2 p2 = m_ps[c.i2];
395
396 b2Vec2 dp1 = p1 - m_p0s[c.i1];
397 b2Vec2 dp2 = p2 - m_p0s[c.i2];
398
399 b2Vec2 u = p2 - p1;
400 float L = u.Normalize();
401
402 b2Vec2 J1 = -u;
403 b2Vec2 J2 = u;
404
405 float sum = c.invMass1 + c.invMass2;
406 if (sum == 0.0f)
407 {
408 continue;
409 }
410
411 const float alpha = 1.0f / (c.spring * dt * dt); // 1 / kg
412 const float beta = dt * dt * c.damper; // kg * s
413 const float sigma = alpha * beta / dt; // non-dimensional
414 float C = L - c.L;
415
416 // This is using the initial velocities
417 float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2);
418
419 float B = C + alpha * c.lambda + sigma * Cdot;
420 float sum2 = (1.0f + sigma) * sum + alpha;
421
422 float impulse = -B / sum2;
423
424 p1 += (c.invMass1 * impulse) * J1;
425 p2 += (c.invMass2 * impulse) * J2;
426
427 m_ps[c.i1] = p1;
428 m_ps[c.i2] = p2;
429 c.lambda += impulse;
430 }
431 }
432
SolveBend_PBD_Angle()433 void b2Rope::SolveBend_PBD_Angle()
434 {
435 const float stiffness = m_tuning.bendStiffness;
436
437 for (int32 i = 0; i < m_bendCount; ++i)
438 {
439 const b2RopeBend& c = m_bendConstraints[i];
440
441 b2Vec2 p1 = m_ps[c.i1];
442 b2Vec2 p2 = m_ps[c.i2];
443 b2Vec2 p3 = m_ps[c.i3];
444
445 b2Vec2 d1 = p2 - p1;
446 b2Vec2 d2 = p3 - p2;
447 float a = b2Cross(d1, d2);
448 float b = b2Dot(d1, d2);
449
450 float angle = b2Atan2(a, b);
451
452 float L1sqr, L2sqr;
453
454 if (m_tuning.isometric)
455 {
456 L1sqr = c.L1 * c.L1;
457 L2sqr = c.L2 * c.L2;
458 }
459 else
460 {
461 L1sqr = d1.LengthSquared();
462 L2sqr = d2.LengthSquared();
463 }
464
465 if (L1sqr * L2sqr == 0.0f)
466 {
467 continue;
468 }
469
470 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
471 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
472
473 b2Vec2 J1 = -Jd1;
474 b2Vec2 J2 = Jd1 - Jd2;
475 b2Vec2 J3 = Jd2;
476
477 float sum;
478 if (m_tuning.fixedEffectiveMass)
479 {
480 sum = c.invEffectiveMass;
481 }
482 else
483 {
484 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
485 }
486
487 if (sum == 0.0f)
488 {
489 sum = c.invEffectiveMass;
490 }
491
492 float impulse = -stiffness * angle / sum;
493
494 p1 += (c.invMass1 * impulse) * J1;
495 p2 += (c.invMass2 * impulse) * J2;
496 p3 += (c.invMass3 * impulse) * J3;
497
498 m_ps[c.i1] = p1;
499 m_ps[c.i2] = p2;
500 m_ps[c.i3] = p3;
501 }
502 }
503
SolveBend_XPBD_Angle(float dt)504 void b2Rope::SolveBend_XPBD_Angle(float dt)
505 {
506 b2Assert(dt > 0.0f);
507
508 for (int32 i = 0; i < m_bendCount; ++i)
509 {
510 b2RopeBend& c = m_bendConstraints[i];
511
512 b2Vec2 p1 = m_ps[c.i1];
513 b2Vec2 p2 = m_ps[c.i2];
514 b2Vec2 p3 = m_ps[c.i3];
515
516 b2Vec2 dp1 = p1 - m_p0s[c.i1];
517 b2Vec2 dp2 = p2 - m_p0s[c.i2];
518 b2Vec2 dp3 = p3 - m_p0s[c.i3];
519
520 b2Vec2 d1 = p2 - p1;
521 b2Vec2 d2 = p3 - p2;
522
523 float L1sqr, L2sqr;
524
525 if (m_tuning.isometric)
526 {
527 L1sqr = c.L1 * c.L1;
528 L2sqr = c.L2 * c.L2;
529 }
530 else
531 {
532 L1sqr = d1.LengthSquared();
533 L2sqr = d2.LengthSquared();
534 }
535
536 if (L1sqr * L2sqr == 0.0f)
537 {
538 continue;
539 }
540
541 float a = b2Cross(d1, d2);
542 float b = b2Dot(d1, d2);
543
544 float angle = b2Atan2(a, b);
545
546 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
547 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
548
549 b2Vec2 J1 = -Jd1;
550 b2Vec2 J2 = Jd1 - Jd2;
551 b2Vec2 J3 = Jd2;
552
553 float sum;
554 if (m_tuning.fixedEffectiveMass)
555 {
556 sum = c.invEffectiveMass;
557 }
558 else
559 {
560 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
561 }
562
563 if (sum == 0.0f)
564 {
565 continue;
566 }
567
568 const float alpha = 1.0f / (c.spring * dt * dt);
569 const float beta = dt * dt * c.damper;
570 const float sigma = alpha * beta / dt;
571 float C = angle;
572
573 // This is using the initial velocities
574 float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2) + b2Dot(J3, dp3);
575
576 float B = C + alpha * c.lambda + sigma * Cdot;
577 float sum2 = (1.0f + sigma) * sum + alpha;
578
579 float impulse = -B / sum2;
580
581 p1 += (c.invMass1 * impulse) * J1;
582 p2 += (c.invMass2 * impulse) * J2;
583 p3 += (c.invMass3 * impulse) * J3;
584
585 m_ps[c.i1] = p1;
586 m_ps[c.i2] = p2;
587 m_ps[c.i3] = p3;
588 c.lambda += impulse;
589 }
590 }
591
ApplyBendForces(float dt)592 void b2Rope::ApplyBendForces(float dt)
593 {
594 // omega = 2 * pi * hz
595 const float omega = 2.0f * b2_pi * m_tuning.bendHertz;
596
597 for (int32 i = 0; i < m_bendCount; ++i)
598 {
599 const b2RopeBend& c = m_bendConstraints[i];
600
601 b2Vec2 p1 = m_ps[c.i1];
602 b2Vec2 p2 = m_ps[c.i2];
603 b2Vec2 p3 = m_ps[c.i3];
604
605 b2Vec2 v1 = m_vs[c.i1];
606 b2Vec2 v2 = m_vs[c.i2];
607 b2Vec2 v3 = m_vs[c.i3];
608
609 b2Vec2 d1 = p2 - p1;
610 b2Vec2 d2 = p3 - p2;
611
612 float L1sqr, L2sqr;
613
614 if (m_tuning.isometric)
615 {
616 L1sqr = c.L1 * c.L1;
617 L2sqr = c.L2 * c.L2;
618 }
619 else
620 {
621 L1sqr = d1.LengthSquared();
622 L2sqr = d2.LengthSquared();
623 }
624
625 if (L1sqr * L2sqr == 0.0f)
626 {
627 continue;
628 }
629
630 float a = b2Cross(d1, d2);
631 float b = b2Dot(d1, d2);
632
633 float angle = b2Atan2(a, b);
634
635 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
636 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
637
638 b2Vec2 J1 = -Jd1;
639 b2Vec2 J2 = Jd1 - Jd2;
640 b2Vec2 J3 = Jd2;
641
642 float sum;
643 if (m_tuning.fixedEffectiveMass)
644 {
645 sum = c.invEffectiveMass;
646 }
647 else
648 {
649 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
650 }
651
652 if (sum == 0.0f)
653 {
654 continue;
655 }
656
657 float mass = 1.0f / sum;
658
659 const float spring = mass * omega * omega;
660 const float damper = 2.0f * mass * m_tuning.bendDamping * omega;
661
662 float C = angle;
663 float Cdot = b2Dot(J1, v1) + b2Dot(J2, v2) + b2Dot(J3, v3);
664
665 float impulse = -dt * (spring * C + damper * Cdot);
666
667 m_vs[c.i1] += (c.invMass1 * impulse) * J1;
668 m_vs[c.i2] += (c.invMass2 * impulse) * J2;
669 m_vs[c.i3] += (c.invMass3 * impulse) * J3;
670 }
671 }
672
SolveBend_PBD_Distance()673 void b2Rope::SolveBend_PBD_Distance()
674 {
675 const float stiffness = m_tuning.bendStiffness;
676
677 for (int32 i = 0; i < m_bendCount; ++i)
678 {
679 const b2RopeBend& c = m_bendConstraints[i];
680
681 int32 i1 = c.i1;
682 int32 i2 = c.i3;
683
684 b2Vec2 p1 = m_ps[i1];
685 b2Vec2 p2 = m_ps[i2];
686
687 b2Vec2 d = p2 - p1;
688 float L = d.Normalize();
689
690 float sum = c.invMass1 + c.invMass3;
691 if (sum == 0.0f)
692 {
693 continue;
694 }
695
696 float s1 = c.invMass1 / sum;
697 float s2 = c.invMass3 / sum;
698
699 p1 -= stiffness * s1 * (c.L1 + c.L2 - L) * d;
700 p2 += stiffness * s2 * (c.L1 + c.L2 - L) * d;
701
702 m_ps[i1] = p1;
703 m_ps[i2] = p2;
704 }
705 }
706
707 // Constraint based implementation of:
708 // P. Volino: Simple Linear Bending Stiffness in Particle Systems
SolveBend_PBD_Height()709 void b2Rope::SolveBend_PBD_Height()
710 {
711 const float stiffness = m_tuning.bendStiffness;
712
713 for (int32 i = 0; i < m_bendCount; ++i)
714 {
715 const b2RopeBend& c = m_bendConstraints[i];
716
717 b2Vec2 p1 = m_ps[c.i1];
718 b2Vec2 p2 = m_ps[c.i2];
719 b2Vec2 p3 = m_ps[c.i3];
720
721 // Barycentric coordinates are held constant
722 b2Vec2 d = c.alpha1 * p1 + c.alpha2 * p3 - p2;
723 float dLen = d.Length();
724
725 if (dLen == 0.0f)
726 {
727 continue;
728 }
729
730 b2Vec2 dHat = (1.0f / dLen) * d;
731
732 b2Vec2 J1 = c.alpha1 * dHat;
733 b2Vec2 J2 = -dHat;
734 b2Vec2 J3 = c.alpha2 * dHat;
735
736 float sum = c.invMass1 * c.alpha1 * c.alpha1 + c.invMass2 + c.invMass3 * c.alpha2 * c.alpha2;
737
738 if (sum == 0.0f)
739 {
740 continue;
741 }
742
743 float C = dLen;
744 float mass = 1.0f / sum;
745 float impulse = -stiffness * mass * C;
746
747 p1 += (c.invMass1 * impulse) * J1;
748 p2 += (c.invMass2 * impulse) * J2;
749 p3 += (c.invMass3 * impulse) * J3;
750
751 m_ps[c.i1] = p1;
752 m_ps[c.i2] = p2;
753 m_ps[c.i3] = p3;
754 }
755 }
756
757 // M. Kelager: A Triangle Bending Constraint Model for PBD
SolveBend_PBD_Triangle()758 void b2Rope::SolveBend_PBD_Triangle()
759 {
760 const float stiffness = m_tuning.bendStiffness;
761
762 for (int32 i = 0; i < m_bendCount; ++i)
763 {
764 const b2RopeBend& c = m_bendConstraints[i];
765
766 b2Vec2 b0 = m_ps[c.i1];
767 b2Vec2 v = m_ps[c.i2];
768 b2Vec2 b1 = m_ps[c.i3];
769
770 float wb0 = c.invMass1;
771 float wv = c.invMass2;
772 float wb1 = c.invMass3;
773
774 float W = wb0 + wb1 + 2.0f * wv;
775 float invW = stiffness / W;
776
777 b2Vec2 d = v - (1.0f / 3.0f) * (b0 + v + b1);
778
779 b2Vec2 db0 = 2.0f * wb0 * invW * d;
780 b2Vec2 dv = -4.0f * wv * invW * d;
781 b2Vec2 db1 = 2.0f * wb1 * invW * d;
782
783 b0 += db0;
784 v += dv;
785 b1 += db1;
786
787 m_ps[c.i1] = b0;
788 m_ps[c.i2] = v;
789 m_ps[c.i3] = b1;
790 }
791 }
792
Draw(b2Draw * draw) const793 void b2Rope::Draw(b2Draw* draw) const
794 {
795 b2Color c(0.4f, 0.5f, 0.7f);
796 b2Color pg(0.1f, 0.8f, 0.1f);
797 b2Color pd(0.7f, 0.2f, 0.4f);
798
799 for (int32 i = 0; i < m_count - 1; ++i)
800 {
801 draw->DrawSegment(m_ps[i], m_ps[i+1], c);
802
803 const b2Color& pc = m_invMasses[i] > 0.0f ? pd : pg;
804 draw->DrawPoint(m_ps[i], 5.0f, pc);
805 }
806
807 const b2Color& pc = m_invMasses[m_count - 1] > 0.0f ? pd : pg;
808 draw->DrawPoint(m_ps[m_count - 1], 5.0f, pc);
809 }
810