· 2 years ago · Sep 30, 2023, 01:50 PM
1// Box2D Amalgamate
2// Usage: Include this header file for definition, define "BOX2D_IMPL" for implementation.
3
4
5
6
7// MIT License
8
9// Copyright (c) 2019 Erin Catto
10
11// Permission is hereby granted, free of charge, to any person obtaining a copy
12// of this software and associated documentation files (the "Software"), to deal
13// in the Software without restriction, including without limitation the rights
14// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15// copies of the Software, and to permit persons to whom the Software is
16// furnished to do so, subject to the following conditions:
17
18// The above copyright notice and this permission notice shall be included in all
19// copies or substantial portions of the Software.
20
21// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27// SOFTWARE.
28
29#ifndef B2_API_H
30#define B2_API_H
31
32#ifdef B2_SHARED
33#if defined _WIN32 || defined __CYGWIN__
34#ifdef box2d_EXPORTS
35#ifdef __GNUC__
36#define B2_API __attribute__ ((dllexport))
37#else
38#define B2_API __declspec(dllexport)
39#endif
40#else
41#ifdef __GNUC__
42#define B2_API __attribute__ ((dllimport))
43#else
44#define B2_API __declspec(dllimport)
45#endif
46#endif
47#else
48#if __GNUC__ >= 4
49#define B2_API __attribute__ ((visibility ("default")))
50#else
51#define B2_API
52#endif
53#endif
54#else
55#define B2_API
56#endif
57
58#endif
59// MIT License
60
61// Copyright (c) 2020 Erin Catto
62
63// Permission is hereby granted, free of charge, to any person obtaining a copy
64// of this software and associated documentation files (the "Software"), to deal
65// in the Software without restriction, including without limitation the rights
66// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
67// copies of the Software, and to permit persons to whom the Software is
68// furnished to do so, subject to the following conditions:
69
70// The above copyright notice and this permission notice shall be included in all
71// copies or substantial portions of the Software.
72
73// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
74// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
75// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
76// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
77// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
78// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
79// SOFTWARE.
80
81#ifndef B2_TYPES_H
82#define B2_TYPES_H
83
84typedef signed char int8;
85typedef signed short int16;
86typedef signed int int32;
87typedef unsigned char uint8;
88typedef unsigned short uint16;
89typedef unsigned int uint32;
90
91#endif
92// MIT License
93
94// Copyright (c) 2019 Erin Catto
95
96// Permission is hereby granted, free of charge, to any person obtaining a copy
97// of this software and associated documentation files (the "Software"), to deal
98// in the Software without restriction, including without limitation the rights
99// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
100// copies of the Software, and to permit persons to whom the Software is
101// furnished to do so, subject to the following conditions:
102
103// The above copyright notice and this permission notice shall be included in all
104// copies or substantial portions of the Software.
105
106// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
107// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
108// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
109// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
110// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
111// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
112// SOFTWARE.
113
114#ifndef B2_SETTINGS_H
115#define B2_SETTINGS_H
116
117//#include "b2_types.h"
118//#include "b2_api.h"
119
120/// @file
121/// Settings that can be overriden for your application
122///
123
124/// Define this macro in your build if you want to override settings
125#ifdef B2_USER_SETTINGS
126
127/// This is a user file that includes custom definitions of the macros, structs, and functions
128/// defined below.
129//#include "b2_user_settings.h"
130
131#else
132
133#include <stdarg.h>
134#include <stdint.h>
135
136// Tunable Constants
137
138/// You can use this to change the length scale used by your game.
139/// For example for inches you could use 39.4.
140#define b2_lengthUnitsPerMeter 1.0f
141
142/// The maximum number of vertices on a convex polygon. You cannot increase
143/// this too much because b2BlockAllocator has a maximum object size.
144#define b2_maxPolygonVertices 8
145
146// User data
147
148/// You can define this to inject whatever data you want in b2Body
149struct B2_API b2BodyUserData {
150 b2BodyUserData() {
151 pointer = 0;
152 }
153
154 /// For legacy compatibility
155 uintptr_t pointer;
156};
157
158/// You can define this to inject whatever data you want in b2Fixture
159struct B2_API b2FixtureUserData {
160 b2FixtureUserData() {
161 pointer = 0;
162 }
163
164 /// For legacy compatibility
165 uintptr_t pointer;
166};
167
168/// You can define this to inject whatever data you want in b2Joint
169struct B2_API b2JointUserData {
170 b2JointUserData() {
171 pointer = 0;
172 }
173
174 /// For legacy compatibility
175 uintptr_t pointer;
176};
177
178// Memory Allocation
179
180/// Default allocation functions
181B2_API void* b2Alloc_Default(int32 size);
182B2_API void b2Free_Default(void* mem);
183
184/// Implement this function to use your own memory allocator.
185inline void* b2Alloc(int32 size) {
186 return b2Alloc_Default(size);
187}
188
189/// If you implement b2Alloc, you should also implement this function.
190inline void b2Free(void* mem) {
191 b2Free_Default(mem);
192}
193
194/// Default logging function
195B2_API void b2Log_Default(const char* string, va_list args);
196
197/// Implement this to use your own logging.
198inline void b2Log(const char* string, ...) {
199 va_list args;
200 va_start(args, string);
201 b2Log_Default(string, args);
202 va_end(args);
203}
204
205
206/// The radius of the polygon/edge shape skin. This should not be modified. Making
207/// this smaller means polygons will have an insufficient buffer for continuous collision.
208/// Making it larger may create artifacts for vertex collision.
209#define b2_polygonRadius (2.0f * b2_linearSlop)
210
211
212#endif // B2_USER_SETTINGS
213
214//#include "b2_common.h"
215
216#endif
217// MIT License
218
219// Copyright (c) 2019 Erin Catto
220
221// Permission is hereby granted, free of charge, to any person obtaining a copy
222// of this software and associated documentation files (the "Software"), to deal
223// in the Software without restriction, including without limitation the rights
224// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
225// copies of the Software, and to permit persons to whom the Software is
226// furnished to do so, subject to the following conditions:
227
228// The above copyright notice and this permission notice shall be included in all
229// copies or substantial portions of the Software.
230
231// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
232// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
233// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
234// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
235// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
236// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
237// SOFTWARE.
238
239#ifndef B2_STACK_ALLOCATOR_H
240#define B2_STACK_ALLOCATOR_H
241
242//#include "b2_api.h"
243//#include "b2_settings.h"
244
245const int32 b2_stackSize = 100 * 1024; // 100k
246const int32 b2_maxStackEntries = 32;
247
248struct B2_API b2StackEntry {
249 char* data;
250 int32 size;
251 bool usedMalloc;
252};
253
254// This is a stack allocator used for fast per step allocations.
255// You must nest allocate/free pairs. The code will assert
256// if you try to interleave multiple allocate/free pairs.
257class B2_API b2StackAllocator {
258public:
259 b2StackAllocator();
260 ~b2StackAllocator();
261
262 void* Allocate(int32 size);
263 void Free(void* p);
264
265 int32 GetMaxAllocation() const;
266
267private:
268
269 char m_data[b2_stackSize];
270 int32 m_index;
271
272 int32 m_allocation;
273 int32 m_maxAllocation;
274
275 b2StackEntry m_entries[b2_maxStackEntries];
276 int32 m_entryCount;
277};
278
279#endif
280// MIT License
281
282// Copyright (c) 2019 Erin Catto
283
284// Permission is hereby granted, free of charge, to any person obtaining a copy
285// of this software and associated documentation files (the "Software"), to deal
286// in the Software without restriction, including without limitation the rights
287// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
288// copies of the Software, and to permit persons to whom the Software is
289// furnished to do so, subject to the following conditions:
290
291// The above copyright notice and this permission notice shall be included in all
292// copies or substantial portions of the Software.
293
294// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
295// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
296// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
297// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
298// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
299// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
300// SOFTWARE.
301
302#ifndef B2_BLOCK_ALLOCATOR_H
303#define B2_BLOCK_ALLOCATOR_H
304
305//#include "b2_api.h"
306//#include "b2_settings.h"
307
308const int32 b2_blockSizeCount = 14;
309
310struct b2Block;
311struct b2Chunk;
312
313/// This is a small object allocator used for allocating small
314/// objects that persist for more than one time step.
315/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
316class B2_API b2BlockAllocator {
317public:
318 b2BlockAllocator();
319 ~b2BlockAllocator();
320
321 /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
322 void* Allocate(int32 size);
323
324 /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
325 void Free(void* p, int32 size);
326
327 void Clear();
328
329private:
330
331 b2Chunk* m_chunks;
332 int32 m_chunkCount;
333 int32 m_chunkSpace;
334
335 b2Block* m_freeLists[b2_blockSizeCount];
336};
337
338#endif
339// MIT License
340
341// Copyright (c) 2019 Erin Catto
342
343// Permission is hereby granted, free of charge, to any person obtaining a copy
344// of this software and associated documentation files (the "Software"), to deal
345// in the Software without restriction, including without limitation the rights
346// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
347// copies of the Software, and to permit persons to whom the Software is
348// furnished to do so, subject to the following conditions:
349
350// The above copyright notice and this permission notice shall be included in all
351// copies or substantial portions of the Software.
352
353// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
354// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
355// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
356// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
357// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
358// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
359// SOFTWARE.
360
361#ifndef B2_MATH_H
362#define B2_MATH_H
363
364#include <math.h>
365#include <stddef.h>
366#include <assert.h>
367#include <float.h>
368
369#if !defined(NDEBUG)
370#define b2DEBUG
371#endif
372
373#define B2_NOT_USED(x) ((void)(x))
374#define b2Assert(A) assert(A)
375
376#define b2_maxFloat FLT_MAX
377#define b2_epsilon FLT_EPSILON
378#define b2_pi 3.14159265359f
379
380
381
382/// This is used to fatten AABBs in the dynamic tree. This allows proxies
383/// to move by a small amount without triggering a tree adjustment.
384/// This is in meters.
385#define b2_aabbExtension (0.1f * b2_lengthUnitsPerMeter)
386
387/// This is used to fatten AABBs in the dynamic tree. This is used to predict
388/// the future position based on the current displacement.
389/// This is a dimensionless multiplier.
390#define b2_aabbMultiplier 4.0f
391
392/// A small length used as a collision and constraint tolerance. Usually it is
393/// chosen to be numerically significant, but visually insignificant. In meters.
394#define b2_linearSlop (0.005f * b2_lengthUnitsPerMeter)
395
396/// A small angle used as a collision and constraint tolerance. Usually it is
397/// chosen to be numerically significant, but visually insignificant.
398#define b2_angularSlop (2.0f / 180.0f * b2_pi)
399
400/// Maximum number of sub-steps per contact in continuous physics simulation.
401#define b2_maxSubSteps 8
402
403
404/// The maximum number of contact points between two convex shapes. Do
405/// not change this value.
406#define b2_maxManifoldPoints 2
407
408//#include "b2_api.h"
409//#include "b2_settings.h"
410
411/// This function is used to ensure that a floating point number is not a NaN or infinity.
412inline bool b2IsValid(float x) {
413 return isfinite(x);
414}
415
416#define b2Sqrt(x) sqrtf(x)
417#define b2Atan2(y, x) atan2f(y, x)
418
419/// A 2D column vector.
420struct B2_API b2Vec2 {
421 /// Default constructor does nothing (for performance).
422 b2Vec2() {}
423
424 /// Construct using coordinates.
425 b2Vec2(float xIn, float yIn) : x(xIn), y(yIn) {}
426
427 /// Set this vector to all zeros.
428 void SetZero() { x = 0.0f; y = 0.0f; }
429
430 /// Set this vector to some specified coordinates.
431 void Set(float x_, float y_) { x = x_; y = y_; }
432
433 /// Negate this vector.
434 b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
435
436 /// Read from and indexed element.
437 float operator () (int32 i) const {
438 return (&x)[i];
439 }
440
441 /// Write to an indexed element.
442 float& operator () (int32 i) {
443 return (&x)[i];
444 }
445
446 /// Add a vector to this vector.
447 void operator += (const b2Vec2& v) {
448 x += v.x; y += v.y;
449 }
450
451 /// Subtract a vector from this vector.
452 void operator -= (const b2Vec2& v) {
453 x -= v.x; y -= v.y;
454 }
455
456 /// Multiply this vector by a scalar.
457 void operator *= (float a) {
458 x *= a; y *= a;
459 }
460
461 /// Get the length of this vector (the norm).
462 float Length() const {
463 return b2Sqrt(x * x + y * y);
464 }
465
466 /// Get the length squared. For performance, use this instead of
467 /// b2Vec2::Length (if possible).
468 float LengthSquared() const {
469 return x * x + y * y;
470 }
471
472 /// Convert this vector into a unit vector. Returns the length.
473 float Normalize() {
474 float length = Length();
475 if (length < b2_epsilon) {
476 return 0.0f;
477 }
478 float invLength = 1.0f / length;
479 x *= invLength;
480 y *= invLength;
481
482 return length;
483 }
484
485 /// Does this vector contain finite coordinates?
486 bool IsValid() const {
487 return b2IsValid(x) && b2IsValid(y);
488 }
489
490 /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
491 b2Vec2 Skew() const {
492 return b2Vec2(-y, x);
493 }
494
495 float x, y;
496};
497
498/// A 2D column vector with 3 elements.
499struct B2_API b2Vec3 {
500 /// Default constructor does nothing (for performance).
501 b2Vec3() {}
502
503 /// Construct using coordinates.
504 b2Vec3(float xIn, float yIn, float zIn) : x(xIn), y(yIn), z(zIn) {}
505
506 /// Set this vector to all zeros.
507 void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
508
509 /// Set this vector to some specified coordinates.
510 void Set(float x_, float y_, float z_) { x = x_; y = y_; z = z_; }
511
512 /// Negate this vector.
513 b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
514
515 /// Add a vector to this vector.
516 void operator += (const b2Vec3& v) {
517 x += v.x; y += v.y; z += v.z;
518 }
519
520 /// Subtract a vector from this vector.
521 void operator -= (const b2Vec3& v) {
522 x -= v.x; y -= v.y; z -= v.z;
523 }
524
525 /// Multiply this vector by a scalar.
526 void operator *= (float s) {
527 x *= s; y *= s; z *= s;
528 }
529
530 float x, y, z;
531};
532
533/// A 2-by-2 matrix. Stored in column-major order.
534struct B2_API b2Mat22 {
535 /// The default constructor does nothing (for performance).
536 b2Mat22() {}
537
538 /// Construct this matrix using columns.
539 b2Mat22(const b2Vec2& c1, const b2Vec2& c2) {
540 ex = c1;
541 ey = c2;
542 }
543
544 /// Construct this matrix using scalars.
545 b2Mat22(float a11, float a12, float a21, float a22) {
546 ex.x = a11; ex.y = a21;
547 ey.x = a12; ey.y = a22;
548 }
549
550 /// Initialize this matrix using columns.
551 void Set(const b2Vec2& c1, const b2Vec2& c2) {
552 ex = c1;
553 ey = c2;
554 }
555
556 /// Set this to the identity matrix.
557 void SetIdentity() {
558 ex.x = 1.0f; ey.x = 0.0f;
559 ex.y = 0.0f; ey.y = 1.0f;
560 }
561
562 /// Set this matrix to all zeros.
563 void SetZero() {
564 ex.x = 0.0f; ey.x = 0.0f;
565 ex.y = 0.0f; ey.y = 0.0f;
566 }
567
568 b2Mat22 GetInverse() const {
569 float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
570 b2Mat22 B;
571 float det = a * d - b * c;
572 if (det != 0.0f) {
573 det = 1.0f / det;
574 }
575 B.ex.x = det * d; B.ey.x = -det * b;
576 B.ex.y = -det * c; B.ey.y = det * a;
577 return B;
578 }
579
580 /// Solve A * x = b, where b is a column vector. This is more efficient
581 /// than computing the inverse in one-shot cases.
582 b2Vec2 Solve(const b2Vec2& b) const {
583 float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
584 float det = a11 * a22 - a12 * a21;
585 if (det != 0.0f) {
586 det = 1.0f / det;
587 }
588 b2Vec2 x;
589 x.x = det * (a22 * b.x - a12 * b.y);
590 x.y = det * (a11 * b.y - a21 * b.x);
591 return x;
592 }
593
594 b2Vec2 ex, ey;
595};
596
597/// A 3-by-3 matrix. Stored in column-major order.
598struct B2_API b2Mat33 {
599 /// The default constructor does nothing (for performance).
600 b2Mat33() {}
601
602 /// Construct this matrix using columns.
603 b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) {
604 ex = c1;
605 ey = c2;
606 ez = c3;
607 }
608
609 /// Set this matrix to all zeros.
610 void SetZero() {
611 ex.SetZero();
612 ey.SetZero();
613 ez.SetZero();
614 }
615
616 /// Solve A * x = b, where b is a column vector. This is more efficient
617 /// than computing the inverse in one-shot cases.
618 b2Vec3 Solve33(const b2Vec3& b) const;
619
620 /// Solve A * x = b, where b is a column vector. This is more efficient
621 /// than computing the inverse in one-shot cases. Solve only the upper
622 /// 2-by-2 matrix equation.
623 b2Vec2 Solve22(const b2Vec2& b) const;
624
625 /// Get the inverse of this matrix as a 2-by-2.
626 /// Returns the zero matrix if singular.
627 void GetInverse22(b2Mat33* M) const;
628
629 /// Get the symmetric inverse of this matrix as a 3-by-3.
630 /// Returns the zero matrix if singular.
631 void GetSymInverse33(b2Mat33* M) const;
632
633 b2Vec3 ex, ey, ez;
634};
635
636/// Rotation
637struct B2_API b2Rot {
638 b2Rot() {}
639
640 /// Initialize from an angle in radians
641 explicit b2Rot(float angle) {
642 /// TODO_ERIN optimize
643 s = sinf(angle);
644 c = cosf(angle);
645 }
646
647 /// Set using an angle in radians.
648 void Set(float angle) {
649 /// TODO_ERIN optimize
650 s = sinf(angle);
651 c = cosf(angle);
652 }
653
654 /// Set to the identity rotation
655 void SetIdentity() {
656 s = 0.0f;
657 c = 1.0f;
658 }
659
660 /// Get the angle in radians
661 float GetAngle() const {
662 return b2Atan2(s, c);
663 }
664
665 /// Get the x-axis
666 b2Vec2 GetXAxis() const {
667 return b2Vec2(c, s);
668 }
669
670 /// Get the u-axis
671 b2Vec2 GetYAxis() const {
672 return b2Vec2(-s, c);
673 }
674
675 /// Sine and cosine
676 float s, c;
677};
678
679/// A transform contains translation and rotation. It is used to represent
680/// the position and orientation of rigid frames.
681struct B2_API b2Transform {
682 /// The default constructor does nothing.
683 b2Transform() {}
684
685 /// Initialize using a position vector and a rotation.
686 b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
687
688 /// Set this to the identity transform.
689 void SetIdentity() {
690 p.SetZero();
691 q.SetIdentity();
692 }
693
694 /// Set this based on the position and angle.
695 void Set(const b2Vec2& position, float angle) {
696 p = position;
697 q.Set(angle);
698 }
699
700 b2Vec2 p;
701 b2Rot q;
702};
703
704/// This describes the motion of a body/shape for TOI computation.
705/// Shapes are defined with respect to the body origin, which may
706/// no coincide with the center of mass. However, to support dynamics
707/// we must interpolate the center of mass position.
708struct B2_API b2Sweep {
709 /// Get the interpolated transform at a specific time.
710 /// @param transform the output transform
711 /// @param beta is a factor in [0,1], where 0 indicates alpha0.
712 void GetTransform(b2Transform* transform, float beta) const;
713
714 /// Advance the sweep forward, yielding a new initial state.
715 /// @param alpha the new initial time.
716 void Advance(float alpha);
717
718 /// Normalize the angles.
719 void Normalize();
720
721 b2Vec2 localCenter; ///< local center of mass position
722 b2Vec2 c0, c; ///< center world positions
723 float a0, a; ///< world angles
724
725 /// Fraction of the current time step in the range [0,1]
726 /// c0 and a0 are the positions at alpha0.
727 float alpha0;
728};
729
730/// Useful constant
731extern B2_API const b2Vec2 b2Vec2_zero;
732
733/// Perform the dot product on two vectors.
734inline float b2Dot(const b2Vec2& a, const b2Vec2& b) {
735 return a.x * b.x + a.y * b.y;
736}
737
738/// Perform the cross product on two vectors. In 2D this produces a scalar.
739inline float b2Cross(const b2Vec2& a, const b2Vec2& b) {
740 return a.x * b.y - a.y * b.x;
741}
742
743/// Perform the cross product on a vector and a scalar. In 2D this produces
744/// a vector.
745inline b2Vec2 b2Cross(const b2Vec2& a, float s) {
746 return b2Vec2(s * a.y, -s * a.x);
747}
748
749/// Perform the cross product on a scalar and a vector. In 2D this produces
750/// a vector.
751inline b2Vec2 b2Cross(float s, const b2Vec2& a) {
752 return b2Vec2(-s * a.y, s * a.x);
753}
754
755/// Multiply a matrix times a vector. If a rotation matrix is provided,
756/// then this transforms the vector from one frame to another.
757inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) {
758 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
759}
760
761/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
762/// then this transforms the vector from one frame to another (inverse transform).
763inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) {
764 return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
765}
766
767/// Add two vectors component-wise.
768inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) {
769 return b2Vec2(a.x + b.x, a.y + b.y);
770}
771
772/// Subtract two vectors component-wise.
773inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) {
774 return b2Vec2(a.x - b.x, a.y - b.y);
775}
776
777inline b2Vec2 operator * (float s, const b2Vec2& a) {
778 return b2Vec2(s * a.x, s * a.y);
779}
780
781inline bool operator == (const b2Vec2& a, const b2Vec2& b) {
782 return a.x == b.x && a.y == b.y;
783}
784
785inline bool operator != (const b2Vec2& a, const b2Vec2& b) {
786 return a.x != b.x || a.y != b.y;
787}
788
789inline float b2Distance(const b2Vec2& a, const b2Vec2& b) {
790 b2Vec2 c = a - b;
791 return c.Length();
792}
793
794inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) {
795 b2Vec2 c = a - b;
796 return b2Dot(c, c);
797}
798
799inline b2Vec3 operator * (float s, const b2Vec3& a) {
800 return b2Vec3(s * a.x, s * a.y, s * a.z);
801}
802
803/// Add two vectors component-wise.
804inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) {
805 return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
806}
807
808/// Subtract two vectors component-wise.
809inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) {
810 return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
811}
812
813/// Perform the dot product on two vectors.
814inline float b2Dot(const b2Vec3& a, const b2Vec3& b) {
815 return a.x * b.x + a.y * b.y + a.z * b.z;
816}
817
818/// Perform the cross product on two vectors.
819inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) {
820 return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
821}
822
823inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) {
824 return b2Mat22(A.ex + B.ex, A.ey + B.ey);
825}
826
827// A * B
828inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) {
829 return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
830}
831
832// A^T * B
833inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) {
834 b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
835 b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
836 return b2Mat22(c1, c2);
837}
838
839/// Multiply a matrix times a vector.
840inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) {
841 return v.x * A.ex + v.y * A.ey + v.z * A.ez;
842}
843
844/// Multiply a matrix times a vector.
845inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) {
846 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
847}
848
849/// Multiply two rotations: q * r
850inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) {
851 // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
852 // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
853 // s = qs * rc + qc * rs
854 // c = qc * rc - qs * rs
855 b2Rot qr;
856 qr.s = q.s * r.c + q.c * r.s;
857 qr.c = q.c * r.c - q.s * r.s;
858 return qr;
859}
860
861/// Transpose multiply two rotations: qT * r
862inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) {
863 // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
864 // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
865 // s = qc * rs - qs * rc
866 // c = qc * rc + qs * rs
867 b2Rot qr;
868 qr.s = q.c * r.s - q.s * r.c;
869 qr.c = q.c * r.c + q.s * r.s;
870 return qr;
871}
872
873/// Rotate a vector
874inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) {
875 return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
876}
877
878/// Inverse rotate a vector
879inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) {
880 return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
881}
882
883inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) {
884 float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
885 float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
886
887 return b2Vec2(x, y);
888}
889
890inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) {
891 float px = v.x - T.p.x;
892 float py = v.y - T.p.y;
893 float x = (T.q.c * px + T.q.s * py);
894 float y = (-T.q.s * px + T.q.c * py);
895
896 return b2Vec2(x, y);
897}
898
899// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
900// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
901inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) {
902 b2Transform C;
903 C.q = b2Mul(A.q, B.q);
904 C.p = b2Mul(A.q, B.p) + A.p;
905 return C;
906}
907
908// v2 = A.q' * (B.q * v1 + B.p - A.p)
909// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
910inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) {
911 b2Transform C;
912 C.q = b2MulT(A.q, B.q);
913 C.p = b2MulT(A.q, B.p - A.p);
914 return C;
915}
916
917template <typename T>
918inline T b2Abs(T a) {
919 return a > T(0) ? a : -a;
920}
921
922inline b2Vec2 b2Abs(const b2Vec2& a) {
923 return b2Vec2(b2Abs(a.x), b2Abs(a.y));
924}
925
926inline b2Mat22 b2Abs(const b2Mat22& A) {
927 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
928}
929
930template <typename T>
931inline T b2Min(T a, T b) {
932 return a < b ? a : b;
933}
934
935inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) {
936 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
937}
938
939template <typename T>
940inline T b2Max(T a, T b) {
941 return a > b ? a : b;
942}
943
944inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) {
945 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
946}
947
948template <typename T>
949inline T b2Clamp(T a, T low, T high) {
950 return b2Max(low, b2Min(a, high));
951}
952
953inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) {
954 return b2Max(low, b2Min(a, high));
955}
956
957template<typename T> inline void b2Swap(T& a, T& b) {
958 T tmp = a;
959 a = b;
960 b = tmp;
961}
962
963/// "Next Largest Power of 2
964/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
965/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
966/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
967/// largest power of 2. For a 32-bit value:"
968inline uint32 b2NextPowerOfTwo(uint32 x) {
969 x |= (x >> 1);
970 x |= (x >> 2);
971 x |= (x >> 4);
972 x |= (x >> 8);
973 x |= (x >> 16);
974 return x + 1;
975}
976
977inline bool b2IsPowerOfTwo(uint32 x) {
978 bool result = x > 0 && (x & (x - 1)) == 0;
979 return result;
980}
981
982// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
983inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const {
984 xf->p = (1.0f - beta) * c0 + beta * c;
985 float angle = (1.0f - beta) * a0 + beta * a;
986 xf->q.Set(angle);
987
988 // Shift to origin
989 xf->p -= b2Mul(xf->q, localCenter);
990}
991
992inline void b2Sweep::Advance(float alpha) {
993 b2Assert(alpha0 < 1.0f);
994 float beta = (alpha - alpha0) / (1.0f - alpha0);
995 c0 += beta * (c - c0);
996 a0 += beta * (a - a0);
997 alpha0 = alpha;
998}
999
1000/// Normalize an angle in radians to be between -pi and pi
1001inline void b2Sweep::Normalize() {
1002 float twoPi = 2.0f * b2_pi;
1003 float d = twoPi * floorf(a0 / twoPi);
1004 a0 -= d;
1005 a -= d;
1006}
1007
1008#endif
1009// MIT License
1010
1011// Copyright (c) 2019 Erin Catto
1012
1013// Permission is hereby granted, free of charge, to any person obtaining a copy
1014// of this software and associated documentation files (the "Software"), to deal
1015// in the Software without restriction, including without limitation the rights
1016// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
1017// copies of the Software, and to permit persons to whom the Software is
1018// furnished to do so, subject to the following conditions:
1019
1020// The above copyright notice and this permission notice shall be included in all
1021// copies or substantial portions of the Software.
1022
1023// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
1024// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
1025// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
1026// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
1027// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
1028// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
1029// SOFTWARE.
1030
1031#ifndef B2_COLLISION_H
1032#define B2_COLLISION_H
1033
1034#include <limits.h>
1035
1036//#include "b2_api.h"
1037//#include "b2_math.h"
1038
1039/// @file
1040/// Structures and functions used for computing contact points, distance
1041/// queries, and TOI queries.
1042
1043class b2Shape;
1044class b2CircleShape;
1045class b2EdgeShape;
1046class b2PolygonShape;
1047
1048const uint8 b2_nullFeature = UCHAR_MAX;
1049
1050/// The features that intersect to form the contact point
1051/// This must be 4 bytes or less.
1052struct B2_API b2ContactFeature {
1053 enum Type {
1054 e_vertex = 0,
1055 e_face = 1
1056 };
1057
1058 uint8 indexA; ///< Feature index on shapeA
1059 uint8 indexB; ///< Feature index on shapeB
1060 uint8 typeA; ///< The feature type on shapeA
1061 uint8 typeB; ///< The feature type on shapeB
1062};
1063
1064/// Contact ids to facilitate warm starting.
1065union B2_API b2ContactID {
1066 b2ContactFeature cf;
1067 uint32 key; ///< Used to quickly compare contact ids.
1068};
1069
1070/// A manifold point is a contact point belonging to a contact
1071/// manifold. It holds details related to the geometry and dynamics
1072/// of the contact points.
1073/// The local point usage depends on the manifold type:
1074/// -e_circles: the local center of circleB
1075/// -e_faceA: the local center of cirlceB or the clip point of polygonB
1076/// -e_faceB: the clip point of polygonA
1077/// This structure is stored across time steps, so we keep it small.
1078/// Note: the impulses are used for internal caching and may not
1079/// provide reliable contact forces, especially for high speed collisions.
1080struct B2_API b2ManifoldPoint {
1081 b2Vec2 localPoint; ///< usage depends on manifold type
1082 float normalImpulse; ///< the non-penetration impulse
1083 float tangentImpulse; ///< the friction impulse
1084 b2ContactID id; ///< uniquely identifies a contact point between two shapes
1085};
1086
1087/// A manifold for two touching convex shapes.
1088/// Box2D supports multiple types of contact:
1089/// - clip point versus plane with radius
1090/// - point versus point with radius (circles)
1091/// The local point usage depends on the manifold type:
1092/// -e_circles: the local center of circleA
1093/// -e_faceA: the center of faceA
1094/// -e_faceB: the center of faceB
1095/// Similarly the local normal usage:
1096/// -e_circles: not used
1097/// -e_faceA: the normal on polygonA
1098/// -e_faceB: the normal on polygonB
1099/// We store contacts in this way so that position correction can
1100/// account for movement, which is critical for continuous physics.
1101/// All contact scenarios must be expressed in one of these types.
1102/// This structure is stored across time steps, so we keep it small.
1103struct B2_API b2Manifold {
1104 enum Type {
1105 e_circles,
1106 e_faceA,
1107 e_faceB
1108 };
1109
1110 b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact
1111 b2Vec2 localNormal; ///< not use for Type::e_points
1112 b2Vec2 localPoint; ///< usage depends on manifold type
1113 Type type;
1114 int32 pointCount; ///< the number of manifold points
1115};
1116
1117/// This is used to compute the current state of a contact manifold.
1118struct B2_API b2WorldManifold {
1119 /// Evaluate the manifold with supplied transforms. This assumes
1120 /// modest motion from the original state. This does not change the
1121 /// point count, impulses, etc. The radii must come from the shapes
1122 /// that generated the manifold.
1123 void Initialize(const b2Manifold* manifold,
1124 const b2Transform& xfA, float radiusA,
1125 const b2Transform& xfB, float radiusB);
1126
1127 b2Vec2 normal; ///< world vector pointing from A to B
1128 b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection)
1129 float separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters
1130};
1131
1132/// This is used for determining the state of contact points.
1133enum b2PointState {
1134 b2_nullState, ///< point does not exist
1135 b2_addState, ///< point was added in the update
1136 b2_persistState, ///< point persisted across the update
1137 b2_removeState ///< point was removed in the update
1138};
1139
1140/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
1141/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
1142B2_API void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
1143 const b2Manifold* manifold1, const b2Manifold* manifold2);
1144
1145/// Used for computing contact manifolds.
1146struct B2_API b2ClipVertex {
1147 b2Vec2 v;
1148 b2ContactID id;
1149};
1150
1151/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
1152struct B2_API b2RayCastInput {
1153 b2Vec2 p1, p2;
1154 float maxFraction;
1155};
1156
1157/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
1158/// come from b2RayCastInput.
1159struct B2_API b2RayCastOutput {
1160 b2Vec2 normal;
1161 float fraction;
1162};
1163
1164/// An axis aligned bounding box.
1165struct B2_API b2AABB {
1166 /// Verify that the bounds are sorted.
1167 bool IsValid() const;
1168
1169 /// Get the center of the AABB.
1170 b2Vec2 GetCenter() const {
1171 return 0.5f * (lowerBound + upperBound);
1172 }
1173
1174 /// Get the extents of the AABB (half-widths).
1175 b2Vec2 GetExtents() const {
1176 return 0.5f * (upperBound - lowerBound);
1177 }
1178
1179 /// Get the perimeter length
1180 float GetPerimeter() const {
1181 float wx = upperBound.x - lowerBound.x;
1182 float wy = upperBound.y - lowerBound.y;
1183 return 2.0f * (wx + wy);
1184 }
1185
1186 /// Combine an AABB into this one.
1187 void Combine(const b2AABB& aabb) {
1188 lowerBound = b2Min(lowerBound, aabb.lowerBound);
1189 upperBound = b2Max(upperBound, aabb.upperBound);
1190 }
1191
1192 /// Combine two AABBs into this one.
1193 void Combine(const b2AABB& aabb1, const b2AABB& aabb2) {
1194 lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
1195 upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
1196 }
1197
1198 /// Does this aabb contain the provided AABB.
1199 bool Contains(const b2AABB& aabb) const {
1200 bool result = true;
1201 result = result && lowerBound.x <= aabb.lowerBound.x;
1202 result = result && lowerBound.y <= aabb.lowerBound.y;
1203 result = result && aabb.upperBound.x <= upperBound.x;
1204 result = result && aabb.upperBound.y <= upperBound.y;
1205 return result;
1206 }
1207
1208 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
1209
1210 b2Vec2 lowerBound; ///< the lower vertex
1211 b2Vec2 upperBound; ///< the upper vertex
1212};
1213
1214/// Compute the collision manifold between two circles.
1215B2_API void b2CollideCircles(b2Manifold* manifold,
1216 const b2CircleShape* circleA, const b2Transform& xfA,
1217 const b2CircleShape* circleB, const b2Transform& xfB);
1218
1219/// Compute the collision manifold between a polygon and a circle.
1220B2_API void b2CollidePolygonAndCircle(b2Manifold* manifold,
1221 const b2PolygonShape* polygonA, const b2Transform& xfA,
1222 const b2CircleShape* circleB, const b2Transform& xfB);
1223
1224/// Compute the collision manifold between two polygons.
1225B2_API void b2CollidePolygons(b2Manifold* manifold,
1226 const b2PolygonShape* polygonA, const b2Transform& xfA,
1227 const b2PolygonShape* polygonB, const b2Transform& xfB);
1228
1229/// Compute the collision manifold between an edge and a circle.
1230B2_API void b2CollideEdgeAndCircle(b2Manifold* manifold,
1231 const b2EdgeShape* polygonA, const b2Transform& xfA,
1232 const b2CircleShape* circleB, const b2Transform& xfB);
1233
1234/// Compute the collision manifold between an edge and a polygon.
1235B2_API void b2CollideEdgeAndPolygon(b2Manifold* manifold,
1236 const b2EdgeShape* edgeA, const b2Transform& xfA,
1237 const b2PolygonShape* circleB, const b2Transform& xfB);
1238
1239/// Clipping for contact manifolds.
1240B2_API int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
1241 const b2Vec2& normal, float offset, int32 vertexIndexA);
1242
1243/// Determine if two generic shapes overlap.
1244B2_API bool b2TestOverlap(const b2Shape* shapeA, int32 indexA,
1245 const b2Shape* shapeB, int32 indexB,
1246 const b2Transform& xfA, const b2Transform& xfB);
1247
1248// ---------------- Inline Functions ------------------------------------------
1249
1250inline bool b2AABB::IsValid() const {
1251 b2Vec2 d = upperBound - lowerBound;
1252 bool valid = d.x >= 0.0f && d.y >= 0.0f;
1253 valid = valid && lowerBound.IsValid() && upperBound.IsValid();
1254 return valid;
1255}
1256
1257inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) {
1258 b2Vec2 d1, d2;
1259 d1 = b.lowerBound - a.upperBound;
1260 d2 = a.lowerBound - b.upperBound;
1261
1262 if (d1.x > 0.0f || d1.y > 0.0f)
1263 return false;
1264
1265 if (d2.x > 0.0f || d2.y > 0.0f)
1266 return false;
1267
1268 return true;
1269}
1270
1271#endif
1272// MIT License
1273
1274// Copyright (c) 2019 Erin Catto
1275
1276// Permission is hereby granted, free of charge, to any person obtaining a copy
1277// of this software and associated documentation files (the "Software"), to deal
1278// in the Software without restriction, including without limitation the rights
1279// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
1280// copies of the Software, and to permit persons to whom the Software is
1281// furnished to do so, subject to the following conditions:
1282
1283// The above copyright notice and this permission notice shall be included in all
1284// copies or substantial portions of the Software.
1285
1286// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
1287// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
1288// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
1289// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
1290// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
1291// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
1292// SOFTWARE.
1293
1294#ifndef B2_SHAPE_H
1295#define B2_SHAPE_H
1296
1297//#include "b2_api.h"
1298//#include "b2_math.h"
1299//#include "b2_collision.h"
1300
1301class b2BlockAllocator;
1302
1303/// This holds the mass data computed for a shape.
1304struct B2_API b2MassData {
1305 /// The mass of the shape, usually in kilograms.
1306 float mass;
1307
1308 /// The position of the shape's centroid relative to the shape's origin.
1309 b2Vec2 center;
1310
1311 /// The rotational inertia of the shape about the local origin.
1312 float I;
1313};
1314
1315/// A shape is used for collision detection. You can create a shape however you like.
1316/// Shapes used for simulation in b2World are created automatically when a b2Fixture
1317/// is created. Shapes may encapsulate a one or more child shapes.
1318class B2_API b2Shape {
1319public:
1320
1321 enum Type {
1322 e_circle = 0,
1323 e_edge = 1,
1324 e_polygon = 2,
1325 e_chain = 3,
1326 e_typeCount = 4
1327 };
1328
1329 virtual ~b2Shape() {}
1330
1331 /// Clone the concrete shape using the provided allocator.
1332 virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
1333
1334 /// Get the type of this shape. You can use this to down cast to the concrete shape.
1335 /// @return the shape type.
1336 Type GetType() const;
1337
1338 /// Get the number of child primitives.
1339 virtual int32 GetChildCount() const = 0;
1340
1341 /// Test a point for containment in this shape. This only works for convex shapes.
1342 /// @param xf the shape world transform.
1343 /// @param p a point in world coordinates.
1344 virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
1345
1346 /// Cast a ray against a child shape.
1347 /// @param output the ray-cast results.
1348 /// @param input the ray-cast input parameters.
1349 /// @param transform the transform to be applied to the shape.
1350 /// @param childIndex the child shape index
1351 virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
1352 const b2Transform& transform, int32 childIndex) const = 0;
1353
1354 /// Given a transform, compute the associated axis aligned bounding box for a child shape.
1355 /// @param aabb returns the axis aligned box.
1356 /// @param xf the world transform of the shape.
1357 /// @param childIndex the child shape
1358 virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
1359
1360 /// Compute the mass properties of this shape using its dimensions and density.
1361 /// The inertia tensor is computed about the local origin.
1362 /// @param massData returns the mass data for this shape.
1363 /// @param density the density in kilograms per meter squared.
1364 virtual void ComputeMass(b2MassData* massData, float density) const = 0;
1365
1366 Type m_type;
1367
1368 /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for
1369 /// making rounded polygons.
1370 float m_radius;
1371};
1372
1373inline b2Shape::Type b2Shape::GetType() const {
1374 return m_type;
1375}
1376
1377#endif
1378// MIT License
1379
1380// Copyright (c) 2019 Erin Catto
1381
1382// Permission is hereby granted, free of charge, to any person obtaining a copy
1383// of this software and associated documentation files (the "Software"), to deal
1384// in the Software without restriction, including without limitation the rights
1385// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
1386// copies of the Software, and to permit persons to whom the Software is
1387// furnished to do so, subject to the following conditions:
1388
1389// The above copyright notice and this permission notice shall be included in all
1390// copies or substantial portions of the Software.
1391
1392// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
1393// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
1394// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
1395// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
1396// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
1397// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
1398// SOFTWARE.
1399
1400#ifndef B2_BODY_H
1401#define B2_BODY_H
1402
1403//#include "b2_api.h"
1404//#include "b2_math.h"
1405//#include "b2_shape.h"
1406
1407class b2Fixture;
1408class b2Joint;
1409class b2Contact;
1410class b2Controller;
1411class b2World;
1412struct b2FixtureDef;
1413struct b2JointEdge;
1414struct b2ContactEdge;
1415
1416/// The body type.
1417/// static: zero mass, zero velocity, may be manually moved
1418/// kinematic: zero mass, non-zero velocity set by user, moved by solver
1419/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
1420enum b2BodyType {
1421 b2_staticBody = 0,
1422 b2_kinematicBody,
1423 b2_dynamicBody
1424};
1425
1426/// A body definition holds all the data needed to construct a rigid body.
1427/// You can safely re-use body definitions. Shapes are added to a body after construction.
1428struct B2_API b2BodyDef {
1429 /// This constructor sets the body definition default values.
1430 b2BodyDef() {
1431 position.Set(0.0f, 0.0f);
1432 angle = 0.0f;
1433 linearVelocity.Set(0.0f, 0.0f);
1434 angularVelocity = 0.0f;
1435 linearDamping = 0.0f;
1436 angularDamping = 0.0f;
1437 allowSleep = true;
1438 awake = true;
1439 fixedRotation = false;
1440 bullet = false;
1441 type = b2_staticBody;
1442 enabled = true;
1443 gravityScale = 1.0f;
1444 }
1445
1446 /// The body type: static, kinematic, or dynamic.
1447 /// Note: if a dynamic body would have zero mass, the mass is set to one.
1448 b2BodyType type;
1449
1450 /// The world position of the body. Avoid creating bodies at the origin
1451 /// since this can lead to many overlapping shapes.
1452 b2Vec2 position;
1453
1454 /// The world angle of the body in radians.
1455 float angle;
1456
1457 /// The linear velocity of the body's origin in world co-ordinates.
1458 b2Vec2 linearVelocity;
1459
1460 /// The angular velocity of the body.
1461 float angularVelocity;
1462
1463 /// Linear damping is use to reduce the linear velocity. The damping parameter
1464 /// can be larger than 1.0f but the damping effect becomes sensitive to the
1465 /// time step when the damping parameter is large.
1466 /// Units are 1/time
1467 float linearDamping;
1468
1469 /// Angular damping is use to reduce the angular velocity. The damping parameter
1470 /// can be larger than 1.0f but the damping effect becomes sensitive to the
1471 /// time step when the damping parameter is large.
1472 /// Units are 1/time
1473 float angularDamping;
1474
1475 /// Set this flag to false if this body should never fall asleep. Note that
1476 /// this increases CPU usage.
1477 bool allowSleep;
1478
1479 /// Is this body initially awake or sleeping?
1480 bool awake;
1481
1482 /// Should this body be prevented from rotating? Useful for characters.
1483 bool fixedRotation;
1484
1485 /// Is this a fast moving body that should be prevented from tunneling through
1486 /// other moving bodies? Note that all bodies are prevented from tunneling through
1487 /// kinematic and static bodies. This setting is only considered on dynamic bodies.
1488 /// @warning You should use this flag sparingly since it increases processing time.
1489 bool bullet;
1490
1491 /// Does this body start out enabled?
1492 bool enabled;
1493
1494 /// Use this to store application specific body data.
1495 b2BodyUserData userData;
1496
1497 /// Scale the gravity applied to this body.
1498 float gravityScale;
1499};
1500
1501/// A rigid body. These are created via b2World::CreateBody.
1502class B2_API b2Body {
1503public:
1504 /// Creates a fixture and attach it to this body. Use this function if you need
1505 /// to set some fixture parameters, like friction. Otherwise you can create the
1506 /// fixture directly from a shape.
1507 /// If the density is non-zero, this function automatically updates the mass of the body.
1508 /// Contacts are not created until the next time step.
1509 /// @param def the fixture definition.
1510 /// @warning This function is locked during callbacks.
1511 b2Fixture* CreateFixture(const b2FixtureDef* def);
1512
1513 /// Creates a fixture from a shape and attach it to this body.
1514 /// This is a convenience function. Use b2FixtureDef if you need to set parameters
1515 /// like friction, restitution, user data, or filtering.
1516 /// If the density is non-zero, this function automatically updates the mass of the body.
1517 /// @param shape the shape to be cloned.
1518 /// @param density the shape density (set to zero for static bodies).
1519 /// @warning This function is locked during callbacks.
1520 b2Fixture* CreateFixture(const b2Shape* shape, float density);
1521
1522 /// Destroy a fixture. This removes the fixture from the broad-phase and
1523 /// destroys all contacts associated with this fixture. This will
1524 /// automatically adjust the mass of the body if the body is dynamic and the
1525 /// fixture has positive density.
1526 /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
1527 /// @param fixture the fixture to be removed.
1528 /// @warning This function is locked during callbacks.
1529 void DestroyFixture(b2Fixture* fixture);
1530
1531 /// Set the position of the body's origin and rotation.
1532 /// Manipulating a body's transform may cause non-physical behavior.
1533 /// Note: contacts are updated on the next call to b2World::Step.
1534 /// @param position the world position of the body's local origin.
1535 /// @param angle the world rotation in radians.
1536 void SetTransform(const b2Vec2& position, float angle);
1537
1538 /// Get the body transform for the body's origin.
1539 /// @return the world transform of the body's origin.
1540 const b2Transform& GetTransform() const;
1541
1542 /// Get the world body origin position.
1543 /// @return the world position of the body's origin.
1544 const b2Vec2& GetPosition() const;
1545
1546 /// Get the angle in radians.
1547 /// @return the current world rotation angle in radians.
1548 float GetAngle() const;
1549
1550 /// Get the world position of the center of mass.
1551 const b2Vec2& GetWorldCenter() const;
1552
1553 /// Get the local position of the center of mass.
1554 const b2Vec2& GetLocalCenter() const;
1555
1556 /// Set the linear velocity of the center of mass.
1557 /// @param v the new linear velocity of the center of mass.
1558 void SetLinearVelocity(const b2Vec2& v);
1559
1560 /// Get the linear velocity of the center of mass.
1561 /// @return the linear velocity of the center of mass.
1562 const b2Vec2& GetLinearVelocity() const;
1563
1564 /// Set the angular velocity.
1565 /// @param omega the new angular velocity in radians/second.
1566 void SetAngularVelocity(float omega);
1567
1568 /// Get the angular velocity.
1569 /// @return the angular velocity in radians/second.
1570 float GetAngularVelocity() const;
1571
1572 /// Apply a force at a world point. If the force is not
1573 /// applied at the center of mass, it will generate a torque and
1574 /// affect the angular velocity. This wakes up the body.
1575 /// @param force the world force vector, usually in Newtons (N).
1576 /// @param point the world position of the point of application.
1577 /// @param wake also wake up the body
1578 void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake);
1579
1580 /// Apply a force to the center of mass. This wakes up the body.
1581 /// @param force the world force vector, usually in Newtons (N).
1582 /// @param wake also wake up the body
1583 void ApplyForceToCenter(const b2Vec2& force, bool wake);
1584
1585 /// Apply a torque. This affects the angular velocity
1586 /// without affecting the linear velocity of the center of mass.
1587 /// @param torque about the z-axis (out of the screen), usually in N-m.
1588 /// @param wake also wake up the body
1589 void ApplyTorque(float torque, bool wake);
1590
1591 /// Apply an impulse at a point. This immediately modifies the velocity.
1592 /// It also modifies the angular velocity if the point of application
1593 /// is not at the center of mass. This wakes up the body.
1594 /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
1595 /// @param point the world position of the point of application.
1596 /// @param wake also wake up the body
1597 void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake);
1598
1599 /// Apply an impulse to the center of mass. This immediately modifies the velocity.
1600 /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
1601 /// @param wake also wake up the body
1602 void ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake);
1603
1604 /// Apply an angular impulse.
1605 /// @param impulse the angular impulse in units of kg*m*m/s
1606 /// @param wake also wake up the body
1607 void ApplyAngularImpulse(float impulse, bool wake);
1608
1609 /// Get the total mass of the body.
1610 /// @return the mass, usually in kilograms (kg).
1611 float GetMass() const;
1612
1613 /// Get the rotational inertia of the body about the local origin.
1614 /// @return the rotational inertia, usually in kg-m^2.
1615 float GetInertia() const;
1616
1617 /// Get the mass data of the body.
1618 /// @return a struct containing the mass, inertia and center of the body.
1619 void GetMassData(b2MassData* data) const;
1620
1621 /// Set the mass properties to override the mass properties of the fixtures.
1622 /// Note that this changes the center of mass position.
1623 /// Note that creating or destroying fixtures can also alter the mass.
1624 /// This function has no effect if the body isn't dynamic.
1625 /// @param data the mass properties.
1626 void SetMassData(const b2MassData* data);
1627
1628 /// This resets the mass properties to the sum of the mass properties of the fixtures.
1629 /// This normally does not need to be called unless you called SetMassData to override
1630 /// the mass and you later want to reset the mass.
1631 void ResetMassData();
1632
1633 /// Get the world coordinates of a point given the local coordinates.
1634 /// @param localPoint a point on the body measured relative the the body's origin.
1635 /// @return the same point expressed in world coordinates.
1636 b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const;
1637
1638 /// Get the world coordinates of a vector given the local coordinates.
1639 /// @param localVector a vector fixed in the body.
1640 /// @return the same vector expressed in world coordinates.
1641 b2Vec2 GetWorldVector(const b2Vec2& localVector) const;
1642
1643 /// Gets a local point relative to the body's origin given a world point.
1644 /// @param worldPoint a point in world coordinates.
1645 /// @return the corresponding local point relative to the body's origin.
1646 b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const;
1647
1648 /// Gets a local vector given a world vector.
1649 /// @param worldVector a vector in world coordinates.
1650 /// @return the corresponding local vector.
1651 b2Vec2 GetLocalVector(const b2Vec2& worldVector) const;
1652
1653 /// Get the world linear velocity of a world point attached to this body.
1654 /// @param worldPoint a point in world coordinates.
1655 /// @return the world velocity of a point.
1656 b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;
1657
1658 /// Get the world velocity of a local point.
1659 /// @param localPoint a point in local coordinates.
1660 /// @return the world velocity of a point.
1661 b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;
1662
1663 /// Get the linear damping of the body.
1664 float GetLinearDamping() const;
1665
1666 /// Set the linear damping of the body.
1667 void SetLinearDamping(float linearDamping);
1668
1669 /// Get the angular damping of the body.
1670 float GetAngularDamping() const;
1671
1672 /// Set the angular damping of the body.
1673 void SetAngularDamping(float angularDamping);
1674
1675 /// Get the gravity scale of the body.
1676 float GetGravityScale() const;
1677
1678 /// Set the gravity scale of the body.
1679 void SetGravityScale(float scale);
1680
1681 /// Set the type of this body. This may alter the mass and velocity.
1682 void SetType(b2BodyType type);
1683
1684 /// Get the type of this body.
1685 b2BodyType GetType() const;
1686
1687 /// Should this body be treated like a bullet for continuous collision detection?
1688 void SetBullet(bool flag);
1689
1690 /// Is this body treated like a bullet for continuous collision detection?
1691 bool IsBullet() const;
1692
1693 /// You can disable sleeping on this body. If you disable sleeping, the
1694 /// body will be woken.
1695 void SetSleepingAllowed(bool flag);
1696
1697 /// Is this body allowed to sleep
1698 bool IsSleepingAllowed() const;
1699
1700 /// Set the sleep state of the body. A sleeping body has very
1701 /// low CPU cost.
1702 /// @param flag set to true to wake the body, false to put it to sleep.
1703 void SetAwake(bool flag);
1704
1705 /// Get the sleeping state of this body.
1706 /// @return true if the body is awake.
1707 bool IsAwake() const;
1708
1709 /// Allow a body to be disabled. A disabled body is not simulated and cannot
1710 /// be collided with or woken up.
1711 /// If you pass a flag of true, all fixtures will be added to the broad-phase.
1712 /// If you pass a flag of false, all fixtures will be removed from the
1713 /// broad-phase and all contacts will be destroyed.
1714 /// Fixtures and joints are otherwise unaffected. You may continue
1715 /// to create/destroy fixtures and joints on disabled bodies.
1716 /// Fixtures on a disabled body are implicitly disabled and will
1717 /// not participate in collisions, ray-casts, or queries.
1718 /// Joints connected to a disabled body are implicitly disabled.
1719 /// An diabled body is still owned by a b2World object and remains
1720 /// in the body list.
1721 void SetEnabled(bool flag);
1722
1723 /// Get the active state of the body.
1724 bool IsEnabled() const;
1725
1726 /// Set this body to have fixed rotation. This causes the mass
1727 /// to be reset.
1728 void SetFixedRotation(bool flag);
1729
1730 /// Does this body have fixed rotation?
1731 bool IsFixedRotation() const;
1732
1733 /// Get the list of all fixtures attached to this body.
1734 b2Fixture* GetFixtureList();
1735 const b2Fixture* GetFixtureList() const;
1736
1737 /// Get the list of all joints attached to this body.
1738 b2JointEdge* GetJointList();
1739 const b2JointEdge* GetJointList() const;
1740
1741 /// Get the list of all contacts attached to this body.
1742 /// @warning this list changes during the time step and you may
1743 /// miss some collisions if you don't use b2ContactListener.
1744 b2ContactEdge* GetContactList();
1745 const b2ContactEdge* GetContactList() const;
1746
1747 /// Get the next body in the world's body list.
1748 b2Body* GetNext();
1749 const b2Body* GetNext() const;
1750
1751 /// Get the user data pointer that was provided in the body definition.
1752 b2BodyUserData& GetUserData();
1753
1754 /// Set the user data. Use this to store your application specific data.
1755 void SetUserData(void* data);
1756
1757 /// Get the parent world of this body.
1758 b2World* GetWorld();
1759 const b2World* GetWorld() const;
1760
1761 /// Dump this body to a file
1762 void Dump();
1763
1764private:
1765
1766 friend class b2World;
1767 friend class b2Island;
1768 friend class b2ContactManager;
1769 friend class b2ContactSolver;
1770 friend class b2Contact;
1771
1772 friend class b2DistanceJoint;
1773 friend class b2FrictionJoint;
1774 friend class b2GearJoint;
1775 friend class b2MotorJoint;
1776 friend class b2MouseJoint;
1777 friend class b2PrismaticJoint;
1778 friend class b2PulleyJoint;
1779 friend class b2RevoluteJoint;
1780 friend class b2RopeJoint;
1781 friend class b2WeldJoint;
1782 friend class b2WheelJoint;
1783
1784 // m_flags
1785 enum {
1786 e_islandFlag = 0x0001,
1787 e_awakeFlag = 0x0002,
1788 e_autoSleepFlag = 0x0004,
1789 e_bulletFlag = 0x0008,
1790 e_fixedRotationFlag = 0x0010,
1791 e_enabledFlag = 0x0020,
1792 e_toiFlag = 0x0040
1793 };
1794
1795 b2Body(const b2BodyDef* bd, b2World* world);
1796 ~b2Body();
1797
1798 void SynchronizeFixtures();
1799 void SynchronizeTransform();
1800
1801 // This is used to prevent connected bodies from colliding.
1802 // It may lie, depending on the collideConnected flag.
1803 bool ShouldCollide(const b2Body* other) const;
1804
1805 void Advance(float t);
1806
1807 b2BodyType m_type;
1808
1809 uint16 m_flags;
1810
1811 int32 m_islandIndex;
1812
1813 b2Transform m_xf; // the body origin transform
1814 b2Sweep m_sweep; // the swept motion for CCD
1815
1816 b2Vec2 m_linearVelocity;
1817 float m_angularVelocity;
1818
1819 b2Vec2 m_force;
1820 float m_torque;
1821
1822 b2World* m_world;
1823 b2Body* m_prev;
1824 b2Body* m_next;
1825
1826 b2Fixture* m_fixtureList;
1827 int32 m_fixtureCount;
1828
1829 b2JointEdge* m_jointList;
1830 b2ContactEdge* m_contactList;
1831
1832 float m_mass, m_invMass;
1833
1834 // Rotational inertia about the center of mass.
1835 float m_I, m_invI;
1836
1837 float m_linearDamping;
1838 float m_angularDamping;
1839 float m_gravityScale;
1840
1841 float m_sleepTime;
1842
1843 b2BodyUserData m_userData;
1844};
1845
1846inline b2BodyType b2Body::GetType() const {
1847 return m_type;
1848}
1849
1850inline const b2Transform& b2Body::GetTransform() const {
1851 return m_xf;
1852}
1853
1854inline const b2Vec2& b2Body::GetPosition() const {
1855 return m_xf.p;
1856}
1857
1858inline float b2Body::GetAngle() const {
1859 return m_sweep.a;
1860}
1861
1862inline const b2Vec2& b2Body::GetWorldCenter() const {
1863 return m_sweep.c;
1864}
1865
1866inline const b2Vec2& b2Body::GetLocalCenter() const {
1867 return m_sweep.localCenter;
1868}
1869
1870inline void b2Body::SetLinearVelocity(const b2Vec2& v) {
1871 if (m_type == b2_staticBody) {
1872 return;
1873 }
1874
1875 if (b2Dot(v, v) > 0.0f) {
1876 SetAwake(true);
1877 }
1878
1879 m_linearVelocity = v;
1880}
1881
1882inline const b2Vec2& b2Body::GetLinearVelocity() const {
1883 return m_linearVelocity;
1884}
1885
1886inline void b2Body::SetAngularVelocity(float w) {
1887 if (m_type == b2_staticBody) {
1888 return;
1889 }
1890
1891 if (w * w > 0.0f) {
1892 SetAwake(true);
1893 }
1894
1895 m_angularVelocity = w;
1896}
1897
1898inline float b2Body::GetAngularVelocity() const {
1899 return m_angularVelocity;
1900}
1901
1902inline float b2Body::GetMass() const {
1903 return m_mass;
1904}
1905
1906inline float b2Body::GetInertia() const {
1907 return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
1908}
1909
1910inline void b2Body::GetMassData(b2MassData* data) const {
1911 data->mass = m_mass;
1912 data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
1913 data->center = m_sweep.localCenter;
1914}
1915
1916inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const {
1917 return b2Mul(m_xf, localPoint);
1918}
1919
1920inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const {
1921 return b2Mul(m_xf.q, localVector);
1922}
1923
1924inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const {
1925 return b2MulT(m_xf, worldPoint);
1926}
1927
1928inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const {
1929 return b2MulT(m_xf.q, worldVector);
1930}
1931
1932inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const {
1933 return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
1934}
1935
1936inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const {
1937 return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
1938}
1939
1940inline float b2Body::GetLinearDamping() const {
1941 return m_linearDamping;
1942}
1943
1944inline void b2Body::SetLinearDamping(float linearDamping) {
1945 m_linearDamping = linearDamping;
1946}
1947
1948inline float b2Body::GetAngularDamping() const {
1949 return m_angularDamping;
1950}
1951
1952inline void b2Body::SetAngularDamping(float angularDamping) {
1953 m_angularDamping = angularDamping;
1954}
1955
1956inline float b2Body::GetGravityScale() const {
1957 return m_gravityScale;
1958}
1959
1960inline void b2Body::SetGravityScale(float scale) {
1961 m_gravityScale = scale;
1962}
1963
1964inline void b2Body::SetBullet(bool flag) {
1965 if (flag) {
1966 m_flags |= e_bulletFlag;
1967 } else {
1968 m_flags &= ~e_bulletFlag;
1969 }
1970}
1971
1972inline bool b2Body::IsBullet() const {
1973 return (m_flags & e_bulletFlag) == e_bulletFlag;
1974}
1975
1976inline void b2Body::SetAwake(bool flag) {
1977 if (m_type == b2_staticBody) {
1978 return;
1979 }
1980
1981 if (flag) {
1982 m_flags |= e_awakeFlag;
1983 m_sleepTime = 0.0f;
1984 } else {
1985 m_flags &= ~e_awakeFlag;
1986 m_sleepTime = 0.0f;
1987 m_linearVelocity.SetZero();
1988 m_angularVelocity = 0.0f;
1989 m_force.SetZero();
1990 m_torque = 0.0f;
1991 }
1992}
1993
1994inline bool b2Body::IsAwake() const {
1995 return (m_flags & e_awakeFlag) == e_awakeFlag;
1996}
1997
1998inline bool b2Body::IsEnabled() const {
1999 return (m_flags & e_enabledFlag) == e_enabledFlag;
2000}
2001
2002inline bool b2Body::IsFixedRotation() const {
2003 return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
2004}
2005
2006inline void b2Body::SetSleepingAllowed(bool flag) {
2007 if (flag) {
2008 m_flags |= e_autoSleepFlag;
2009 } else {
2010 m_flags &= ~e_autoSleepFlag;
2011 SetAwake(true);
2012 }
2013}
2014
2015inline bool b2Body::IsSleepingAllowed() const {
2016 return (m_flags & e_autoSleepFlag) == e_autoSleepFlag;
2017}
2018
2019inline b2Fixture* b2Body::GetFixtureList() {
2020 return m_fixtureList;
2021}
2022
2023inline const b2Fixture* b2Body::GetFixtureList() const {
2024 return m_fixtureList;
2025}
2026
2027inline b2JointEdge* b2Body::GetJointList() {
2028 return m_jointList;
2029}
2030
2031inline const b2JointEdge* b2Body::GetJointList() const {
2032 return m_jointList;
2033}
2034
2035inline b2ContactEdge* b2Body::GetContactList() {
2036 return m_contactList;
2037}
2038
2039inline const b2ContactEdge* b2Body::GetContactList() const {
2040 return m_contactList;
2041}
2042
2043inline b2Body* b2Body::GetNext() {
2044 return m_next;
2045}
2046
2047inline const b2Body* b2Body::GetNext() const {
2048 return m_next;
2049}
2050
2051inline b2BodyUserData& b2Body::GetUserData() {
2052 return m_userData;
2053}
2054
2055inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) {
2056 if (m_type != b2_dynamicBody) {
2057 return;
2058 }
2059
2060 if (wake && (m_flags & e_awakeFlag) == 0) {
2061 SetAwake(true);
2062 }
2063
2064 // Don't accumulate a force if the body is sleeping.
2065 if (m_flags & e_awakeFlag) {
2066 m_force += force;
2067 m_torque += b2Cross(point - m_sweep.c, force);
2068 }
2069}
2070
2071inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) {
2072 if (m_type != b2_dynamicBody) {
2073 return;
2074 }
2075
2076 if (wake && (m_flags & e_awakeFlag) == 0) {
2077 SetAwake(true);
2078 }
2079
2080 // Don't accumulate a force if the body is sleeping
2081 if (m_flags & e_awakeFlag) {
2082 m_force += force;
2083 }
2084}
2085
2086inline void b2Body::ApplyTorque(float torque, bool wake) {
2087 if (m_type != b2_dynamicBody) {
2088 return;
2089 }
2090
2091 if (wake && (m_flags & e_awakeFlag) == 0) {
2092 SetAwake(true);
2093 }
2094
2095 // Don't accumulate a force if the body is sleeping
2096 if (m_flags & e_awakeFlag) {
2097 m_torque += torque;
2098 }
2099}
2100
2101inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) {
2102 if (m_type != b2_dynamicBody) {
2103 return;
2104 }
2105
2106 if (wake && (m_flags & e_awakeFlag) == 0) {
2107 SetAwake(true);
2108 }
2109
2110 // Don't accumulate velocity if the body is sleeping
2111 if (m_flags & e_awakeFlag) {
2112 m_linearVelocity += m_invMass * impulse;
2113 m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
2114 }
2115}
2116
2117inline void b2Body::ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake) {
2118 if (m_type != b2_dynamicBody) {
2119 return;
2120 }
2121
2122 if (wake && (m_flags & e_awakeFlag) == 0) {
2123 SetAwake(true);
2124 }
2125
2126 // Don't accumulate velocity if the body is sleeping
2127 if (m_flags & e_awakeFlag) {
2128 m_linearVelocity += m_invMass * impulse;
2129 }
2130}
2131
2132inline void b2Body::ApplyAngularImpulse(float impulse, bool wake) {
2133 if (m_type != b2_dynamicBody) {
2134 return;
2135 }
2136
2137 if (wake && (m_flags & e_awakeFlag) == 0) {
2138 SetAwake(true);
2139 }
2140
2141 // Don't accumulate velocity if the body is sleeping
2142 if (m_flags & e_awakeFlag) {
2143 m_angularVelocity += m_invI * impulse;
2144 }
2145}
2146
2147inline void b2Body::SynchronizeTransform() {
2148 m_xf.q.Set(m_sweep.a);
2149 m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
2150}
2151
2152inline void b2Body::Advance(float alpha) {
2153 // Advance to the new safe time. This doesn't sync the broad-phase.
2154 m_sweep.Advance(alpha);
2155 m_sweep.c = m_sweep.c0;
2156 m_sweep.a = m_sweep.a0;
2157 m_xf.q.Set(m_sweep.a);
2158 m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
2159}
2160
2161inline b2World* b2Body::GetWorld() {
2162 return m_world;
2163}
2164
2165inline const b2World* b2Body::GetWorld() const {
2166 return m_world;
2167}
2168
2169#endif
2170// MIT License
2171
2172// Copyright (c) 2019 Erin Catto
2173
2174// Permission is hereby granted, free of charge, to any person obtaining a copy
2175// of this software and associated documentation files (the "Software"), to deal
2176// in the Software without restriction, including without limitation the rights
2177// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2178// copies of the Software, and to permit persons to whom the Software is
2179// furnished to do so, subject to the following conditions:
2180
2181// The above copyright notice and this permission notice shall be included in all
2182// copies or substantial portions of the Software.
2183
2184// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2185// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2186// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2187// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2188// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2189// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2190// SOFTWARE.
2191
2192#ifndef B2_GROWABLE_STACK_H
2193#define B2_GROWABLE_STACK_H
2194
2195#include <string.h>
2196
2197//#include "b2_settings.h"
2198
2199/// This is a growable LIFO stack with an initial capacity of N.
2200/// If the stack size exceeds the initial capacity, the heap is used
2201/// to increase the size of the stack.
2202template <typename T, int32 N>
2203class b2GrowableStack {
2204public:
2205 b2GrowableStack() {
2206 m_stack = m_array;
2207 m_count = 0;
2208 m_capacity = N;
2209 }
2210
2211 ~b2GrowableStack() {
2212 if (m_stack != m_array) {
2213 b2Free(m_stack);
2214 m_stack = nullptr;
2215 }
2216 }
2217
2218 void Push(const T& element) {
2219 if (m_count == m_capacity) {
2220 T* old = m_stack;
2221 m_capacity *= 2;
2222 m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
2223 memcpy(m_stack, old, m_count * sizeof(T));
2224 if (old != m_array) {
2225 b2Free(old);
2226 }
2227 }
2228
2229 m_stack[m_count] = element;
2230 ++m_count;
2231 }
2232
2233 T Pop() {
2234 b2Assert(m_count > 0);
2235 --m_count;
2236 return m_stack[m_count];
2237 }
2238
2239 int32 GetCount() {
2240 return m_count;
2241 }
2242
2243private:
2244 T* m_stack;
2245 T m_array[N];
2246 int32 m_count;
2247 int32 m_capacity;
2248};
2249
2250
2251#endif
2252// MIT License
2253
2254// Copyright (c) 2019 Erin Catto
2255
2256// Permission is hereby granted, free of charge, to any person obtaining a copy
2257// of this software and associated documentation files (the "Software"), to deal
2258// in the Software without restriction, including without limitation the rights
2259// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2260// copies of the Software, and to permit persons to whom the Software is
2261// furnished to do so, subject to the following conditions:
2262
2263// The above copyright notice and this permission notice shall be included in all
2264// copies or substantial portions of the Software.
2265
2266// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2267// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2268// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2269// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2270// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2271// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2272// SOFTWARE.
2273
2274#ifndef B2_DYNAMIC_TREE_H
2275#define B2_DYNAMIC_TREE_H
2276
2277//#include "b2_api.h"
2278//#include "b2_collision.h"
2279//#include "b2_growable_stack.h"
2280
2281#define b2_nullNode (-1)
2282
2283/// A node in the dynamic tree. The client does not interact with this directly.
2284struct B2_API b2TreeNode {
2285 bool IsLeaf() const {
2286 return child1 == b2_nullNode;
2287 }
2288
2289 /// Enlarged AABB
2290 b2AABB aabb;
2291
2292 void* userData;
2293
2294 union {
2295 int32 parent;
2296 int32 next;
2297 };
2298
2299 int32 child1;
2300 int32 child2;
2301
2302 // leaf = 0, free node = -1
2303 int32 height;
2304
2305 bool moved;
2306};
2307
2308/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
2309/// A dynamic tree arranges data in a binary tree to accelerate
2310/// queries such as volume queries and ray casts. Leafs are proxies
2311/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
2312/// so that the proxy AABB is bigger than the client object. This allows the client
2313/// object to move by small amounts without triggering a tree update.
2314///
2315/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
2316class B2_API b2DynamicTree {
2317public:
2318 /// Constructing the tree initializes the node pool.
2319 b2DynamicTree();
2320
2321 /// Destroy the tree, freeing the node pool.
2322 ~b2DynamicTree();
2323
2324 /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
2325 int32 CreateProxy(const b2AABB& aabb, void* userData);
2326
2327 /// Destroy a proxy. This asserts if the id is invalid.
2328 void DestroyProxy(int32 proxyId);
2329
2330 /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
2331 /// then the proxy is removed from the tree and re-inserted. Otherwise
2332 /// the function returns immediately.
2333 /// @return true if the proxy was re-inserted.
2334 bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
2335
2336 /// Get proxy user data.
2337 /// @return the proxy user data or 0 if the id is invalid.
2338 void* GetUserData(int32 proxyId) const;
2339
2340 bool WasMoved(int32 proxyId) const;
2341 void ClearMoved(int32 proxyId);
2342
2343 /// Get the fat AABB for a proxy.
2344 const b2AABB& GetFatAABB(int32 proxyId) const;
2345
2346 /// Query an AABB for overlapping proxies. The callback class
2347 /// is called for each proxy that overlaps the supplied AABB.
2348 template <typename T>
2349 void Query(T* callback, const b2AABB& aabb) const;
2350
2351 /// Ray-cast against the proxies in the tree. This relies on the callback
2352 /// to perform a exact ray-cast in the case were the proxy contains a shape.
2353 /// The callback also performs the any collision filtering. This has performance
2354 /// roughly equal to k * log(n), where k is the number of collisions and n is the
2355 /// number of proxies in the tree.
2356 /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
2357 /// @param callback a callback class that is called for each proxy that is hit by the ray.
2358 template <typename T>
2359 void RayCast(T* callback, const b2RayCastInput& input) const;
2360
2361 /// Validate this tree. For testing.
2362 void Validate() const;
2363
2364 /// Compute the height of the binary tree in O(N) time. Should not be
2365 /// called often.
2366 int32 GetHeight() const;
2367
2368 /// Get the maximum balance of an node in the tree. The balance is the difference
2369 /// in height of the two children of a node.
2370 int32 GetMaxBalance() const;
2371
2372 /// Get the ratio of the sum of the node areas to the root area.
2373 float GetAreaRatio() const;
2374
2375 /// Build an optimal tree. Very expensive. For testing.
2376 void RebuildBottomUp();
2377
2378 /// Shift the world origin. Useful for large worlds.
2379 /// The shift formula is: position -= newOrigin
2380 /// @param newOrigin the new origin with respect to the old origin
2381 void ShiftOrigin(const b2Vec2& newOrigin);
2382
2383private:
2384
2385 int32 AllocateNode();
2386 void FreeNode(int32 node);
2387
2388 void InsertLeaf(int32 node);
2389 void RemoveLeaf(int32 node);
2390
2391 int32 Balance(int32 index);
2392
2393 int32 ComputeHeight() const;
2394 int32 ComputeHeight(int32 nodeId) const;
2395
2396 void ValidateStructure(int32 index) const;
2397 void ValidateMetrics(int32 index) const;
2398
2399 int32 m_root;
2400
2401 b2TreeNode* m_nodes;
2402 int32 m_nodeCount;
2403 int32 m_nodeCapacity;
2404
2405 int32 m_freeList;
2406
2407 int32 m_insertionCount;
2408};
2409
2410inline void* b2DynamicTree::GetUserData(int32 proxyId) const {
2411 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
2412 return m_nodes[proxyId].userData;
2413}
2414
2415inline bool b2DynamicTree::WasMoved(int32 proxyId) const {
2416 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
2417 return m_nodes[proxyId].moved;
2418}
2419
2420inline void b2DynamicTree::ClearMoved(int32 proxyId) {
2421 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
2422 m_nodes[proxyId].moved = false;
2423}
2424
2425inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const {
2426 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
2427 return m_nodes[proxyId].aabb;
2428}
2429
2430template <typename T>
2431inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const {
2432 b2GrowableStack<int32, 256> stack;
2433 stack.Push(m_root);
2434
2435 while (stack.GetCount() > 0) {
2436 int32 nodeId = stack.Pop();
2437 if (nodeId == b2_nullNode) {
2438 continue;
2439 }
2440
2441 const b2TreeNode* node = m_nodes + nodeId;
2442
2443 if (b2TestOverlap(node->aabb, aabb)) {
2444 if (node->IsLeaf()) {
2445 bool proceed = callback->QueryCallback(nodeId);
2446 if (proceed == false) {
2447 return;
2448 }
2449 } else {
2450 stack.Push(node->child1);
2451 stack.Push(node->child2);
2452 }
2453 }
2454 }
2455}
2456
2457template <typename T>
2458inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const {
2459 b2Vec2 p1 = input.p1;
2460 b2Vec2 p2 = input.p2;
2461 b2Vec2 r = p2 - p1;
2462 b2Assert(r.LengthSquared() > 0.0f);
2463 r.Normalize();
2464
2465 // v is perpendicular to the segment.
2466 b2Vec2 v = b2Cross(1.0f, r);
2467 b2Vec2 abs_v = b2Abs(v);
2468
2469 // Separating axis for segment (Gino, p80).
2470 // |dot(v, p1 - c)| > dot(|v|, h)
2471
2472 float maxFraction = input.maxFraction;
2473
2474 // Build a bounding box for the segment.
2475 b2AABB segmentAABB;
2476 {
2477 b2Vec2 t = p1 + maxFraction * (p2 - p1);
2478 segmentAABB.lowerBound = b2Min(p1, t);
2479 segmentAABB.upperBound = b2Max(p1, t);
2480 }
2481
2482 b2GrowableStack<int32, 256> stack;
2483 stack.Push(m_root);
2484
2485 while (stack.GetCount() > 0) {
2486 int32 nodeId = stack.Pop();
2487 if (nodeId == b2_nullNode) {
2488 continue;
2489 }
2490
2491 const b2TreeNode* node = m_nodes + nodeId;
2492
2493 if (b2TestOverlap(node->aabb, segmentAABB) == false) {
2494 continue;
2495 }
2496
2497 // Separating axis for segment (Gino, p80).
2498 // |dot(v, p1 - c)| > dot(|v|, h)
2499 b2Vec2 c = node->aabb.GetCenter();
2500 b2Vec2 h = node->aabb.GetExtents();
2501 float separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
2502 if (separation > 0.0f) {
2503 continue;
2504 }
2505
2506 if (node->IsLeaf()) {
2507 b2RayCastInput subInput;
2508 subInput.p1 = input.p1;
2509 subInput.p2 = input.p2;
2510 subInput.maxFraction = maxFraction;
2511
2512 float value = callback->RayCastCallback(subInput, nodeId);
2513
2514 if (value == 0.0f) {
2515 // The client has terminated the ray cast.
2516 return;
2517 }
2518
2519 if (value > 0.0f) {
2520 // Update segment bounding box.
2521 maxFraction = value;
2522 b2Vec2 t = p1 + maxFraction * (p2 - p1);
2523 segmentAABB.lowerBound = b2Min(p1, t);
2524 segmentAABB.upperBound = b2Max(p1, t);
2525 }
2526 } else {
2527 stack.Push(node->child1);
2528 stack.Push(node->child2);
2529 }
2530 }
2531}
2532
2533#endif
2534// MIT License
2535
2536// Copyright (c) 2019 Erin Catto
2537
2538// Permission is hereby granted, free of charge, to any person obtaining a copy
2539// of this software and associated documentation files (the "Software"), to deal
2540// in the Software without restriction, including without limitation the rights
2541// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2542// copies of the Software, and to permit persons to whom the Software is
2543// furnished to do so, subject to the following conditions:
2544
2545// The above copyright notice and this permission notice shall be included in all
2546// copies or substantial portions of the Software.
2547
2548// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2549// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2550// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2551// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2552// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2553// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2554// SOFTWARE.
2555
2556#ifndef B2_BROAD_PHASE_H
2557#define B2_BROAD_PHASE_H
2558
2559//#include "b2_api.h"
2560//#include "b2_settings.h"
2561//#include "b2_collision.h"
2562//#include "b2_dynamic_tree.h"
2563
2564struct B2_API b2Pair {
2565 int32 proxyIdA;
2566 int32 proxyIdB;
2567};
2568
2569/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
2570/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
2571/// It is up to the client to consume the new pairs and to track subsequent overlap.
2572class B2_API b2BroadPhase {
2573public:
2574
2575 enum {
2576 e_nullProxy = -1
2577 };
2578
2579 b2BroadPhase();
2580 ~b2BroadPhase();
2581
2582 /// Create a proxy with an initial AABB. Pairs are not reported until
2583 /// UpdatePairs is called.
2584 int32 CreateProxy(const b2AABB& aabb, void* userData);
2585
2586 /// Destroy a proxy. It is up to the client to remove any pairs.
2587 void DestroyProxy(int32 proxyId);
2588
2589 /// Call MoveProxy as many times as you like, then when you are done
2590 /// call UpdatePairs to finalized the proxy pairs (for your time step).
2591 void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
2592
2593 /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
2594 void TouchProxy(int32 proxyId);
2595
2596 /// Get the fat AABB for a proxy.
2597 const b2AABB& GetFatAABB(int32 proxyId) const;
2598
2599 /// Get user data from a proxy. Returns nullptr if the id is invalid.
2600 void* GetUserData(int32 proxyId) const;
2601
2602 /// Test overlap of fat AABBs.
2603 bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
2604
2605 /// Get the number of proxies.
2606 int32 GetProxyCount() const;
2607
2608 /// Update the pairs. This results in pair callbacks. This can only add pairs.
2609 template <typename T>
2610 void UpdatePairs(T* callback);
2611
2612 /// Query an AABB for overlapping proxies. The callback class
2613 /// is called for each proxy that overlaps the supplied AABB.
2614 template <typename T>
2615 void Query(T* callback, const b2AABB& aabb) const;
2616
2617 /// Ray-cast against the proxies in the tree. This relies on the callback
2618 /// to perform a exact ray-cast in the case were the proxy contains a shape.
2619 /// The callback also performs the any collision filtering. This has performance
2620 /// roughly equal to k * log(n), where k is the number of collisions and n is the
2621 /// number of proxies in the tree.
2622 /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
2623 /// @param callback a callback class that is called for each proxy that is hit by the ray.
2624 template <typename T>
2625 void RayCast(T* callback, const b2RayCastInput& input) const;
2626
2627 /// Get the height of the embedded tree.
2628 int32 GetTreeHeight() const;
2629
2630 /// Get the balance of the embedded tree.
2631 int32 GetTreeBalance() const;
2632
2633 /// Get the quality metric of the embedded tree.
2634 float GetTreeQuality() const;
2635
2636 /// Shift the world origin. Useful for large worlds.
2637 /// The shift formula is: position -= newOrigin
2638 /// @param newOrigin the new origin with respect to the old origin
2639 void ShiftOrigin(const b2Vec2& newOrigin);
2640
2641private:
2642
2643 friend class b2DynamicTree;
2644
2645 void BufferMove(int32 proxyId);
2646 void UnBufferMove(int32 proxyId);
2647
2648 bool QueryCallback(int32 proxyId);
2649
2650 b2DynamicTree m_tree;
2651
2652 int32 m_proxyCount;
2653
2654 int32* m_moveBuffer;
2655 int32 m_moveCapacity;
2656 int32 m_moveCount;
2657
2658 b2Pair* m_pairBuffer;
2659 int32 m_pairCapacity;
2660 int32 m_pairCount;
2661
2662 int32 m_queryProxyId;
2663};
2664
2665inline void* b2BroadPhase::GetUserData(int32 proxyId) const {
2666 return m_tree.GetUserData(proxyId);
2667}
2668
2669inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const {
2670 const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
2671 const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
2672 return b2TestOverlap(aabbA, aabbB);
2673}
2674
2675inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const {
2676 return m_tree.GetFatAABB(proxyId);
2677}
2678
2679inline int32 b2BroadPhase::GetProxyCount() const {
2680 return m_proxyCount;
2681}
2682
2683inline int32 b2BroadPhase::GetTreeHeight() const {
2684 return m_tree.GetHeight();
2685}
2686
2687inline int32 b2BroadPhase::GetTreeBalance() const {
2688 return m_tree.GetMaxBalance();
2689}
2690
2691inline float b2BroadPhase::GetTreeQuality() const {
2692 return m_tree.GetAreaRatio();
2693}
2694
2695template <typename T>
2696void b2BroadPhase::UpdatePairs(T* callback) {
2697 // Reset pair buffer
2698 m_pairCount = 0;
2699
2700 // Perform tree queries for all moving proxies.
2701 for (int32 i = 0; i < m_moveCount; ++i) {
2702 m_queryProxyId = m_moveBuffer[i];
2703 if (m_queryProxyId == e_nullProxy) {
2704 continue;
2705 }
2706
2707 // We have to query the tree with the fat AABB so that
2708 // we don't fail to create a pair that may touch later.
2709 const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
2710
2711 // Query tree, create pairs and add them pair buffer.
2712 m_tree.Query(this, fatAABB);
2713 }
2714
2715 // Send pairs to caller
2716 for (int32 i = 0; i < m_pairCount; ++i) {
2717 b2Pair* primaryPair = m_pairBuffer + i;
2718 void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
2719 void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
2720
2721 callback->AddPair(userDataA, userDataB);
2722 }
2723
2724 // Clear move flags
2725 for (int32 i = 0; i < m_moveCount; ++i) {
2726 int32 proxyId = m_moveBuffer[i];
2727 if (proxyId == e_nullProxy) {
2728 continue;
2729 }
2730
2731 m_tree.ClearMoved(proxyId);
2732 }
2733
2734 // Reset move buffer
2735 m_moveCount = 0;
2736}
2737
2738template <typename T>
2739inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const {
2740 m_tree.Query(callback, aabb);
2741}
2742
2743template <typename T>
2744inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const {
2745 m_tree.RayCast(callback, input);
2746}
2747
2748inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) {
2749 m_tree.ShiftOrigin(newOrigin);
2750}
2751
2752#endif
2753// MIT License
2754
2755// Copyright (c) 2019 Erin Catto
2756
2757// Permission is hereby granted, free of charge, to any person obtaining a copy
2758// of this software and associated documentation files (the "Software"), to deal
2759// in the Software without restriction, including without limitation the rights
2760// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2761// copies of the Software, and to permit persons to whom the Software is
2762// furnished to do so, subject to the following conditions:
2763
2764// The above copyright notice and this permission notice shall be included in all
2765// copies or substantial portions of the Software.
2766
2767// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2768// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2769// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2770// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2771// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2772// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2773// SOFTWARE.
2774
2775#ifndef B2_CHAIN_SHAPE_H
2776#define B2_CHAIN_SHAPE_H
2777
2778//#include "b2_api.h"
2779//#include "b2_shape.h"
2780
2781class b2EdgeShape;
2782
2783/// A chain shape is a free form sequence of line segments.
2784/// The chain has one-sided collision, with the surface normal pointing to the right of the edge.
2785/// This provides a counter-clockwise winding like the polygon shape.
2786/// Connectivity information is used to create smooth collisions.
2787/// @warning the chain will not collide properly if there are self-intersections.
2788class B2_API b2ChainShape : public b2Shape {
2789public:
2790 b2ChainShape();
2791
2792 /// The destructor frees the vertices using b2Free.
2793 ~b2ChainShape();
2794
2795 /// Clear all data.
2796 void Clear();
2797
2798 /// Create a loop. This automatically adjusts connectivity.
2799 /// @param vertices an array of vertices, these are copied
2800 /// @param count the vertex count
2801 void CreateLoop(const b2Vec2* vertices, int32 count);
2802
2803 /// Create a chain with ghost vertices to connect multiple chains together.
2804 /// @param vertices an array of vertices, these are copied
2805 /// @param count the vertex count
2806 /// @param prevVertex previous vertex from chain that connects to the start
2807 /// @param nextVertex next vertex from chain that connects to the end
2808 void CreateChain(const b2Vec2* vertices, int32 count,
2809 const b2Vec2& prevVertex, const b2Vec2& nextVertex);
2810
2811 /// Implement b2Shape. Vertices are cloned using b2Alloc.
2812 b2Shape* Clone(b2BlockAllocator* allocator) const override;
2813
2814 /// @see b2Shape::GetChildCount
2815 int32 GetChildCount() const override;
2816
2817 /// Get a child edge.
2818 void GetChildEdge(b2EdgeShape* edge, int32 index) const;
2819
2820 /// This always return false.
2821 /// @see b2Shape::TestPoint
2822 bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
2823
2824 /// Implement b2Shape.
2825 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
2826 const b2Transform& transform, int32 childIndex) const override;
2827
2828 /// @see b2Shape::ComputeAABB
2829 void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
2830
2831 /// Chains have zero mass.
2832 /// @see b2Shape::ComputeMass
2833 void ComputeMass(b2MassData* massData, float density) const override;
2834
2835 /// The vertices. Owned by this class.
2836 b2Vec2* m_vertices;
2837
2838 /// The vertex count.
2839 int32 m_count;
2840
2841 b2Vec2 m_prevVertex, m_nextVertex;
2842};
2843
2844inline b2ChainShape::b2ChainShape() {
2845 m_type = e_chain;
2846 m_radius = b2_polygonRadius;
2847 m_vertices = nullptr;
2848 m_count = 0;
2849}
2850
2851#endif
2852// MIT License
2853
2854// Copyright (c) 2019 Erin Catto
2855
2856// Permission is hereby granted, free of charge, to any person obtaining a copy
2857// of this software and associated documentation files (the "Software"), to deal
2858// in the Software without restriction, including without limitation the rights
2859// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2860// copies of the Software, and to permit persons to whom the Software is
2861// furnished to do so, subject to the following conditions:
2862
2863// The above copyright notice and this permission notice shall be included in all
2864// copies or substantial portions of the Software.
2865
2866// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2867// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2868// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2869// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2870// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2871// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2872// SOFTWARE.
2873
2874#ifndef B2_CIRCLE_SHAPE_H
2875#define B2_CIRCLE_SHAPE_H
2876
2877//#include "b2_api.h"
2878//#include "b2_shape.h"
2879
2880/// A solid circle shape
2881class B2_API b2CircleShape : public b2Shape {
2882public:
2883 b2CircleShape();
2884
2885 /// Implement b2Shape.
2886 b2Shape* Clone(b2BlockAllocator* allocator) const override;
2887
2888 /// @see b2Shape::GetChildCount
2889 int32 GetChildCount() const override;
2890
2891 /// Implement b2Shape.
2892 bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
2893
2894 /// Implement b2Shape.
2895 /// @note because the circle is solid, rays that start inside do not hit because the normal is
2896 /// not defined.
2897 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
2898 const b2Transform& transform, int32 childIndex) const override;
2899
2900 /// @see b2Shape::ComputeAABB
2901 void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
2902
2903 /// @see b2Shape::ComputeMass
2904 void ComputeMass(b2MassData* massData, float density) const override;
2905
2906 /// Position
2907 b2Vec2 m_p;
2908};
2909
2910inline b2CircleShape::b2CircleShape() {
2911 m_type = e_circle;
2912 m_radius = 0.0f;
2913 m_p.SetZero();
2914}
2915
2916#endif
2917// MIT License
2918
2919// Copyright (c) 2019 Erin Catto
2920
2921// Permission is hereby granted, free of charge, to any person obtaining a copy
2922// of this software and associated documentation files (the "Software"), to deal
2923// in the Software without restriction, including without limitation the rights
2924// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
2925// copies of the Software, and to permit persons to whom the Software is
2926// furnished to do so, subject to the following conditions:
2927
2928// The above copyright notice and this permission notice shall be included in all
2929// copies or substantial portions of the Software.
2930
2931// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
2932// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
2933// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
2934// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
2935// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
2936// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
2937// SOFTWARE.
2938
2939#ifndef B2_COMMON_H
2940#define B2_COMMON_H
2941
2942//#include "b2_settings.h"
2943
2944
2945/// @file
2946/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
2947///
2948
2949// Collision
2950
2951
2952// Dynamics
2953
2954/// Maximum number of contacts to be handled to solve a TOI impact.
2955#define b2_maxTOIContacts 32
2956
2957/// The maximum linear position correction used when solving constraints. This helps to
2958/// prevent overshoot. Meters.
2959#define b2_maxLinearCorrection (0.2f * b2_lengthUnitsPerMeter)
2960
2961/// The maximum angular position correction used when solving constraints. This helps to
2962/// prevent overshoot.
2963#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi)
2964
2965/// The maximum linear translation of a body per step. This limit is very large and is used
2966/// to prevent numerical problems. You shouldn't need to adjust this. Meters.
2967#define b2_maxTranslation (2.0f * b2_lengthUnitsPerMeter)
2968#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation)
2969
2970/// The maximum angular velocity of a body. This limit is very large and is used
2971/// to prevent numerical problems. You shouldn't need to adjust this.
2972#define b2_maxRotation (0.5f * b2_pi)
2973#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation)
2974
2975/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
2976/// that overlap is removed in one time step. However using values close to 1 often lead
2977/// to overshoot.
2978#define b2_baumgarte 0.2f
2979#define b2_toiBaumgarte 0.75f
2980
2981
2982// Sleep
2983
2984/// The time that a body must be still before it will go to sleep.
2985#define b2_timeToSleep 0.5f
2986
2987/// A body cannot sleep if its linear velocity is above this tolerance.
2988#define b2_linearSleepTolerance (0.01f * b2_lengthUnitsPerMeter)
2989
2990/// A body cannot sleep if its angular velocity is above this tolerance.
2991#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi)
2992
2993/// Dump to a file. Only one dump file allowed at a time.
2994void b2OpenDump(const char* fileName);
2995void b2Dump(const char* string, ...);
2996void b2CloseDump();
2997
2998/// Version numbering scheme.
2999/// See http://en.wikipedia.org/wiki/Software_versioning
3000struct b2Version {
3001 int32 major; ///< significant changes
3002 int32 minor; ///< incremental changes
3003 int32 revision; ///< bug fixes
3004};
3005
3006/// Current version.
3007extern B2_API b2Version b2_version;
3008
3009#endif
3010// MIT License
3011
3012// Copyright (c) 2019 Erin Catto
3013
3014// Permission is hereby granted, free of charge, to any person obtaining a copy
3015// of this software and associated documentation files (the "Software"), to deal
3016// in the Software without restriction, including without limitation the rights
3017// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
3018// copies of the Software, and to permit persons to whom the Software is
3019// furnished to do so, subject to the following conditions:
3020
3021// The above copyright notice and this permission notice shall be included in all
3022// copies or substantial portions of the Software.
3023
3024// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
3025// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
3026// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
3027// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
3028// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
3029// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
3030// SOFTWARE.
3031
3032#ifndef B2_FIXTURE_H
3033#define B2_FIXTURE_H
3034
3035//#include "b2_api.h"
3036//#include "b2_body.h"
3037//#include "b2_collision.h"
3038//#include "b2_shape.h"
3039
3040class b2BlockAllocator;
3041class b2Body;
3042class b2BroadPhase;
3043class b2Fixture;
3044
3045/// This holds contact filtering data.
3046struct B2_API b2Filter {
3047 b2Filter() {
3048 categoryBits = 0x0001;
3049 maskBits = 0xFFFF;
3050 groupIndex = 0;
3051 }
3052
3053 /// The collision category bits. Normally you would just set one bit.
3054 uint16 categoryBits;
3055
3056 /// The collision mask bits. This states the categories that this
3057 /// shape would accept for collision.
3058 uint16 maskBits;
3059
3060 /// Collision groups allow a certain group of objects to never collide (negative)
3061 /// or always collide (positive). Zero means no collision group. Non-zero group
3062 /// filtering always wins against the mask bits.
3063 int16 groupIndex;
3064};
3065
3066/// A fixture definition is used to create a fixture. This class defines an
3067/// abstract fixture definition. You can reuse fixture definitions safely.
3068struct B2_API b2FixtureDef {
3069 /// The constructor sets the default fixture definition values.
3070 b2FixtureDef() {
3071 shape = nullptr;
3072 friction = 0.2f;
3073 restitution = 0.0f;
3074 restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter;
3075 density = 0.0f;
3076 isSensor = false;
3077 }
3078
3079 /// The shape, this must be set. The shape will be cloned, so you
3080 /// can create the shape on the stack.
3081 const b2Shape* shape;
3082
3083 /// Use this to store application specific fixture data.
3084 b2FixtureUserData userData;
3085
3086 /// The friction coefficient, usually in the range [0,1].
3087 float friction;
3088
3089 /// The restitution (elasticity) usually in the range [0,1].
3090 float restitution;
3091
3092 /// Restitution velocity threshold, usually in m/s. Collisions above this
3093 /// speed have restitution applied (will bounce).
3094 float restitutionThreshold;
3095
3096 /// The density, usually in kg/m^2.
3097 float density;
3098
3099 /// A sensor shape collects contact information but never generates a collision
3100 /// response.
3101 bool isSensor;
3102
3103 /// Contact filtering data.
3104 b2Filter filter;
3105};
3106
3107/// This proxy is used internally to connect fixtures to the broad-phase.
3108struct B2_API b2FixtureProxy {
3109 b2AABB aabb;
3110 b2Fixture* fixture;
3111 int32 childIndex;
3112 int32 proxyId;
3113};
3114
3115/// A fixture is used to attach a shape to a body for collision detection. A fixture
3116/// inherits its transform from its parent. Fixtures hold additional non-geometric data
3117/// such as friction, collision filters, etc.
3118/// Fixtures are created via b2Body::CreateFixture.
3119/// @warning you cannot reuse fixtures.
3120class B2_API b2Fixture {
3121public:
3122 /// Get the type of the child shape. You can use this to down cast to the concrete shape.
3123 /// @return the shape type.
3124 b2Shape::Type GetType() const;
3125
3126 /// Get the child shape. You can modify the child shape, however you should not change the
3127 /// number of vertices because this will crash some collision caching mechanisms.
3128 /// Manipulating the shape may lead to non-physical behavior.
3129 b2Shape* GetShape();
3130 const b2Shape* GetShape() const;
3131
3132 /// Set if this fixture is a sensor.
3133 void SetSensor(bool sensor);
3134
3135 /// Is this fixture a sensor (non-solid)?
3136 /// @return the true if the shape is a sensor.
3137 bool IsSensor() const;
3138
3139 /// Set the contact filtering data. This will not update contacts until the next time
3140 /// step when either parent body is active and awake.
3141 /// This automatically calls Refilter.
3142 void SetFilterData(const b2Filter& filter);
3143
3144 /// Get the contact filtering data.
3145 const b2Filter& GetFilterData() const;
3146
3147 /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
3148 void Refilter();
3149
3150 /// Get the parent body of this fixture. This is nullptr if the fixture is not attached.
3151 /// @return the parent body.
3152 b2Body* GetBody();
3153 const b2Body* GetBody() const;
3154
3155 /// Get the next fixture in the parent body's fixture list.
3156 /// @return the next shape.
3157 b2Fixture* GetNext();
3158 const b2Fixture* GetNext() const;
3159
3160 /// Get the user data that was assigned in the fixture definition. Use this to
3161 /// store your application specific data.
3162 b2FixtureUserData& GetUserData();
3163
3164 /// Test a point for containment in this fixture.
3165 /// @param p a point in world coordinates.
3166 bool TestPoint(const b2Vec2& p) const;
3167
3168 /// Cast a ray against this shape.
3169 /// @param output the ray-cast results.
3170 /// @param input the ray-cast input parameters.
3171 /// @param childIndex the child shape index (e.g. edge index)
3172 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const;
3173
3174 /// Get the mass data for this fixture. The mass data is based on the density and
3175 /// the shape. The rotational inertia is about the shape's origin. This operation
3176 /// may be expensive.
3177 void GetMassData(b2MassData* massData) const;
3178
3179 /// Set the density of this fixture. This will _not_ automatically adjust the mass
3180 /// of the body. You must call b2Body::ResetMassData to update the body's mass.
3181 void SetDensity(float density);
3182
3183 /// Get the density of this fixture.
3184 float GetDensity() const;
3185
3186 /// Get the coefficient of friction.
3187 float GetFriction() const;
3188
3189 /// Set the coefficient of friction. This will _not_ change the friction of
3190 /// existing contacts.
3191 void SetFriction(float friction);
3192
3193 /// Get the coefficient of restitution.
3194 float GetRestitution() const;
3195
3196 /// Set the coefficient of restitution. This will _not_ change the restitution of
3197 /// existing contacts.
3198 void SetRestitution(float restitution);
3199
3200 /// Get the restitution velocity threshold.
3201 float GetRestitutionThreshold() const;
3202
3203 /// Set the restitution threshold. This will _not_ change the restitution threshold of
3204 /// existing contacts.
3205 void SetRestitutionThreshold(float threshold);
3206
3207 /// Get the fixture's AABB. This AABB may be enlarge and/or stale.
3208 /// If you need a more accurate AABB, compute it using the shape and
3209 /// the body transform.
3210 const b2AABB& GetAABB(int32 childIndex) const;
3211
3212 /// Dump this fixture to the log file.
3213 void Dump(int32 bodyIndex);
3214
3215protected:
3216
3217 friend class b2Body;
3218 friend class b2World;
3219 friend class b2Contact;
3220 friend class b2ContactManager;
3221
3222 b2Fixture();
3223
3224 // We need separation create/destroy functions from the constructor/destructor because
3225 // the destructor cannot access the allocator (no destructor arguments allowed by C++).
3226 void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def);
3227 void Destroy(b2BlockAllocator* allocator);
3228
3229 // These support body activation/deactivation.
3230 void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf);
3231 void DestroyProxies(b2BroadPhase* broadPhase);
3232
3233 void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
3234
3235 float m_density;
3236
3237 b2Fixture* m_next;
3238 b2Body* m_body;
3239
3240 b2Shape* m_shape;
3241
3242 float m_friction;
3243 float m_restitution;
3244 float m_restitutionThreshold;
3245
3246 b2FixtureProxy* m_proxies;
3247 int32 m_proxyCount;
3248
3249 b2Filter m_filter;
3250
3251 bool m_isSensor;
3252
3253 b2FixtureUserData m_userData;
3254};
3255
3256inline b2Shape::Type b2Fixture::GetType() const {
3257 return m_shape->GetType();
3258}
3259
3260inline b2Shape* b2Fixture::GetShape() {
3261 return m_shape;
3262}
3263
3264inline const b2Shape* b2Fixture::GetShape() const {
3265 return m_shape;
3266}
3267
3268inline bool b2Fixture::IsSensor() const {
3269 return m_isSensor;
3270}
3271
3272inline const b2Filter& b2Fixture::GetFilterData() const {
3273 return m_filter;
3274}
3275
3276inline b2FixtureUserData& b2Fixture::GetUserData() {
3277 return m_userData;
3278}
3279
3280inline b2Body* b2Fixture::GetBody() {
3281 return m_body;
3282}
3283
3284inline const b2Body* b2Fixture::GetBody() const {
3285 return m_body;
3286}
3287
3288inline b2Fixture* b2Fixture::GetNext() {
3289 return m_next;
3290}
3291
3292inline const b2Fixture* b2Fixture::GetNext() const {
3293 return m_next;
3294}
3295
3296inline void b2Fixture::SetDensity(float density) {
3297 b2Assert(b2IsValid(density) && density >= 0.0f);
3298 m_density = density;
3299}
3300
3301inline float b2Fixture::GetDensity() const {
3302 return m_density;
3303}
3304
3305inline float b2Fixture::GetFriction() const {
3306 return m_friction;
3307}
3308
3309inline void b2Fixture::SetFriction(float friction) {
3310 m_friction = friction;
3311}
3312
3313inline float b2Fixture::GetRestitution() const {
3314 return m_restitution;
3315}
3316
3317inline void b2Fixture::SetRestitution(float restitution) {
3318 m_restitution = restitution;
3319}
3320
3321inline float b2Fixture::GetRestitutionThreshold() const {
3322 return m_restitutionThreshold;
3323}
3324
3325inline void b2Fixture::SetRestitutionThreshold(float threshold) {
3326 m_restitutionThreshold = threshold;
3327}
3328
3329inline bool b2Fixture::TestPoint(const b2Vec2& p) const {
3330 return m_shape->TestPoint(m_body->GetTransform(), p);
3331}
3332
3333inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const {
3334 return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex);
3335}
3336
3337inline void b2Fixture::GetMassData(b2MassData* massData) const {
3338 m_shape->ComputeMass(massData, m_density);
3339}
3340
3341inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const {
3342 b2Assert(0 <= childIndex && childIndex < m_proxyCount);
3343 return m_proxies[childIndex].aabb;
3344}
3345
3346#endif
3347// MIT License
3348
3349// Copyright (c) 2019 Erin Catto
3350
3351// Permission is hereby granted, free of charge, to any person obtaining a copy
3352// of this software and associated documentation files (the "Software"), to deal
3353// in the Software without restriction, including without limitation the rights
3354// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
3355// copies of the Software, and to permit persons to whom the Software is
3356// furnished to do so, subject to the following conditions:
3357
3358// The above copyright notice and this permission notice shall be included in all
3359// copies or substantial portions of the Software.
3360
3361// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
3362// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
3363// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
3364// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
3365// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
3366// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
3367// SOFTWARE.
3368
3369#ifndef B2_CONTACT_H
3370#define B2_CONTACT_H
3371
3372//#include "b2_api.h"
3373//#include "b2_collision.h"
3374//#include "b2_fixture.h"
3375//#include "b2_math.h"
3376//#include "b2_shape.h"
3377
3378class b2Body;
3379class b2Contact;
3380class b2Fixture;
3381class b2World;
3382class b2BlockAllocator;
3383class b2StackAllocator;
3384class b2ContactListener;
3385
3386/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
3387/// For example, anything slides on ice.
3388inline float b2MixFriction(float friction1, float friction2) {
3389 return b2Sqrt(friction1 * friction2);
3390}
3391
3392/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
3393/// For example, a superball bounces on anything.
3394inline float b2MixRestitution(float restitution1, float restitution2) {
3395 return restitution1 > restitution2 ? restitution1 : restitution2;
3396}
3397
3398/// Restitution mixing law. This picks the lowest value.
3399inline float b2MixRestitutionThreshold(float threshold1, float threshold2) {
3400 return threshold1 < threshold2 ? threshold1 : threshold2;
3401}
3402
3403typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, int32 indexA,
3404 b2Fixture* fixtureB, int32 indexB,
3405 b2BlockAllocator* allocator);
3406typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
3407
3408struct B2_API b2ContactRegister {
3409 b2ContactCreateFcn* createFcn;
3410 b2ContactDestroyFcn* destroyFcn;
3411 bool primary;
3412};
3413
3414/// A contact edge is used to connect bodies and contacts together
3415/// in a contact graph where each body is a node and each contact
3416/// is an edge. A contact edge belongs to a doubly linked list
3417/// maintained in each attached body. Each contact has two contact
3418/// nodes, one for each attached body.
3419struct B2_API b2ContactEdge {
3420 b2Body* other; ///< provides quick access to the other body attached.
3421 b2Contact* contact; ///< the contact
3422 b2ContactEdge* prev; ///< the previous contact edge in the body's contact list
3423 b2ContactEdge* next; ///< the next contact edge in the body's contact list
3424};
3425
3426/// The class manages contact between two shapes. A contact exists for each overlapping
3427/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
3428/// that has no contact points.
3429class B2_API b2Contact {
3430public:
3431
3432 /// Get the contact manifold. Do not modify the manifold unless you understand the
3433 /// internals of Box2D.
3434 b2Manifold* GetManifold();
3435 const b2Manifold* GetManifold() const;
3436
3437 /// Get the world manifold.
3438 void GetWorldManifold(b2WorldManifold* worldManifold) const;
3439
3440 /// Is this contact touching?
3441 bool IsTouching() const;
3442
3443 /// Enable/disable this contact. This can be used inside the pre-solve
3444 /// contact listener. The contact is only disabled for the current
3445 /// time step (or sub-step in continuous collisions).
3446 void SetEnabled(bool flag);
3447
3448 /// Has this contact been disabled?
3449 bool IsEnabled() const;
3450
3451 /// Get the next contact in the world's contact list.
3452 b2Contact* GetNext();
3453 const b2Contact* GetNext() const;
3454
3455 /// Get fixture A in this contact.
3456 b2Fixture* GetFixtureA();
3457 const b2Fixture* GetFixtureA() const;
3458
3459 /// Get the child primitive index for fixture A.
3460 int32 GetChildIndexA() const;
3461
3462 /// Get fixture B in this contact.
3463 b2Fixture* GetFixtureB();
3464 const b2Fixture* GetFixtureB() const;
3465
3466 /// Get the child primitive index for fixture B.
3467 int32 GetChildIndexB() const;
3468
3469 /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
3470 /// This value persists until set or reset.
3471 void SetFriction(float friction);
3472
3473 /// Get the friction.
3474 float GetFriction() const;
3475
3476 /// Reset the friction mixture to the default value.
3477 void ResetFriction();
3478
3479 /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
3480 /// The value persists until you set or reset.
3481 void SetRestitution(float restitution);
3482
3483 /// Get the restitution.
3484 float GetRestitution() const;
3485
3486 /// Reset the restitution to the default value.
3487 void ResetRestitution();
3488
3489 /// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve.
3490 /// The value persists until you set or reset.
3491 void SetRestitutionThreshold(float threshold);
3492
3493 /// Get the restitution threshold.
3494 float GetRestitutionThreshold() const;
3495
3496 /// Reset the restitution threshold to the default value.
3497 void ResetRestitutionThreshold();
3498
3499 /// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
3500 void SetTangentSpeed(float speed);
3501
3502 /// Get the desired tangent speed. In meters per second.
3503 float GetTangentSpeed() const;
3504
3505 /// Evaluate this contact with your own manifold and transforms.
3506 virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
3507
3508protected:
3509 friend class b2ContactManager;
3510 friend class b2World;
3511 friend class b2ContactSolver;
3512 friend class b2Body;
3513 friend class b2Fixture;
3514
3515 // Flags stored in m_flags
3516 enum {
3517 // Used when crawling contact graph when forming islands.
3518 e_islandFlag = 0x0001,
3519
3520 // Set when the shapes are touching.
3521 e_touchingFlag = 0x0002,
3522
3523 // This contact can be disabled (by user)
3524 e_enabledFlag = 0x0004,
3525
3526 // This contact needs filtering because a fixture filter was changed.
3527 e_filterFlag = 0x0008,
3528
3529 // This bullet contact had a TOI event
3530 e_bulletHitFlag = 0x0010,
3531
3532 // This contact has a valid TOI in m_toi
3533 e_toiFlag = 0x0020
3534 };
3535
3536 /// Flag this contact for filtering. Filtering will occur the next time step.
3537 void FlagForFiltering();
3538
3539 static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
3540 b2Shape::Type typeA, b2Shape::Type typeB);
3541 static void InitializeRegisters();
3542 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
3543 static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
3544 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
3545
3546 b2Contact() : m_fixtureA(nullptr), m_fixtureB(nullptr) {}
3547 b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
3548 virtual ~b2Contact() {}
3549
3550 void Update(b2ContactListener* listener);
3551
3552 static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
3553 static bool s_initialized;
3554
3555 uint32 m_flags;
3556
3557 // World pool and list pointers.
3558 b2Contact* m_prev;
3559 b2Contact* m_next;
3560
3561 // Nodes for connecting bodies.
3562 b2ContactEdge m_nodeA;
3563 b2ContactEdge m_nodeB;
3564
3565 b2Fixture* m_fixtureA;
3566 b2Fixture* m_fixtureB;
3567
3568 int32 m_indexA;
3569 int32 m_indexB;
3570
3571 b2Manifold m_manifold;
3572
3573 int32 m_toiCount;
3574 float m_toi;
3575
3576 float m_friction;
3577 float m_restitution;
3578 float m_restitutionThreshold;
3579
3580 float m_tangentSpeed;
3581};
3582
3583inline b2Manifold* b2Contact::GetManifold() {
3584 return &m_manifold;
3585}
3586
3587inline const b2Manifold* b2Contact::GetManifold() const {
3588 return &m_manifold;
3589}
3590
3591inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const {
3592 const b2Body* bodyA = m_fixtureA->GetBody();
3593 const b2Body* bodyB = m_fixtureB->GetBody();
3594 const b2Shape* shapeA = m_fixtureA->GetShape();
3595 const b2Shape* shapeB = m_fixtureB->GetShape();
3596
3597 worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
3598}
3599
3600inline void b2Contact::SetEnabled(bool flag) {
3601 if (flag) {
3602 m_flags |= e_enabledFlag;
3603 } else {
3604 m_flags &= ~e_enabledFlag;
3605 }
3606}
3607
3608inline bool b2Contact::IsEnabled() const {
3609 return (m_flags & e_enabledFlag) == e_enabledFlag;
3610}
3611
3612inline bool b2Contact::IsTouching() const {
3613 return (m_flags & e_touchingFlag) == e_touchingFlag;
3614}
3615
3616inline b2Contact* b2Contact::GetNext() {
3617 return m_next;
3618}
3619
3620inline const b2Contact* b2Contact::GetNext() const {
3621 return m_next;
3622}
3623
3624inline b2Fixture* b2Contact::GetFixtureA() {
3625 return m_fixtureA;
3626}
3627
3628inline const b2Fixture* b2Contact::GetFixtureA() const {
3629 return m_fixtureA;
3630}
3631
3632inline b2Fixture* b2Contact::GetFixtureB() {
3633 return m_fixtureB;
3634}
3635
3636inline int32 b2Contact::GetChildIndexA() const {
3637 return m_indexA;
3638}
3639
3640inline const b2Fixture* b2Contact::GetFixtureB() const {
3641 return m_fixtureB;
3642}
3643
3644inline int32 b2Contact::GetChildIndexB() const {
3645 return m_indexB;
3646}
3647
3648inline void b2Contact::FlagForFiltering() {
3649 m_flags |= e_filterFlag;
3650}
3651
3652inline void b2Contact::SetFriction(float friction) {
3653 m_friction = friction;
3654}
3655
3656inline float b2Contact::GetFriction() const {
3657 return m_friction;
3658}
3659
3660inline void b2Contact::ResetFriction() {
3661 m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
3662}
3663
3664inline void b2Contact::SetRestitution(float restitution) {
3665 m_restitution = restitution;
3666}
3667
3668inline float b2Contact::GetRestitution() const {
3669 return m_restitution;
3670}
3671
3672inline void b2Contact::ResetRestitution() {
3673 m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
3674}
3675
3676inline void b2Contact::SetRestitutionThreshold(float threshold) {
3677 m_restitutionThreshold = threshold;
3678}
3679
3680inline float b2Contact::GetRestitutionThreshold() const {
3681 return m_restitutionThreshold;
3682}
3683
3684inline void b2Contact::ResetRestitutionThreshold() {
3685 m_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold);
3686}
3687
3688inline void b2Contact::SetTangentSpeed(float speed) {
3689 m_tangentSpeed = speed;
3690}
3691
3692inline float b2Contact::GetTangentSpeed() const {
3693 return m_tangentSpeed;
3694}
3695
3696#endif
3697// MIT License
3698
3699// Copyright (c) 2019 Erin Catto
3700
3701// Permission is hereby granted, free of charge, to any person obtaining a copy
3702// of this software and associated documentation files (the "Software"), to deal
3703// in the Software without restriction, including without limitation the rights
3704// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
3705// copies of the Software, and to permit persons to whom the Software is
3706// furnished to do so, subject to the following conditions:
3707
3708// The above copyright notice and this permission notice shall be included in all
3709// copies or substantial portions of the Software.
3710
3711// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
3712// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
3713// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
3714// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
3715// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
3716// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
3717// SOFTWARE.
3718
3719#ifndef B2_CONTACT_MANAGER_H
3720#define B2_CONTACT_MANAGER_H
3721
3722//#include "b2_api.h"
3723//#include "b2_broad_phase.h"
3724
3725class b2Contact;
3726class b2ContactFilter;
3727class b2ContactListener;
3728class b2BlockAllocator;
3729
3730// Delegate of b2World.
3731class B2_API b2ContactManager {
3732public:
3733 b2ContactManager();
3734
3735 // Broad-phase callback.
3736 void AddPair(void* proxyUserDataA, void* proxyUserDataB);
3737
3738 void FindNewContacts();
3739
3740 void Destroy(b2Contact* c);
3741
3742 void Collide();
3743
3744 b2BroadPhase m_broadPhase;
3745 b2Contact* m_contactList;
3746 int32 m_contactCount;
3747 b2ContactFilter* m_contactFilter;
3748 b2ContactListener* m_contactListener;
3749 b2BlockAllocator* m_allocator;
3750};
3751
3752#endif
3753// MIT License
3754
3755// Copyright (c) 2019 Erin Catto
3756
3757// Permission is hereby granted, free of charge, to any person obtaining a copy
3758// of this software and associated documentation files (the "Software"), to deal
3759// in the Software without restriction, including without limitation the rights
3760// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
3761// copies of the Software, and to permit persons to whom the Software is
3762// furnished to do so, subject to the following conditions:
3763
3764// The above copyright notice and this permission notice shall be included in all
3765// copies or substantial portions of the Software.
3766
3767// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
3768// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
3769// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
3770// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
3771// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
3772// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
3773// SOFTWARE.
3774
3775#ifndef B2_DISTANCE_H
3776#define B2_DISTANCE_H
3777
3778//#include "b2_api.h"
3779//#include "b2_math.h"
3780
3781class b2Shape;
3782
3783/// A distance proxy is used by the GJK algorithm.
3784/// It encapsulates any shape.
3785struct B2_API b2DistanceProxy {
3786 b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {}
3787
3788 /// Initialize the proxy using the given shape. The shape
3789 /// must remain in scope while the proxy is in use.
3790 void Set(const b2Shape* shape, int32 index);
3791
3792 /// Initialize the proxy using a vertex cloud and radius. The vertices
3793 /// must remain in scope while the proxy is in use.
3794 void Set(const b2Vec2* vertices, int32 count, float radius);
3795
3796 /// Get the supporting vertex index in the given direction.
3797 int32 GetSupport(const b2Vec2& d) const;
3798
3799 /// Get the supporting vertex in the given direction.
3800 const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
3801
3802 /// Get the vertex count.
3803 int32 GetVertexCount() const;
3804
3805 /// Get a vertex by index. Used by b2Distance.
3806 const b2Vec2& GetVertex(int32 index) const;
3807
3808 b2Vec2 m_buffer[2];
3809 const b2Vec2* m_vertices;
3810 int32 m_count;
3811 float m_radius;
3812};
3813
3814/// Used to warm start b2Distance.
3815/// Set count to zero on first call.
3816struct B2_API b2SimplexCache {
3817 float metric; ///< length or area
3818 uint16 count;
3819 uint8 indexA[3]; ///< vertices on shape A
3820 uint8 indexB[3]; ///< vertices on shape B
3821};
3822
3823/// Input for b2Distance.
3824/// You have to option to use the shape radii
3825/// in the computation. Even
3826struct B2_API b2DistanceInput {
3827 b2DistanceProxy proxyA;
3828 b2DistanceProxy proxyB;
3829 b2Transform transformA;
3830 b2Transform transformB;
3831 bool useRadii;
3832};
3833
3834/// Output for b2Distance.
3835struct B2_API b2DistanceOutput {
3836 b2Vec2 pointA; ///< closest point on shapeA
3837 b2Vec2 pointB; ///< closest point on shapeB
3838 float distance;
3839 int32 iterations; ///< number of GJK iterations used
3840};
3841
3842/// Compute the closest points between two shapes. Supports any combination of:
3843/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
3844/// On the first call set b2SimplexCache.count to zero.
3845B2_API void b2Distance(b2DistanceOutput* output,
3846 b2SimplexCache* cache,
3847 const b2DistanceInput* input);
3848
3849/// Input parameters for b2ShapeCast
3850struct B2_API b2ShapeCastInput {
3851 b2DistanceProxy proxyA;
3852 b2DistanceProxy proxyB;
3853 b2Transform transformA;
3854 b2Transform transformB;
3855 b2Vec2 translationB;
3856};
3857
3858/// Output results for b2ShapeCast
3859struct B2_API b2ShapeCastOutput {
3860 b2Vec2 point;
3861 b2Vec2 normal;
3862 float lambda;
3863 int32 iterations;
3864};
3865
3866/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
3867/// @returns true if hit, false if there is no hit or an initial overlap
3868B2_API bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input);
3869
3870//////////////////////////////////////////////////////////////////////////
3871
3872inline int32 b2DistanceProxy::GetVertexCount() const {
3873 return m_count;
3874}
3875
3876inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const {
3877 b2Assert(0 <= index && index < m_count);
3878 return m_vertices[index];
3879}
3880
3881inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const {
3882 int32 bestIndex = 0;
3883 float bestValue = b2Dot(m_vertices[0], d);
3884 for (int32 i = 1; i < m_count; ++i) {
3885 float value = b2Dot(m_vertices[i], d);
3886 if (value > bestValue) {
3887 bestIndex = i;
3888 bestValue = value;
3889 }
3890 }
3891
3892 return bestIndex;
3893}
3894
3895inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const {
3896 int32 bestIndex = 0;
3897 float bestValue = b2Dot(m_vertices[0], d);
3898 for (int32 i = 1; i < m_count; ++i) {
3899 float value = b2Dot(m_vertices[i], d);
3900 if (value > bestValue) {
3901 bestIndex = i;
3902 bestValue = value;
3903 }
3904 }
3905
3906 return m_vertices[bestIndex];
3907}
3908
3909#endif
3910// MIT License
3911
3912// Copyright (c) 2019 Erin Catto
3913
3914// Permission is hereby granted, free of charge, to any person obtaining a copy
3915// of this software and associated documentation files (the "Software"), to deal
3916// in the Software without restriction, including without limitation the rights
3917// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
3918// copies of the Software, and to permit persons to whom the Software is
3919// furnished to do so, subject to the following conditions:
3920
3921// The above copyright notice and this permission notice shall be included in all
3922// copies or substantial portions of the Software.
3923
3924// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
3925// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
3926// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
3927// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
3928// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
3929// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
3930// SOFTWARE.
3931
3932#ifndef B2_JOINT_H
3933#define B2_JOINT_H
3934
3935//#include "b2_api.h"
3936//#include "b2_math.h"
3937
3938class b2Body;
3939class b2Draw;
3940class b2Joint;
3941struct b2SolverData;
3942class b2BlockAllocator;
3943
3944enum b2JointType {
3945 e_unknownJoint,
3946 e_revoluteJoint,
3947 e_prismaticJoint,
3948 e_distanceJoint,
3949 e_pulleyJoint,
3950 e_mouseJoint,
3951 e_gearJoint,
3952 e_wheelJoint,
3953 e_weldJoint,
3954 e_frictionJoint,
3955 e_ropeJoint,
3956 e_motorJoint
3957};
3958
3959struct B2_API b2Jacobian {
3960 b2Vec2 linear;
3961 float angularA;
3962 float angularB;
3963};
3964
3965/// A joint edge is used to connect bodies and joints together
3966/// in a joint graph where each body is a node and each joint
3967/// is an edge. A joint edge belongs to a doubly linked list
3968/// maintained in each attached body. Each joint has two joint
3969/// nodes, one for each attached body.
3970struct B2_API b2JointEdge {
3971 b2Body* other; ///< provides quick access to the other body attached.
3972 b2Joint* joint; ///< the joint
3973 b2JointEdge* prev; ///< the previous joint edge in the body's joint list
3974 b2JointEdge* next; ///< the next joint edge in the body's joint list
3975};
3976
3977/// Joint definitions are used to construct joints.
3978struct B2_API b2JointDef {
3979 b2JointDef() {
3980 type = e_unknownJoint;
3981 bodyA = nullptr;
3982 bodyB = nullptr;
3983 collideConnected = false;
3984 }
3985
3986 /// The joint type is set automatically for concrete joint types.
3987 b2JointType type;
3988
3989 /// Use this to attach application specific data to your joints.
3990 b2JointUserData userData;
3991
3992 /// The first attached body.
3993 b2Body* bodyA;
3994
3995 /// The second attached body.
3996 b2Body* bodyB;
3997
3998 /// Set this flag to true if the attached bodies should collide.
3999 bool collideConnected;
4000};
4001
4002/// Utility to compute linear stiffness values from frequency and damping ratio
4003B2_API void b2LinearStiffness(float& stiffness, float& damping,
4004 float frequencyHertz, float dampingRatio,
4005 const b2Body* bodyA, const b2Body* bodyB);
4006
4007/// Utility to compute rotational stiffness values frequency and damping ratio
4008B2_API void b2AngularStiffness(float& stiffness, float& damping,
4009 float frequencyHertz, float dampingRatio,
4010 const b2Body* bodyA, const b2Body* bodyB);
4011
4012/// The base joint class. Joints are used to constraint two bodies together in
4013/// various fashions. Some joints also feature limits and motors.
4014class B2_API b2Joint {
4015public:
4016
4017 /// Get the type of the concrete joint.
4018 b2JointType GetType() const;
4019
4020 /// Get the first body attached to this joint.
4021 b2Body* GetBodyA();
4022
4023 /// Get the second body attached to this joint.
4024 b2Body* GetBodyB();
4025
4026 /// Get the anchor point on bodyA in world coordinates.
4027 virtual b2Vec2 GetAnchorA() const = 0;
4028
4029 /// Get the anchor point on bodyB in world coordinates.
4030 virtual b2Vec2 GetAnchorB() const = 0;
4031
4032 /// Get the reaction force on bodyB at the joint anchor in Newtons.
4033 virtual b2Vec2 GetReactionForce(float inv_dt) const = 0;
4034
4035 /// Get the reaction torque on bodyB in N*m.
4036 virtual float GetReactionTorque(float inv_dt) const = 0;
4037
4038 /// Get the next joint the world joint list.
4039 b2Joint* GetNext();
4040 const b2Joint* GetNext() const;
4041
4042 /// Get the user data pointer.
4043 b2JointUserData& GetUserData();
4044
4045 /// Short-cut function to determine if either body is enabled.
4046 bool IsEnabled() const;
4047
4048 /// Get collide connected.
4049 /// Note: modifying the collide connect flag won't work correctly because
4050 /// the flag is only checked when fixture AABBs begin to overlap.
4051 bool GetCollideConnected() const;
4052
4053 /// Dump this joint to the log file.
4054 virtual void Dump() { b2Dump("// Dump is not supported for this joint type.\n"); }
4055
4056 /// Shift the origin for any points stored in world coordinates.
4057 virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); }
4058
4059 /// Debug draw this joint
4060 virtual void Draw(b2Draw* draw) const;
4061
4062protected:
4063 friend class b2World;
4064 friend class b2Body;
4065 friend class b2Island;
4066 friend class b2GearJoint;
4067
4068 static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
4069 static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
4070
4071 b2Joint(const b2JointDef* def);
4072 virtual ~b2Joint() {}
4073
4074 virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
4075 virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
4076
4077 // This returns true if the position errors are within tolerance.
4078 virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
4079
4080 b2JointType m_type;
4081 b2Joint* m_prev;
4082 b2Joint* m_next;
4083 b2JointEdge m_edgeA;
4084 b2JointEdge m_edgeB;
4085 b2Body* m_bodyA;
4086 b2Body* m_bodyB;
4087
4088 int32 m_index;
4089
4090 bool m_islandFlag;
4091 bool m_collideConnected;
4092
4093 b2JointUserData m_userData;
4094};
4095
4096inline b2JointType b2Joint::GetType() const {
4097 return m_type;
4098}
4099
4100inline b2Body* b2Joint::GetBodyA() {
4101 return m_bodyA;
4102}
4103
4104inline b2Body* b2Joint::GetBodyB() {
4105 return m_bodyB;
4106}
4107
4108inline b2Joint* b2Joint::GetNext() {
4109 return m_next;
4110}
4111
4112inline const b2Joint* b2Joint::GetNext() const {
4113 return m_next;
4114}
4115
4116inline b2JointUserData& b2Joint::GetUserData() {
4117 return m_userData;
4118}
4119
4120inline bool b2Joint::GetCollideConnected() const {
4121 return m_collideConnected;
4122}
4123
4124#endif
4125// MIT License
4126
4127// Copyright (c) 2019 Erin Catto
4128
4129// Permission is hereby granted, free of charge, to any person obtaining a copy
4130// of this software and associated documentation files (the "Software"), to deal
4131// in the Software without restriction, including without limitation the rights
4132// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4133// copies of the Software, and to permit persons to whom the Software is
4134// furnished to do so, subject to the following conditions:
4135
4136// The above copyright notice and this permission notice shall be included in all
4137// copies or substantial portions of the Software.
4138
4139// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4140// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4141// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4142// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4143// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4144// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4145// SOFTWARE.
4146
4147#ifndef B2_DISTANCE_JOINT_H
4148#define B2_DISTANCE_JOINT_H
4149
4150//#include "b2_api.h"
4151//#include "b2_joint.h"
4152
4153/// Distance joint definition. This requires defining an anchor point on both
4154/// bodies and the non-zero distance of the distance joint. The definition uses
4155/// local anchor points so that the initial configuration can violate the
4156/// constraint slightly. This helps when saving and loading a game.
4157struct B2_API b2DistanceJointDef : public b2JointDef {
4158 b2DistanceJointDef() {
4159 type = e_distanceJoint;
4160 localAnchorA.Set(0.0f, 0.0f);
4161 localAnchorB.Set(0.0f, 0.0f);
4162 length = 1.0f;
4163 minLength = 0.0f;
4164 maxLength = FLT_MAX;
4165 stiffness = 0.0f;
4166 damping = 0.0f;
4167 }
4168
4169 /// Initialize the bodies, anchors, and rest length using world space anchors.
4170 /// The minimum and maximum lengths are set to the rest length.
4171 void Initialize(b2Body* bodyA, b2Body* bodyB,
4172 const b2Vec2& anchorA, const b2Vec2& anchorB);
4173
4174 /// The local anchor point relative to bodyA's origin.
4175 b2Vec2 localAnchorA;
4176
4177 /// The local anchor point relative to bodyB's origin.
4178 b2Vec2 localAnchorB;
4179
4180 /// The rest length of this joint. Clamped to a stable minimum value.
4181 float length;
4182
4183 /// Minimum length. Clamped to a stable minimum value.
4184 float minLength;
4185
4186 /// Maximum length. Must be greater than or equal to the minimum length.
4187 float maxLength;
4188
4189 /// The linear stiffness in N/m.
4190 float stiffness;
4191
4192 /// The linear damping in N*s/m.
4193 float damping;
4194};
4195
4196/// A distance joint constrains two points on two bodies to remain at a fixed
4197/// distance from each other. You can view this as a massless, rigid rod.
4198class B2_API b2DistanceJoint : public b2Joint {
4199public:
4200
4201 b2Vec2 GetAnchorA() const override;
4202 b2Vec2 GetAnchorB() const override;
4203
4204 /// Get the reaction force given the inverse time step.
4205 /// Unit is N.
4206 b2Vec2 GetReactionForce(float inv_dt) const override;
4207
4208 /// Get the reaction torque given the inverse time step.
4209 /// Unit is N*m. This is always zero for a distance joint.
4210 float GetReactionTorque(float inv_dt) const override;
4211
4212 /// The local anchor point relative to bodyA's origin.
4213 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
4214
4215 /// The local anchor point relative to bodyB's origin.
4216 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
4217
4218 /// Get the rest length
4219 float GetLength() const { return m_length; }
4220
4221 /// Set the rest length
4222 /// @returns clamped rest length
4223 float SetLength(float length);
4224
4225 /// Get the minimum length
4226 float GetMinLength() const { return m_minLength; }
4227
4228 /// Set the minimum length
4229 /// @returns the clamped minimum length
4230 float SetMinLength(float minLength);
4231
4232 /// Get the maximum length
4233 float GetMaxLength() const { return m_maxLength; }
4234
4235 /// Set the maximum length
4236 /// @returns the clamped maximum length
4237 float SetMaxLength(float maxLength);
4238
4239 /// Get the current length
4240 float GetCurrentLength() const;
4241
4242 /// Set/get the linear stiffness in N/m
4243 void SetStiffness(float stiffness) { m_stiffness = stiffness; }
4244 float GetStiffness() const { return m_stiffness; }
4245
4246 /// Set/get linear damping in N*s/m
4247 void SetDamping(float damping) { m_damping = damping; }
4248 float GetDamping() const { return m_damping; }
4249
4250 /// Dump joint to dmLog
4251 void Dump() override;
4252
4253 ///
4254 void Draw(b2Draw* draw) const override;
4255
4256protected:
4257
4258 friend class b2Joint;
4259 b2DistanceJoint(const b2DistanceJointDef* data);
4260
4261 void InitVelocityConstraints(const b2SolverData& data) override;
4262 void SolveVelocityConstraints(const b2SolverData& data) override;
4263 bool SolvePositionConstraints(const b2SolverData& data) override;
4264
4265 float m_stiffness;
4266 float m_damping;
4267 float m_bias;
4268 float m_length;
4269 float m_minLength;
4270 float m_maxLength;
4271
4272 // Solver shared
4273 b2Vec2 m_localAnchorA;
4274 b2Vec2 m_localAnchorB;
4275 float m_gamma;
4276 float m_impulse;
4277 float m_lowerImpulse;
4278 float m_upperImpulse;
4279
4280 // Solver temp
4281 int32 m_indexA;
4282 int32 m_indexB;
4283 b2Vec2 m_u;
4284 b2Vec2 m_rA;
4285 b2Vec2 m_rB;
4286 b2Vec2 m_localCenterA;
4287 b2Vec2 m_localCenterB;
4288 float m_currentLength;
4289 float m_invMassA;
4290 float m_invMassB;
4291 float m_invIA;
4292 float m_invIB;
4293 float m_softMass;
4294 float m_mass;
4295};
4296
4297#endif
4298// MIT License
4299
4300// Copyright (c) 2019 Erin Catto
4301
4302// Permission is hereby granted, free of charge, to any person obtaining a copy
4303// of this software and associated documentation files (the "Software"), to deal
4304// in the Software without restriction, including without limitation the rights
4305// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4306// copies of the Software, and to permit persons to whom the Software is
4307// furnished to do so, subject to the following conditions:
4308
4309// The above copyright notice and this permission notice shall be included in all
4310// copies or substantial portions of the Software.
4311
4312// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4313// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4314// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4315// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4316// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4317// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4318// SOFTWARE.
4319
4320#ifndef B2_DRAW_H
4321#define B2_DRAW_H
4322
4323//#include "b2_api.h"
4324//#include "b2_math.h"
4325
4326/// Color for debug drawing. Each value has the range [0,1].
4327struct B2_API b2Color {
4328 b2Color() {}
4329 b2Color(float rIn, float gIn, float bIn, float aIn = 1.0f) {
4330 r = rIn; g = gIn; b = bIn; a = aIn;
4331 }
4332
4333 void Set(float rIn, float gIn, float bIn, float aIn = 1.0f) {
4334 r = rIn; g = gIn; b = bIn; a = aIn;
4335 }
4336
4337 float r, g, b, a;
4338};
4339
4340/// Implement and register this class with a b2World to provide debug drawing of physics
4341/// entities in your game.
4342class B2_API b2Draw {
4343public:
4344 b2Draw();
4345
4346 virtual ~b2Draw() {}
4347
4348 enum {
4349 e_shapeBit = 0x0001, ///< draw shapes
4350 e_jointBit = 0x0002, ///< draw joint connections
4351 e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes
4352 e_pairBit = 0x0008, ///< draw broad-phase pairs
4353 e_centerOfMassBit = 0x0010 ///< draw center of mass frame
4354 };
4355
4356 /// Set the drawing flags.
4357 void SetFlags(uint32 flags);
4358
4359 /// Get the drawing flags.
4360 uint32 GetFlags() const;
4361
4362 /// Append flags to the current flags.
4363 void AppendFlags(uint32 flags);
4364
4365 /// Clear flags from the current flags.
4366 void ClearFlags(uint32 flags);
4367
4368 /// Draw a closed polygon provided in CCW order.
4369 virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
4370
4371 /// Draw a solid closed polygon provided in CCW order.
4372 virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
4373
4374 /// Draw a circle.
4375 virtual void DrawCircle(const b2Vec2& center, float radius, const b2Color& color) = 0;
4376
4377 /// Draw a solid circle.
4378 virtual void DrawSolidCircle(const b2Vec2& center, float radius, const b2Vec2& axis, const b2Color& color) = 0;
4379
4380 /// Draw a line segment.
4381 virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
4382
4383 /// Draw a transform. Choose your own length scale.
4384 /// @param xf a transform.
4385 virtual void DrawTransform(const b2Transform& xf) = 0;
4386
4387 /// Draw a point.
4388 virtual void DrawPoint(const b2Vec2& p, float size, const b2Color& color) = 0;
4389
4390protected:
4391 uint32 m_drawFlags;
4392};
4393
4394#endif
4395// MIT License
4396
4397// Copyright (c) 2019 Erin Catto
4398
4399// Permission is hereby granted, free of charge, to any person obtaining a copy
4400// of this software and associated documentation files (the "Software"), to deal
4401// in the Software without restriction, including without limitation the rights
4402// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4403// copies of the Software, and to permit persons to whom the Software is
4404// furnished to do so, subject to the following conditions:
4405
4406// The above copyright notice and this permission notice shall be included in all
4407// copies or substantial portions of the Software.
4408
4409// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4410// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4411// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4412// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4413// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4414// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4415// SOFTWARE.
4416
4417#ifndef B2_EDGE_SHAPE_H
4418#define B2_EDGE_SHAPE_H
4419
4420//#include "b2_api.h"
4421//#include "b2_shape.h"
4422
4423/// A line segment (edge) shape. These can be connected in chains or loops
4424/// to other edge shapes. Edges created independently are two-sided and do
4425/// no provide smooth movement across junctions.
4426class B2_API b2EdgeShape : public b2Shape {
4427public:
4428 b2EdgeShape();
4429
4430 /// Set this as a part of a sequence. Vertex v0 precedes the edge and vertex v3
4431 /// follows. These extra vertices are used to provide smooth movement
4432 /// across junctions. This also makes the collision one-sided. The edge
4433 /// normal points to the right looking from v1 to v2.
4434 void SetOneSided(const b2Vec2& v0, const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& v3);
4435
4436 /// Set this as an isolated edge. Collision is two-sided.
4437 void SetTwoSided(const b2Vec2& v1, const b2Vec2& v2);
4438
4439 /// Implement b2Shape.
4440 b2Shape* Clone(b2BlockAllocator* allocator) const override;
4441
4442 /// @see b2Shape::GetChildCount
4443 int32 GetChildCount() const override;
4444
4445 /// @see b2Shape::TestPoint
4446 bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
4447
4448 /// Implement b2Shape.
4449 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
4450 const b2Transform& transform, int32 childIndex) const override;
4451
4452 /// @see b2Shape::ComputeAABB
4453 void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
4454
4455 /// @see b2Shape::ComputeMass
4456 void ComputeMass(b2MassData* massData, float density) const override;
4457
4458 /// These are the edge vertices
4459 b2Vec2 m_vertex1, m_vertex2;
4460
4461 /// Optional adjacent vertices. These are used for smooth collision.
4462 b2Vec2 m_vertex0, m_vertex3;
4463
4464 /// Uses m_vertex0 and m_vertex3 to create smooth collision.
4465 bool m_oneSided;
4466};
4467
4468inline b2EdgeShape::b2EdgeShape() {
4469 m_type = e_edge;
4470 m_radius = b2_polygonRadius;
4471 m_vertex0.x = 0.0f;
4472 m_vertex0.y = 0.0f;
4473 m_vertex3.x = 0.0f;
4474 m_vertex3.y = 0.0f;
4475 m_oneSided = false;
4476}
4477
4478#endif
4479// MIT License
4480
4481// Copyright (c) 2019 Erin Catto
4482
4483// Permission is hereby granted, free of charge, to any person obtaining a copy
4484// of this software and associated documentation files (the "Software"), to deal
4485// in the Software without restriction, including without limitation the rights
4486// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4487// copies of the Software, and to permit persons to whom the Software is
4488// furnished to do so, subject to the following conditions:
4489
4490// The above copyright notice and this permission notice shall be included in all
4491// copies or substantial portions of the Software.
4492
4493// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4494// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4495// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4496// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4497// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4498// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4499// SOFTWARE.
4500
4501#ifndef B2_FRICTION_JOINT_H
4502#define B2_FRICTION_JOINT_H
4503
4504//#include "b2_api.h"
4505//#include "b2_joint.h"
4506
4507/// Friction joint definition.
4508struct B2_API b2FrictionJointDef : public b2JointDef {
4509 b2FrictionJointDef() {
4510 type = e_frictionJoint;
4511 localAnchorA.SetZero();
4512 localAnchorB.SetZero();
4513 maxForce = 0.0f;
4514 maxTorque = 0.0f;
4515 }
4516
4517 /// Initialize the bodies, anchors, axis, and reference angle using the world
4518 /// anchor and world axis.
4519 void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
4520
4521 /// The local anchor point relative to bodyA's origin.
4522 b2Vec2 localAnchorA;
4523
4524 /// The local anchor point relative to bodyB's origin.
4525 b2Vec2 localAnchorB;
4526
4527 /// The maximum friction force in N.
4528 float maxForce;
4529
4530 /// The maximum friction torque in N-m.
4531 float maxTorque;
4532};
4533
4534/// Friction joint. This is used for top-down friction.
4535/// It provides 2D translational friction and angular friction.
4536class B2_API b2FrictionJoint : public b2Joint {
4537public:
4538 b2Vec2 GetAnchorA() const override;
4539 b2Vec2 GetAnchorB() const override;
4540
4541 b2Vec2 GetReactionForce(float inv_dt) const override;
4542 float GetReactionTorque(float inv_dt) const override;
4543
4544 /// The local anchor point relative to bodyA's origin.
4545 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
4546
4547 /// The local anchor point relative to bodyB's origin.
4548 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
4549
4550 /// Set the maximum friction force in N.
4551 void SetMaxForce(float force);
4552
4553 /// Get the maximum friction force in N.
4554 float GetMaxForce() const;
4555
4556 /// Set the maximum friction torque in N*m.
4557 void SetMaxTorque(float torque);
4558
4559 /// Get the maximum friction torque in N*m.
4560 float GetMaxTorque() const;
4561
4562 /// Dump joint to dmLog
4563 void Dump() override;
4564
4565protected:
4566
4567 friend class b2Joint;
4568
4569 b2FrictionJoint(const b2FrictionJointDef* def);
4570
4571 void InitVelocityConstraints(const b2SolverData& data) override;
4572 void SolveVelocityConstraints(const b2SolverData& data) override;
4573 bool SolvePositionConstraints(const b2SolverData& data) override;
4574
4575 b2Vec2 m_localAnchorA;
4576 b2Vec2 m_localAnchorB;
4577
4578 // Solver shared
4579 b2Vec2 m_linearImpulse;
4580 float m_angularImpulse;
4581 float m_maxForce;
4582 float m_maxTorque;
4583
4584 // Solver temp
4585 int32 m_indexA;
4586 int32 m_indexB;
4587 b2Vec2 m_rA;
4588 b2Vec2 m_rB;
4589 b2Vec2 m_localCenterA;
4590 b2Vec2 m_localCenterB;
4591 float m_invMassA;
4592 float m_invMassB;
4593 float m_invIA;
4594 float m_invIB;
4595 b2Mat22 m_linearMass;
4596 float m_angularMass;
4597};
4598
4599#endif
4600// MIT License
4601
4602// Copyright (c) 2019 Erin Catto
4603
4604// Permission is hereby granted, free of charge, to any person obtaining a copy
4605// of this software and associated documentation files (the "Software"), to deal
4606// in the Software without restriction, including without limitation the rights
4607// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4608// copies of the Software, and to permit persons to whom the Software is
4609// furnished to do so, subject to the following conditions:
4610
4611// The above copyright notice and this permission notice shall be included in all
4612// copies or substantial portions of the Software.
4613
4614// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4615// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4616// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4617// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4618// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4619// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4620// SOFTWARE.
4621
4622#ifndef B2_GEAR_JOINT_H
4623#define B2_GEAR_JOINT_H
4624
4625//#include "b2_joint.h"
4626
4627/// Gear joint definition. This definition requires two existing
4628/// revolute or prismatic joints (any combination will work).
4629/// @warning bodyB on the input joints must both be dynamic
4630struct B2_API b2GearJointDef : public b2JointDef {
4631 b2GearJointDef() {
4632 type = e_gearJoint;
4633 joint1 = nullptr;
4634 joint2 = nullptr;
4635 ratio = 1.0f;
4636 }
4637
4638 /// The first revolute/prismatic joint attached to the gear joint.
4639 b2Joint* joint1;
4640
4641 /// The second revolute/prismatic joint attached to the gear joint.
4642 b2Joint* joint2;
4643
4644 /// The gear ratio.
4645 /// @see b2GearJoint for explanation.
4646 float ratio;
4647};
4648
4649/// A gear joint is used to connect two joints together. Either joint
4650/// can be a revolute or prismatic joint. You specify a gear ratio
4651/// to bind the motions together:
4652/// coordinate1 + ratio * coordinate2 = constant
4653/// The ratio can be negative or positive. If one joint is a revolute joint
4654/// and the other joint is a prismatic joint, then the ratio will have units
4655/// of length or units of 1/length.
4656/// @warning You have to manually destroy the gear joint if joint1 or joint2
4657/// is destroyed.
4658class B2_API b2GearJoint : public b2Joint {
4659public:
4660 b2Vec2 GetAnchorA() const override;
4661 b2Vec2 GetAnchorB() const override;
4662
4663 b2Vec2 GetReactionForce(float inv_dt) const override;
4664 float GetReactionTorque(float inv_dt) const override;
4665
4666 /// Get the first joint.
4667 b2Joint* GetJoint1() { return m_joint1; }
4668
4669 /// Get the second joint.
4670 b2Joint* GetJoint2() { return m_joint2; }
4671
4672 /// Set/Get the gear ratio.
4673 void SetRatio(float ratio);
4674 float GetRatio() const;
4675
4676 /// Dump joint to dmLog
4677 void Dump() override;
4678
4679protected:
4680
4681 friend class b2Joint;
4682 b2GearJoint(const b2GearJointDef* data);
4683
4684 void InitVelocityConstraints(const b2SolverData& data) override;
4685 void SolveVelocityConstraints(const b2SolverData& data) override;
4686 bool SolvePositionConstraints(const b2SolverData& data) override;
4687
4688 b2Joint* m_joint1;
4689 b2Joint* m_joint2;
4690
4691 b2JointType m_typeA;
4692 b2JointType m_typeB;
4693
4694 // Body A is connected to body C
4695 // Body B is connected to body D
4696 b2Body* m_bodyC;
4697 b2Body* m_bodyD;
4698
4699 // Solver shared
4700 b2Vec2 m_localAnchorA;
4701 b2Vec2 m_localAnchorB;
4702 b2Vec2 m_localAnchorC;
4703 b2Vec2 m_localAnchorD;
4704
4705 b2Vec2 m_localAxisC;
4706 b2Vec2 m_localAxisD;
4707
4708 float m_referenceAngleA;
4709 float m_referenceAngleB;
4710
4711 float m_constant;
4712 float m_ratio;
4713
4714 float m_impulse;
4715
4716 // Solver temp
4717 int32 m_indexA, m_indexB, m_indexC, m_indexD;
4718 b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
4719 float m_mA, m_mB, m_mC, m_mD;
4720 float m_iA, m_iB, m_iC, m_iD;
4721 b2Vec2 m_JvAC, m_JvBD;
4722 float m_JwA, m_JwB, m_JwC, m_JwD;
4723 float m_mass;
4724};
4725
4726#endif
4727// MIT License
4728
4729// Copyright (c) 2019 Erin Catto
4730
4731// Permission is hereby granted, free of charge, to any person obtaining a copy
4732// of this software and associated documentation files (the "Software"), to deal
4733// in the Software without restriction, including without limitation the rights
4734// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4735// copies of the Software, and to permit persons to whom the Software is
4736// furnished to do so, subject to the following conditions:
4737
4738// The above copyright notice and this permission notice shall be included in all
4739// copies or substantial portions of the Software.
4740
4741// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4742// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4743// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4744// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4745// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4746// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4747// SOFTWARE.
4748
4749#ifndef B2_MOTOR_JOINT_H
4750#define B2_MOTOR_JOINT_H
4751
4752//#include "b2_api.h"
4753//#include "b2_joint.h"
4754
4755/// Motor joint definition.
4756struct B2_API b2MotorJointDef : public b2JointDef {
4757 b2MotorJointDef() {
4758 type = e_motorJoint;
4759 linearOffset.SetZero();
4760 angularOffset = 0.0f;
4761 maxForce = 1.0f;
4762 maxTorque = 1.0f;
4763 correctionFactor = 0.3f;
4764 }
4765
4766 /// Initialize the bodies and offsets using the current transforms.
4767 void Initialize(b2Body* bodyA, b2Body* bodyB);
4768
4769 /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
4770 b2Vec2 linearOffset;
4771
4772 /// The bodyB angle minus bodyA angle in radians.
4773 float angularOffset;
4774
4775 /// The maximum motor force in N.
4776 float maxForce;
4777
4778 /// The maximum motor torque in N-m.
4779 float maxTorque;
4780
4781 /// Position correction factor in the range [0,1].
4782 float correctionFactor;
4783};
4784
4785/// A motor joint is used to control the relative motion
4786/// between two bodies. A typical usage is to control the movement
4787/// of a dynamic body with respect to the ground.
4788class B2_API b2MotorJoint : public b2Joint {
4789public:
4790 b2Vec2 GetAnchorA() const override;
4791 b2Vec2 GetAnchorB() const override;
4792
4793 b2Vec2 GetReactionForce(float inv_dt) const override;
4794 float GetReactionTorque(float inv_dt) const override;
4795
4796 /// Set/get the target linear offset, in frame A, in meters.
4797 void SetLinearOffset(const b2Vec2& linearOffset);
4798 const b2Vec2& GetLinearOffset() const;
4799
4800 /// Set/get the target angular offset, in radians.
4801 void SetAngularOffset(float angularOffset);
4802 float GetAngularOffset() const;
4803
4804 /// Set the maximum friction force in N.
4805 void SetMaxForce(float force);
4806
4807 /// Get the maximum friction force in N.
4808 float GetMaxForce() const;
4809
4810 /// Set the maximum friction torque in N*m.
4811 void SetMaxTorque(float torque);
4812
4813 /// Get the maximum friction torque in N*m.
4814 float GetMaxTorque() const;
4815
4816 /// Set the position correction factor in the range [0,1].
4817 void SetCorrectionFactor(float factor);
4818
4819 /// Get the position correction factor in the range [0,1].
4820 float GetCorrectionFactor() const;
4821
4822 /// Dump to b2Log
4823 void Dump() override;
4824
4825protected:
4826
4827 friend class b2Joint;
4828
4829 b2MotorJoint(const b2MotorJointDef* def);
4830
4831 void InitVelocityConstraints(const b2SolverData& data) override;
4832 void SolveVelocityConstraints(const b2SolverData& data) override;
4833 bool SolvePositionConstraints(const b2SolverData& data) override;
4834
4835 // Solver shared
4836 b2Vec2 m_linearOffset;
4837 float m_angularOffset;
4838 b2Vec2 m_linearImpulse;
4839 float m_angularImpulse;
4840 float m_maxForce;
4841 float m_maxTorque;
4842 float m_correctionFactor;
4843
4844 // Solver temp
4845 int32 m_indexA;
4846 int32 m_indexB;
4847 b2Vec2 m_rA;
4848 b2Vec2 m_rB;
4849 b2Vec2 m_localCenterA;
4850 b2Vec2 m_localCenterB;
4851 b2Vec2 m_linearError;
4852 float m_angularError;
4853 float m_invMassA;
4854 float m_invMassB;
4855 float m_invIA;
4856 float m_invIB;
4857 b2Mat22 m_linearMass;
4858 float m_angularMass;
4859};
4860
4861#endif
4862// MIT License
4863
4864// Copyright (c) 2019 Erin Catto
4865
4866// Permission is hereby granted, free of charge, to any person obtaining a copy
4867// of this software and associated documentation files (the "Software"), to deal
4868// in the Software without restriction, including without limitation the rights
4869// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
4870// copies of the Software, and to permit persons to whom the Software is
4871// furnished to do so, subject to the following conditions:
4872
4873// The above copyright notice and this permission notice shall be included in all
4874// copies or substantial portions of the Software.
4875
4876// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
4877// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
4878// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
4879// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
4880// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
4881// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
4882// SOFTWARE.
4883
4884#ifndef B2_MOUSE_JOINT_H
4885#define B2_MOUSE_JOINT_H
4886
4887//#include "b2_api.h"
4888//#include "b2_joint.h"
4889
4890/// Mouse joint definition. This requires a world target point,
4891/// tuning parameters, and the time step.
4892struct B2_API b2MouseJointDef : public b2JointDef {
4893 b2MouseJointDef() {
4894 type = e_mouseJoint;
4895 target.Set(0.0f, 0.0f);
4896 maxForce = 0.0f;
4897 stiffness = 0.0f;
4898 damping = 0.0f;
4899 }
4900
4901 /// The initial world target point. This is assumed
4902 /// to coincide with the body anchor initially.
4903 b2Vec2 target;
4904
4905 /// The maximum constraint force that can be exerted
4906 /// to move the candidate body. Usually you will express
4907 /// as some multiple of the weight (multiplier * mass * gravity).
4908 float maxForce;
4909
4910 /// The linear stiffness in N/m
4911 float stiffness;
4912
4913 /// The linear damping in N*s/m
4914 float damping;
4915};
4916
4917/// A mouse joint is used to make a point on a body track a
4918/// specified world point. This a soft constraint with a maximum
4919/// force. This allows the constraint to stretch and without
4920/// applying huge forces.
4921/// NOTE: this joint is not documented in the manual because it was
4922/// developed to be used in the testbed. If you want to learn how to
4923/// use the mouse joint, look at the testbed.
4924class B2_API b2MouseJoint : public b2Joint {
4925public:
4926
4927 /// Implements b2Joint.
4928 b2Vec2 GetAnchorA() const override;
4929
4930 /// Implements b2Joint.
4931 b2Vec2 GetAnchorB() const override;
4932
4933 /// Implements b2Joint.
4934 b2Vec2 GetReactionForce(float inv_dt) const override;
4935
4936 /// Implements b2Joint.
4937 float GetReactionTorque(float inv_dt) const override;
4938
4939 /// Use this to update the target point.
4940 void SetTarget(const b2Vec2& target);
4941 const b2Vec2& GetTarget() const;
4942
4943 /// Set/get the maximum force in Newtons.
4944 void SetMaxForce(float force);
4945 float GetMaxForce() const;
4946
4947 /// Set/get the linear stiffness in N/m
4948 void SetStiffness(float stiffness) { m_stiffness = stiffness; }
4949 float GetStiffness() const { return m_stiffness; }
4950
4951 /// Set/get linear damping in N*s/m
4952 void SetDamping(float damping) { m_damping = damping; }
4953 float GetDamping() const { return m_damping; }
4954
4955 /// The mouse joint does not support dumping.
4956 void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); }
4957
4958 /// Implement b2Joint::ShiftOrigin
4959 void ShiftOrigin(const b2Vec2& newOrigin) override;
4960
4961protected:
4962 friend class b2Joint;
4963
4964 b2MouseJoint(const b2MouseJointDef* def);
4965
4966 void InitVelocityConstraints(const b2SolverData& data) override;
4967 void SolveVelocityConstraints(const b2SolverData& data) override;
4968 bool SolvePositionConstraints(const b2SolverData& data) override;
4969
4970 b2Vec2 m_localAnchorB;
4971 b2Vec2 m_targetA;
4972 float m_stiffness;
4973 float m_damping;
4974 float m_beta;
4975
4976 // Solver shared
4977 b2Vec2 m_impulse;
4978 float m_maxForce;
4979 float m_gamma;
4980
4981 // Solver temp
4982 int32 m_indexA;
4983 int32 m_indexB;
4984 b2Vec2 m_rB;
4985 b2Vec2 m_localCenterB;
4986 float m_invMassB;
4987 float m_invIB;
4988 b2Mat22 m_mass;
4989 b2Vec2 m_C;
4990};
4991
4992#endif
4993// MIT License
4994
4995// Copyright (c) 2019 Erin Catto
4996
4997// Permission is hereby granted, free of charge, to any person obtaining a copy
4998// of this software and associated documentation files (the "Software"), to deal
4999// in the Software without restriction, including without limitation the rights
5000// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5001// copies of the Software, and to permit persons to whom the Software is
5002// furnished to do so, subject to the following conditions:
5003
5004// The above copyright notice and this permission notice shall be included in all
5005// copies or substantial portions of the Software.
5006
5007// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5008// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5009// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5010// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5011// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5012// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5013// SOFTWARE.
5014#ifndef B2_POLYGON_SHAPE_H
5015#define B2_POLYGON_SHAPE_H
5016
5017//#include "b2_api.h"
5018//#include "b2_shape.h"
5019
5020/// A solid convex polygon. It is assumed that the interior of the polygon is to
5021/// the left of each edge.
5022/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
5023/// In most cases you should not need many vertices for a convex polygon.
5024class B2_API b2PolygonShape : public b2Shape {
5025public:
5026 b2PolygonShape();
5027
5028 /// Implement b2Shape.
5029 b2Shape* Clone(b2BlockAllocator* allocator) const override;
5030
5031 /// @see b2Shape::GetChildCount
5032 int32 GetChildCount() const override;
5033
5034 /// Create a convex hull from the given array of local points.
5035 /// The count must be in the range [3, b2_maxPolygonVertices].
5036 /// @warning the points may be re-ordered, even if they form a convex polygon
5037 /// @warning collinear points are handled but not removed. Collinear points
5038 /// may lead to poor stacking behavior.
5039 void Set(const b2Vec2* points, int32 count);
5040
5041 /// Build vertices to represent an axis-aligned box centered on the local origin.
5042 /// @param hx the half-width.
5043 /// @param hy the half-height.
5044 void SetAsBox(float hx, float hy);
5045
5046 /// Build vertices to represent an oriented box.
5047 /// @param hx the half-width.
5048 /// @param hy the half-height.
5049 /// @param center the center of the box in local coordinates.
5050 /// @param angle the rotation of the box in local coordinates.
5051 void SetAsBox(float hx, float hy, const b2Vec2& center, float angle);
5052
5053 /// @see b2Shape::TestPoint
5054 bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
5055
5056 /// Implement b2Shape.
5057 /// @note because the polygon is solid, rays that start inside do not hit because the normal is
5058 /// not defined.
5059 bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
5060 const b2Transform& transform, int32 childIndex) const override;
5061
5062 /// @see b2Shape::ComputeAABB
5063 void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
5064
5065 /// @see b2Shape::ComputeMass
5066 void ComputeMass(b2MassData* massData, float density) const override;
5067
5068 /// Validate convexity. This is a very time consuming operation.
5069 /// @returns true if valid
5070 bool Validate() const;
5071
5072 b2Vec2 m_centroid;
5073 b2Vec2 m_vertices[b2_maxPolygonVertices];
5074 b2Vec2 m_normals[b2_maxPolygonVertices];
5075 int32 m_count;
5076};
5077
5078inline b2PolygonShape::b2PolygonShape() {
5079 m_type = e_polygon;
5080 m_radius = b2_polygonRadius;
5081 m_count = 0;
5082 m_centroid.SetZero();
5083}
5084
5085#endif
5086// MIT License
5087
5088// Copyright (c) 2019 Erin Catto
5089
5090// Permission is hereby granted, free of charge, to any person obtaining a copy
5091// of this software and associated documentation files (the "Software"), to deal
5092// in the Software without restriction, including without limitation the rights
5093// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5094// copies of the Software, and to permit persons to whom the Software is
5095// furnished to do so, subject to the following conditions:
5096
5097// The above copyright notice and this permission notice shall be included in all
5098// copies or substantial portions of the Software.
5099
5100// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5101// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5102// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5103// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5104// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5105// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5106// SOFTWARE.
5107
5108#ifndef B2_PRISMATIC_JOINT_H
5109#define B2_PRISMATIC_JOINT_H
5110
5111//#include "b2_api.h"
5112//#include "b2_joint.h"
5113
5114/// Prismatic joint definition. This requires defining a line of
5115/// motion using an axis and an anchor point. The definition uses local
5116/// anchor points and a local axis so that the initial configuration
5117/// can violate the constraint slightly. The joint translation is zero
5118/// when the local anchor points coincide in world space. Using local
5119/// anchors and a local axis helps when saving and loading a game.
5120struct B2_API b2PrismaticJointDef : public b2JointDef {
5121 b2PrismaticJointDef() {
5122 type = e_prismaticJoint;
5123 localAnchorA.SetZero();
5124 localAnchorB.SetZero();
5125 localAxisA.Set(1.0f, 0.0f);
5126 referenceAngle = 0.0f;
5127 enableLimit = false;
5128 lowerTranslation = 0.0f;
5129 upperTranslation = 0.0f;
5130 enableMotor = false;
5131 maxMotorForce = 0.0f;
5132 motorSpeed = 0.0f;
5133 }
5134
5135 /// Initialize the bodies, anchors, axis, and reference angle using the world
5136 /// anchor and unit world axis.
5137 void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
5138
5139 /// The local anchor point relative to bodyA's origin.
5140 b2Vec2 localAnchorA;
5141
5142 /// The local anchor point relative to bodyB's origin.
5143 b2Vec2 localAnchorB;
5144
5145 /// The local translation unit axis in bodyA.
5146 b2Vec2 localAxisA;
5147
5148 /// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
5149 float referenceAngle;
5150
5151 /// Enable/disable the joint limit.
5152 bool enableLimit;
5153
5154 /// The lower translation limit, usually in meters.
5155 float lowerTranslation;
5156
5157 /// The upper translation limit, usually in meters.
5158 float upperTranslation;
5159
5160 /// Enable/disable the joint motor.
5161 bool enableMotor;
5162
5163 /// The maximum motor torque, usually in N-m.
5164 float maxMotorForce;
5165
5166 /// The desired motor speed in radians per second.
5167 float motorSpeed;
5168};
5169
5170/// A prismatic joint. This joint provides one degree of freedom: translation
5171/// along an axis fixed in bodyA. Relative rotation is prevented. You can
5172/// use a joint limit to restrict the range of motion and a joint motor to
5173/// drive the motion or to model joint friction.
5174class B2_API b2PrismaticJoint : public b2Joint {
5175public:
5176 b2Vec2 GetAnchorA() const override;
5177 b2Vec2 GetAnchorB() const override;
5178
5179 b2Vec2 GetReactionForce(float inv_dt) const override;
5180 float GetReactionTorque(float inv_dt) const override;
5181
5182 /// The local anchor point relative to bodyA's origin.
5183 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
5184
5185 /// The local anchor point relative to bodyB's origin.
5186 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
5187
5188 /// The local joint axis relative to bodyA.
5189 const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
5190
5191 /// Get the reference angle.
5192 float GetReferenceAngle() const { return m_referenceAngle; }
5193
5194 /// Get the current joint translation, usually in meters.
5195 float GetJointTranslation() const;
5196
5197 /// Get the current joint translation speed, usually in meters per second.
5198 float GetJointSpeed() const;
5199
5200 /// Is the joint limit enabled?
5201 bool IsLimitEnabled() const;
5202
5203 /// Enable/disable the joint limit.
5204 void EnableLimit(bool flag);
5205
5206 /// Get the lower joint limit, usually in meters.
5207 float GetLowerLimit() const;
5208
5209 /// Get the upper joint limit, usually in meters.
5210 float GetUpperLimit() const;
5211
5212 /// Set the joint limits, usually in meters.
5213 void SetLimits(float lower, float upper);
5214
5215 /// Is the joint motor enabled?
5216 bool IsMotorEnabled() const;
5217
5218 /// Enable/disable the joint motor.
5219 void EnableMotor(bool flag);
5220
5221 /// Set the motor speed, usually in meters per second.
5222 void SetMotorSpeed(float speed);
5223
5224 /// Get the motor speed, usually in meters per second.
5225 float GetMotorSpeed() const;
5226
5227 /// Set the maximum motor force, usually in N.
5228 void SetMaxMotorForce(float force);
5229 float GetMaxMotorForce() const { return m_maxMotorForce; }
5230
5231 /// Get the current motor force given the inverse time step, usually in N.
5232 float GetMotorForce(float inv_dt) const;
5233
5234 /// Dump to b2Log
5235 void Dump() override;
5236
5237 ///
5238 void Draw(b2Draw* draw) const override;
5239
5240protected:
5241 friend class b2Joint;
5242 friend class b2GearJoint;
5243 b2PrismaticJoint(const b2PrismaticJointDef* def);
5244
5245 void InitVelocityConstraints(const b2SolverData& data) override;
5246 void SolveVelocityConstraints(const b2SolverData& data) override;
5247 bool SolvePositionConstraints(const b2SolverData& data) override;
5248
5249 b2Vec2 m_localAnchorA;
5250 b2Vec2 m_localAnchorB;
5251 b2Vec2 m_localXAxisA;
5252 b2Vec2 m_localYAxisA;
5253 float m_referenceAngle;
5254 b2Vec2 m_impulse;
5255 float m_motorImpulse;
5256 float m_lowerImpulse;
5257 float m_upperImpulse;
5258 float m_lowerTranslation;
5259 float m_upperTranslation;
5260 float m_maxMotorForce;
5261 float m_motorSpeed;
5262 bool m_enableLimit;
5263 bool m_enableMotor;
5264
5265 // Solver temp
5266 int32 m_indexA;
5267 int32 m_indexB;
5268 b2Vec2 m_localCenterA;
5269 b2Vec2 m_localCenterB;
5270 float m_invMassA;
5271 float m_invMassB;
5272 float m_invIA;
5273 float m_invIB;
5274 b2Vec2 m_axis, m_perp;
5275 float m_s1, m_s2;
5276 float m_a1, m_a2;
5277 b2Mat22 m_K;
5278 float m_translation;
5279 float m_axialMass;
5280};
5281
5282inline float b2PrismaticJoint::GetMotorSpeed() const {
5283 return m_motorSpeed;
5284}
5285
5286#endif
5287// MIT License
5288
5289// Copyright (c) 2019 Erin Catto
5290
5291// Permission is hereby granted, free of charge, to any person obtaining a copy
5292// of this software and associated documentation files (the "Software"), to deal
5293// in the Software without restriction, including without limitation the rights
5294// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5295// copies of the Software, and to permit persons to whom the Software is
5296// furnished to do so, subject to the following conditions:
5297
5298// The above copyright notice and this permission notice shall be included in all
5299// copies or substantial portions of the Software.
5300
5301// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5302// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5303// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5304// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5305// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5306// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5307// SOFTWARE.
5308
5309#ifndef B2_PULLEY_JOINT_H
5310#define B2_PULLEY_JOINT_H
5311
5312//#include "b2_api.h"
5313//#include "b2_joint.h"
5314
5315const float b2_minPulleyLength = 2.0f;
5316
5317/// Pulley joint definition. This requires two ground anchors,
5318/// two dynamic body anchor points, and a pulley ratio.
5319struct B2_API b2PulleyJointDef : public b2JointDef {
5320 b2PulleyJointDef() {
5321 type = e_pulleyJoint;
5322 groundAnchorA.Set(-1.0f, 1.0f);
5323 groundAnchorB.Set(1.0f, 1.0f);
5324 localAnchorA.Set(-1.0f, 0.0f);
5325 localAnchorB.Set(1.0f, 0.0f);
5326 lengthA = 0.0f;
5327 lengthB = 0.0f;
5328 ratio = 1.0f;
5329 collideConnected = true;
5330 }
5331
5332 /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
5333 void Initialize(b2Body* bodyA, b2Body* bodyB,
5334 const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,
5335 const b2Vec2& anchorA, const b2Vec2& anchorB,
5336 float ratio);
5337
5338 /// The first ground anchor in world coordinates. This point never moves.
5339 b2Vec2 groundAnchorA;
5340
5341 /// The second ground anchor in world coordinates. This point never moves.
5342 b2Vec2 groundAnchorB;
5343
5344 /// The local anchor point relative to bodyA's origin.
5345 b2Vec2 localAnchorA;
5346
5347 /// The local anchor point relative to bodyB's origin.
5348 b2Vec2 localAnchorB;
5349
5350 /// The a reference length for the segment attached to bodyA.
5351 float lengthA;
5352
5353 /// The a reference length for the segment attached to bodyB.
5354 float lengthB;
5355
5356 /// The pulley ratio, used to simulate a block-and-tackle.
5357 float ratio;
5358};
5359
5360/// The pulley joint is connected to two bodies and two fixed ground points.
5361/// The pulley supports a ratio such that:
5362/// length1 + ratio * length2 <= constant
5363/// Yes, the force transmitted is scaled by the ratio.
5364/// Warning: the pulley joint can get a bit squirrelly by itself. They often
5365/// work better when combined with prismatic joints. You should also cover the
5366/// the anchor points with static shapes to prevent one side from going to
5367/// zero length.
5368class B2_API b2PulleyJoint : public b2Joint {
5369public:
5370 b2Vec2 GetAnchorA() const override;
5371 b2Vec2 GetAnchorB() const override;
5372
5373 b2Vec2 GetReactionForce(float inv_dt) const override;
5374 float GetReactionTorque(float inv_dt) const override;
5375
5376 /// Get the first ground anchor.
5377 b2Vec2 GetGroundAnchorA() const;
5378
5379 /// Get the second ground anchor.
5380 b2Vec2 GetGroundAnchorB() const;
5381
5382 /// Get the current length of the segment attached to bodyA.
5383 float GetLengthA() const;
5384
5385 /// Get the current length of the segment attached to bodyB.
5386 float GetLengthB() const;
5387
5388 /// Get the pulley ratio.
5389 float GetRatio() const;
5390
5391 /// Get the current length of the segment attached to bodyA.
5392 float GetCurrentLengthA() const;
5393
5394 /// Get the current length of the segment attached to bodyB.
5395 float GetCurrentLengthB() const;
5396
5397 /// Dump joint to dmLog
5398 void Dump() override;
5399
5400 /// Implement b2Joint::ShiftOrigin
5401 void ShiftOrigin(const b2Vec2& newOrigin) override;
5402
5403protected:
5404
5405 friend class b2Joint;
5406 b2PulleyJoint(const b2PulleyJointDef* data);
5407
5408 void InitVelocityConstraints(const b2SolverData& data) override;
5409 void SolveVelocityConstraints(const b2SolverData& data) override;
5410 bool SolvePositionConstraints(const b2SolverData& data) override;
5411
5412 b2Vec2 m_groundAnchorA;
5413 b2Vec2 m_groundAnchorB;
5414 float m_lengthA;
5415 float m_lengthB;
5416
5417 // Solver shared
5418 b2Vec2 m_localAnchorA;
5419 b2Vec2 m_localAnchorB;
5420 float m_constant;
5421 float m_ratio;
5422 float m_impulse;
5423
5424 // Solver temp
5425 int32 m_indexA;
5426 int32 m_indexB;
5427 b2Vec2 m_uA;
5428 b2Vec2 m_uB;
5429 b2Vec2 m_rA;
5430 b2Vec2 m_rB;
5431 b2Vec2 m_localCenterA;
5432 b2Vec2 m_localCenterB;
5433 float m_invMassA;
5434 float m_invMassB;
5435 float m_invIA;
5436 float m_invIB;
5437 float m_mass;
5438};
5439
5440#endif
5441// MIT License
5442
5443// Copyright (c) 2019 Erin Catto
5444
5445// Permission is hereby granted, free of charge, to any person obtaining a copy
5446// of this software and associated documentation files (the "Software"), to deal
5447// in the Software without restriction, including without limitation the rights
5448// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5449// copies of the Software, and to permit persons to whom the Software is
5450// furnished to do so, subject to the following conditions:
5451
5452// The above copyright notice and this permission notice shall be included in all
5453// copies or substantial portions of the Software.
5454
5455// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5456// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5457// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5458// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5459// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5460// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5461// SOFTWARE.
5462
5463#ifndef B2_REVOLUTE_JOINT_H
5464#define B2_REVOLUTE_JOINT_H
5465
5466//#include "b2_api.h"
5467//#include "b2_joint.h"
5468
5469/// Revolute joint definition. This requires defining an anchor point where the
5470/// bodies are joined. The definition uses local anchor points so that the
5471/// initial configuration can violate the constraint slightly. You also need to
5472/// specify the initial relative angle for joint limits. This helps when saving
5473/// and loading a game.
5474/// The local anchor points are measured from the body's origin
5475/// rather than the center of mass because:
5476/// 1. you might not know where the center of mass will be.
5477/// 2. if you add/remove shapes from a body and recompute the mass,
5478/// the joints will be broken.
5479struct B2_API b2RevoluteJointDef : public b2JointDef {
5480 b2RevoluteJointDef() {
5481 type = e_revoluteJoint;
5482 localAnchorA.Set(0.0f, 0.0f);
5483 localAnchorB.Set(0.0f, 0.0f);
5484 referenceAngle = 0.0f;
5485 lowerAngle = 0.0f;
5486 upperAngle = 0.0f;
5487 maxMotorTorque = 0.0f;
5488 motorSpeed = 0.0f;
5489 enableLimit = false;
5490 enableMotor = false;
5491 }
5492
5493 /// Initialize the bodies, anchors, and reference angle using a world
5494 /// anchor point.
5495 void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
5496
5497 /// The local anchor point relative to bodyA's origin.
5498 b2Vec2 localAnchorA;
5499
5500 /// The local anchor point relative to bodyB's origin.
5501 b2Vec2 localAnchorB;
5502
5503 /// The bodyB angle minus bodyA angle in the reference state (radians).
5504 float referenceAngle;
5505
5506 /// A flag to enable joint limits.
5507 bool enableLimit;
5508
5509 /// The lower angle for the joint limit (radians).
5510 float lowerAngle;
5511
5512 /// The upper angle for the joint limit (radians).
5513 float upperAngle;
5514
5515 /// A flag to enable the joint motor.
5516 bool enableMotor;
5517
5518 /// The desired motor speed. Usually in radians per second.
5519 float motorSpeed;
5520
5521 /// The maximum motor torque used to achieve the desired motor speed.
5522 /// Usually in N-m.
5523 float maxMotorTorque;
5524};
5525
5526/// A revolute joint constrains two bodies to share a common point while they
5527/// are free to rotate about the point. The relative rotation about the shared
5528/// point is the joint angle. You can limit the relative rotation with
5529/// a joint limit that specifies a lower and upper angle. You can use a motor
5530/// to drive the relative rotation about the shared point. A maximum motor torque
5531/// is provided so that infinite forces are not generated.
5532class B2_API b2RevoluteJoint : public b2Joint {
5533public:
5534 b2Vec2 GetAnchorA() const override;
5535 b2Vec2 GetAnchorB() const override;
5536
5537 /// The local anchor point relative to bodyA's origin.
5538 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
5539
5540 /// The local anchor point relative to bodyB's origin.
5541 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
5542
5543 /// Get the reference angle.
5544 float GetReferenceAngle() const { return m_referenceAngle; }
5545
5546 /// Get the current joint angle in radians.
5547 float GetJointAngle() const;
5548
5549 /// Get the current joint angle speed in radians per second.
5550 float GetJointSpeed() const;
5551
5552 /// Is the joint limit enabled?
5553 bool IsLimitEnabled() const;
5554
5555 /// Enable/disable the joint limit.
5556 void EnableLimit(bool flag);
5557
5558 /// Get the lower joint limit in radians.
5559 float GetLowerLimit() const;
5560
5561 /// Get the upper joint limit in radians.
5562 float GetUpperLimit() const;
5563
5564 /// Set the joint limits in radians.
5565 void SetLimits(float lower, float upper);
5566
5567 /// Is the joint motor enabled?
5568 bool IsMotorEnabled() const;
5569
5570 /// Enable/disable the joint motor.
5571 void EnableMotor(bool flag);
5572
5573 /// Set the motor speed in radians per second.
5574 void SetMotorSpeed(float speed);
5575
5576 /// Get the motor speed in radians per second.
5577 float GetMotorSpeed() const;
5578
5579 /// Set the maximum motor torque, usually in N-m.
5580 void SetMaxMotorTorque(float torque);
5581 float GetMaxMotorTorque() const { return m_maxMotorTorque; }
5582
5583 /// Get the reaction force given the inverse time step.
5584 /// Unit is N.
5585 b2Vec2 GetReactionForce(float inv_dt) const override;
5586
5587 /// Get the reaction torque due to the joint limit given the inverse time step.
5588 /// Unit is N*m.
5589 float GetReactionTorque(float inv_dt) const override;
5590
5591 /// Get the current motor torque given the inverse time step.
5592 /// Unit is N*m.
5593 float GetMotorTorque(float inv_dt) const;
5594
5595 /// Dump to b2Log.
5596 void Dump() override;
5597
5598 ///
5599 void Draw(b2Draw* draw) const override;
5600
5601protected:
5602
5603 friend class b2Joint;
5604 friend class b2GearJoint;
5605
5606 b2RevoluteJoint(const b2RevoluteJointDef* def);
5607
5608 void InitVelocityConstraints(const b2SolverData& data) override;
5609 void SolveVelocityConstraints(const b2SolverData& data) override;
5610 bool SolvePositionConstraints(const b2SolverData& data) override;
5611
5612 // Solver shared
5613 b2Vec2 m_localAnchorA;
5614 b2Vec2 m_localAnchorB;
5615 b2Vec2 m_impulse;
5616 float m_motorImpulse;
5617 float m_lowerImpulse;
5618 float m_upperImpulse;
5619 bool m_enableMotor;
5620 float m_maxMotorTorque;
5621 float m_motorSpeed;
5622 bool m_enableLimit;
5623 float m_referenceAngle;
5624 float m_lowerAngle;
5625 float m_upperAngle;
5626
5627 // Solver temp
5628 int32 m_indexA;
5629 int32 m_indexB;
5630 b2Vec2 m_rA;
5631 b2Vec2 m_rB;
5632 b2Vec2 m_localCenterA;
5633 b2Vec2 m_localCenterB;
5634 float m_invMassA;
5635 float m_invMassB;
5636 float m_invIA;
5637 float m_invIB;
5638 b2Mat22 m_K;
5639 float m_angle;
5640 float m_axialMass;
5641};
5642
5643inline float b2RevoluteJoint::GetMotorSpeed() const {
5644 return m_motorSpeed;
5645}
5646
5647#endif
5648// MIT License
5649
5650// Copyright (c) 2019 Erin Catto
5651
5652// Permission is hereby granted, free of charge, to any person obtaining a copy
5653// of this software and associated documentation files (the "Software"), to deal
5654// in the Software without restriction, including without limitation the rights
5655// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5656// copies of the Software, and to permit persons to whom the Software is
5657// furnished to do so, subject to the following conditions:
5658
5659// The above copyright notice and this permission notice shall be included in all
5660// copies or substantial portions of the Software.
5661
5662// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5663// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5664// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5665// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5666// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5667// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5668// SOFTWARE.
5669
5670#ifndef B2_ROPE_H
5671#define B2_ROPE_H
5672
5673//#include "b2_api.h"
5674//#include "b2_math.h"
5675
5676class b2Draw;
5677struct b2RopeStretch;
5678struct b2RopeBend;
5679
5680enum b2StretchingModel {
5681 b2_pbdStretchingModel,
5682 b2_xpbdStretchingModel
5683};
5684
5685enum b2BendingModel {
5686 b2_springAngleBendingModel = 0,
5687 b2_pbdAngleBendingModel,
5688 b2_xpbdAngleBendingModel,
5689 b2_pbdDistanceBendingModel,
5690 b2_pbdHeightBendingModel,
5691 b2_pbdTriangleBendingModel
5692};
5693
5694///
5695struct B2_API b2RopeTuning {
5696 b2RopeTuning() {
5697 stretchingModel = b2_pbdStretchingModel;
5698 bendingModel = b2_pbdAngleBendingModel;
5699 damping = 0.0f;
5700 stretchStiffness = 1.0f;
5701 bendStiffness = 0.5f;
5702 bendHertz = 1.0f;
5703 bendDamping = 0.0f;
5704 isometric = false;
5705 fixedEffectiveMass = false;
5706 warmStart = false;
5707 }
5708
5709 b2StretchingModel stretchingModel;
5710 b2BendingModel bendingModel;
5711 float damping;
5712 float stretchStiffness;
5713 float stretchHertz;
5714 float stretchDamping;
5715 float bendStiffness;
5716 float bendHertz;
5717 float bendDamping;
5718 bool isometric;
5719 bool fixedEffectiveMass;
5720 bool warmStart;
5721};
5722
5723///
5724struct B2_API b2RopeDef {
5725 b2RopeDef() {
5726 position.SetZero();
5727 vertices = nullptr;
5728 count = 0;
5729 masses = nullptr;
5730 gravity.SetZero();
5731 }
5732
5733 b2Vec2 position;
5734 b2Vec2* vertices;
5735 int32 count;
5736 float* masses;
5737 b2Vec2 gravity;
5738 b2RopeTuning tuning;
5739};
5740
5741///
5742class B2_API b2Rope {
5743public:
5744 b2Rope();
5745 ~b2Rope();
5746
5747 ///
5748 void Create(const b2RopeDef& def);
5749
5750 ///
5751 void SetTuning(const b2RopeTuning& tuning);
5752
5753 ///
5754 void Step(float timeStep, int32 iterations, const b2Vec2& position);
5755
5756 ///
5757 void Reset(const b2Vec2& position);
5758
5759 ///
5760 void Draw(b2Draw* draw) const;
5761
5762private:
5763
5764 void SolveStretch_PBD();
5765 void SolveStretch_XPBD(float dt);
5766 void SolveBend_PBD_Angle();
5767 void SolveBend_XPBD_Angle(float dt);
5768 void SolveBend_PBD_Distance();
5769 void SolveBend_PBD_Height();
5770 void SolveBend_PBD_Triangle();
5771 void ApplyBendForces(float dt);
5772
5773 b2Vec2 m_position;
5774
5775 int32 m_count;
5776 int32 m_stretchCount;
5777 int32 m_bendCount;
5778
5779 b2RopeStretch* m_stretchConstraints;
5780 b2RopeBend* m_bendConstraints;
5781
5782 b2Vec2* m_bindPositions;
5783 b2Vec2* m_ps;
5784 b2Vec2* m_p0s;
5785 b2Vec2* m_vs;
5786
5787 float* m_invMasses;
5788 b2Vec2 m_gravity;
5789
5790 b2RopeTuning m_tuning;
5791};
5792
5793#endif
5794// MIT License
5795
5796// Copyright (c) 2019 Erin Catto
5797
5798// Permission is hereby granted, free of charge, to any person obtaining a copy
5799// of this software and associated documentation files (the "Software"), to deal
5800// in the Software without restriction, including without limitation the rights
5801// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5802// copies of the Software, and to permit persons to whom the Software is
5803// furnished to do so, subject to the following conditions:
5804
5805// The above copyright notice and this permission notice shall be included in all
5806// copies or substantial portions of the Software.
5807
5808// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5809// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5810// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5811// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5812// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5813// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5814// SOFTWARE.
5815
5816#ifndef B2_TIMER_H
5817#define B2_TIMER_H
5818
5819//#include "b2_api.h"
5820//#include "b2_settings.h"
5821
5822/// Timer for profiling. This has platform specific code and may
5823/// not work on every platform.
5824class B2_API b2Timer {
5825public:
5826
5827 /// Constructor
5828 b2Timer();
5829
5830 /// Reset the timer.
5831 void Reset();
5832
5833 /// Get the time since construction or the last reset.
5834 float GetMilliseconds() const;
5835
5836private:
5837
5838#if defined(_WIN32)
5839 double m_start;
5840 static double s_invFrequency;
5841#elif defined(__linux__) || defined (__APPLE__)
5842 unsigned long long m_start_sec;
5843 unsigned long long m_start_usec;
5844#endif
5845};
5846
5847#endif
5848// MIT License
5849
5850// Copyright (c) 2019 Erin Catto
5851
5852// Permission is hereby granted, free of charge, to any person obtaining a copy
5853// of this software and associated documentation files (the "Software"), to deal
5854// in the Software without restriction, including without limitation the rights
5855// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5856// copies of the Software, and to permit persons to whom the Software is
5857// furnished to do so, subject to the following conditions:
5858
5859// The above copyright notice and this permission notice shall be included in all
5860// copies or substantial portions of the Software.
5861
5862// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5863// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5864// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5865// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5866// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5867// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5868// SOFTWARE.
5869
5870#ifndef B2_TIME_OF_IMPACT_H
5871#define B2_TIME_OF_IMPACT_H
5872
5873//#include "b2_api.h"
5874//#include "b2_math.h"
5875//#include "b2_distance.h"
5876
5877/// Input parameters for b2TimeOfImpact
5878struct B2_API b2TOIInput {
5879 b2DistanceProxy proxyA;
5880 b2DistanceProxy proxyB;
5881 b2Sweep sweepA;
5882 b2Sweep sweepB;
5883 float tMax; // defines sweep interval [0, tMax]
5884};
5885
5886/// Output parameters for b2TimeOfImpact.
5887struct B2_API b2TOIOutput {
5888 enum State {
5889 e_unknown,
5890 e_failed,
5891 e_overlapped,
5892 e_touching,
5893 e_separated
5894 };
5895
5896 State state;
5897 float t;
5898};
5899
5900/// Compute the upper bound on time before two shapes penetrate. Time is represented as
5901/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
5902/// non-tunneling collisions. If you change the time interval, you should call this function
5903/// again.
5904/// Note: use b2Distance to compute the contact point and normal at the time of impact.
5905B2_API void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
5906
5907#endif
5908// MIT License
5909
5910// Copyright (c) 2019 Erin Catto
5911
5912// Permission is hereby granted, free of charge, to any person obtaining a copy
5913// of this software and associated documentation files (the "Software"), to deal
5914// in the Software without restriction, including without limitation the rights
5915// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5916// copies of the Software, and to permit persons to whom the Software is
5917// furnished to do so, subject to the following conditions:
5918
5919// The above copyright notice and this permission notice shall be included in all
5920// copies or substantial portions of the Software.
5921
5922// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5923// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5924// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5925// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5926// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5927// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5928// SOFTWARE.
5929#ifndef B2_TIME_STEP_H
5930#define B2_TIME_STEP_H
5931
5932//#include "b2_api.h"
5933//#include "b2_math.h"
5934
5935/// Profiling data. Times are in milliseconds.
5936struct B2_API b2Profile {
5937 float step;
5938 float collide;
5939 float solve;
5940 float solveInit;
5941 float solveVelocity;
5942 float solvePosition;
5943 float broadphase;
5944 float solveTOI;
5945};
5946
5947/// This is an internal structure.
5948struct B2_API b2TimeStep {
5949 float dt; // time step
5950 float inv_dt; // inverse time step (0 if dt == 0).
5951 float dtRatio; // dt * inv_dt0
5952 int32 velocityIterations;
5953 int32 positionIterations;
5954 bool warmStarting;
5955};
5956
5957/// This is an internal structure.
5958struct B2_API b2Position {
5959 b2Vec2 c;
5960 float a;
5961};
5962
5963/// This is an internal structure.
5964struct B2_API b2Velocity {
5965 b2Vec2 v;
5966 float w;
5967};
5968
5969/// Solver Data
5970struct B2_API b2SolverData {
5971 b2TimeStep step;
5972 b2Position* positions;
5973 b2Velocity* velocities;
5974};
5975
5976#endif
5977// MIT License
5978
5979// Copyright (c) 2019 Erin Catto
5980
5981// Permission is hereby granted, free of charge, to any person obtaining a copy
5982// of this software and associated documentation files (the "Software"), to deal
5983// in the Software without restriction, including without limitation the rights
5984// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5985// copies of the Software, and to permit persons to whom the Software is
5986// furnished to do so, subject to the following conditions:
5987
5988// The above copyright notice and this permission notice shall be included in all
5989// copies or substantial portions of the Software.
5990
5991// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5992// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
5993// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
5994// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
5995// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
5996// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
5997// SOFTWARE.
5998
5999#ifndef B2_WELD_JOINT_H
6000#define B2_WELD_JOINT_H
6001
6002//#include "b2_api.h"
6003//#include "b2_joint.h"
6004
6005/// Weld joint definition. You need to specify local anchor points
6006/// where they are attached and the relative body angle. The position
6007/// of the anchor points is important for computing the reaction torque.
6008struct B2_API b2WeldJointDef : public b2JointDef {
6009 b2WeldJointDef() {
6010 type = e_weldJoint;
6011 localAnchorA.Set(0.0f, 0.0f);
6012 localAnchorB.Set(0.0f, 0.0f);
6013 referenceAngle = 0.0f;
6014 stiffness = 0.0f;
6015 damping = 0.0f;
6016 }
6017
6018 /// Initialize the bodies, anchors, reference angle, stiffness, and damping.
6019 /// @param bodyA the first body connected by this joint
6020 /// @param bodyB the second body connected by this joint
6021 /// @param anchor the point of connection in world coordinates
6022 void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
6023
6024 /// The local anchor point relative to bodyA's origin.
6025 b2Vec2 localAnchorA;
6026
6027 /// The local anchor point relative to bodyB's origin.
6028 b2Vec2 localAnchorB;
6029
6030 /// The bodyB angle minus bodyA angle in the reference state (radians).
6031 float referenceAngle;
6032
6033 /// The rotational stiffness in N*m
6034 /// Disable softness with a value of 0
6035 float stiffness;
6036
6037 /// The rotational damping in N*m*s
6038 float damping;
6039};
6040
6041/// A weld joint essentially glues two bodies together. A weld joint may
6042/// distort somewhat because the island constraint solver is approximate.
6043class B2_API b2WeldJoint : public b2Joint {
6044public:
6045 b2Vec2 GetAnchorA() const override;
6046 b2Vec2 GetAnchorB() const override;
6047
6048 b2Vec2 GetReactionForce(float inv_dt) const override;
6049 float GetReactionTorque(float inv_dt) const override;
6050
6051 /// The local anchor point relative to bodyA's origin.
6052 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
6053
6054 /// The local anchor point relative to bodyB's origin.
6055 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
6056
6057 /// Get the reference angle.
6058 float GetReferenceAngle() const { return m_referenceAngle; }
6059
6060 /// Set/get stiffness in N*m
6061 void SetStiffness(float hz) { m_stiffness = hz; }
6062 float GetStiffness() const { return m_stiffness; }
6063
6064 /// Set/get damping in N*m*s
6065 void SetDamping(float damping) { m_damping = damping; }
6066 float GetDamping() const { return m_damping; }
6067
6068 /// Dump to b2Log
6069 void Dump() override;
6070
6071protected:
6072
6073 friend class b2Joint;
6074
6075 b2WeldJoint(const b2WeldJointDef* def);
6076
6077 void InitVelocityConstraints(const b2SolverData& data) override;
6078 void SolveVelocityConstraints(const b2SolverData& data) override;
6079 bool SolvePositionConstraints(const b2SolverData& data) override;
6080
6081 float m_stiffness;
6082 float m_damping;
6083 float m_bias;
6084
6085 // Solver shared
6086 b2Vec2 m_localAnchorA;
6087 b2Vec2 m_localAnchorB;
6088 float m_referenceAngle;
6089 float m_gamma;
6090 b2Vec3 m_impulse;
6091
6092 // Solver temp
6093 int32 m_indexA;
6094 int32 m_indexB;
6095 b2Vec2 m_rA;
6096 b2Vec2 m_rB;
6097 b2Vec2 m_localCenterA;
6098 b2Vec2 m_localCenterB;
6099 float m_invMassA;
6100 float m_invMassB;
6101 float m_invIA;
6102 float m_invIB;
6103 b2Mat33 m_mass;
6104};
6105
6106#endif
6107// MIT License
6108
6109// Copyright (c) 2019 Erin Catto
6110
6111// Permission is hereby granted, free of charge, to any person obtaining a copy
6112// of this software and associated documentation files (the "Software"), to deal
6113// in the Software without restriction, including without limitation the rights
6114// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6115// copies of the Software, and to permit persons to whom the Software is
6116// furnished to do so, subject to the following conditions:
6117
6118// The above copyright notice and this permission notice shall be included in all
6119// copies or substantial portions of the Software.
6120
6121// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6122// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6123// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6124// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6125// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6126// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6127// SOFTWARE.
6128
6129#ifndef B2_WHEEL_JOINT_H
6130#define B2_WHEEL_JOINT_H
6131
6132//#include "b2_api.h"
6133//#include "b2_joint.h"
6134
6135/// Wheel joint definition. This requires defining a line of
6136/// motion using an axis and an anchor point. The definition uses local
6137/// anchor points and a local axis so that the initial configuration
6138/// can violate the constraint slightly. The joint translation is zero
6139/// when the local anchor points coincide in world space. Using local
6140/// anchors and a local axis helps when saving and loading a game.
6141struct B2_API b2WheelJointDef : public b2JointDef {
6142 b2WheelJointDef() {
6143 type = e_wheelJoint;
6144 localAnchorA.SetZero();
6145 localAnchorB.SetZero();
6146 localAxisA.Set(1.0f, 0.0f);
6147 enableLimit = false;
6148 lowerTranslation = 0.0f;
6149 upperTranslation = 0.0f;
6150 enableMotor = false;
6151 maxMotorTorque = 0.0f;
6152 motorSpeed = 0.0f;
6153 stiffness = 0.0f;
6154 damping = 0.0f;
6155 }
6156
6157 /// Initialize the bodies, anchors, axis, and reference angle using the world
6158 /// anchor and world axis.
6159 void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
6160
6161 /// The local anchor point relative to bodyA's origin.
6162 b2Vec2 localAnchorA;
6163
6164 /// The local anchor point relative to bodyB's origin.
6165 b2Vec2 localAnchorB;
6166
6167 /// The local translation axis in bodyA.
6168 b2Vec2 localAxisA;
6169
6170 /// Enable/disable the joint limit.
6171 bool enableLimit;
6172
6173 /// The lower translation limit, usually in meters.
6174 float lowerTranslation;
6175
6176 /// The upper translation limit, usually in meters.
6177 float upperTranslation;
6178
6179 /// Enable/disable the joint motor.
6180 bool enableMotor;
6181
6182 /// The maximum motor torque, usually in N-m.
6183 float maxMotorTorque;
6184
6185 /// The desired motor speed in radians per second.
6186 float motorSpeed;
6187
6188 /// Suspension stiffness. Typically in units N/m.
6189 float stiffness;
6190
6191 /// Suspension damping. Typically in units of N*s/m.
6192 float damping;
6193};
6194
6195/// A wheel joint. This joint provides two degrees of freedom: translation
6196/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to
6197/// line constraint with a rotational motor and a linear spring/damper. The spring/damper is
6198/// initialized upon creation. This joint is designed for vehicle suspensions.
6199class B2_API b2WheelJoint : public b2Joint {
6200public:
6201 b2Vec2 GetAnchorA() const override;
6202 b2Vec2 GetAnchorB() const override;
6203
6204 b2Vec2 GetReactionForce(float inv_dt) const override;
6205 float GetReactionTorque(float inv_dt) const override;
6206
6207 /// The local anchor point relative to bodyA's origin.
6208 const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
6209
6210 /// The local anchor point relative to bodyB's origin.
6211 const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
6212
6213 /// The local joint axis relative to bodyA.
6214 const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
6215
6216 /// Get the current joint translation, usually in meters.
6217 float GetJointTranslation() const;
6218
6219 /// Get the current joint linear speed, usually in meters per second.
6220 float GetJointLinearSpeed() const;
6221
6222 /// Get the current joint angle in radians.
6223 float GetJointAngle() const;
6224
6225 /// Get the current joint angular speed in radians per second.
6226 float GetJointAngularSpeed() const;
6227
6228 /// Is the joint limit enabled?
6229 bool IsLimitEnabled() const;
6230
6231 /// Enable/disable the joint translation limit.
6232 void EnableLimit(bool flag);
6233
6234 /// Get the lower joint translation limit, usually in meters.
6235 float GetLowerLimit() const;
6236
6237 /// Get the upper joint translation limit, usually in meters.
6238 float GetUpperLimit() const;
6239
6240 /// Set the joint translation limits, usually in meters.
6241 void SetLimits(float lower, float upper);
6242
6243 /// Is the joint motor enabled?
6244 bool IsMotorEnabled() const;
6245
6246 /// Enable/disable the joint motor.
6247 void EnableMotor(bool flag);
6248
6249 /// Set the motor speed, usually in radians per second.
6250 void SetMotorSpeed(float speed);
6251
6252 /// Get the motor speed, usually in radians per second.
6253 float GetMotorSpeed() const;
6254
6255 /// Set/Get the maximum motor force, usually in N-m.
6256 void SetMaxMotorTorque(float torque);
6257 float GetMaxMotorTorque() const;
6258
6259 /// Get the current motor torque given the inverse time step, usually in N-m.
6260 float GetMotorTorque(float inv_dt) const;
6261
6262 /// Access spring stiffness
6263 void SetStiffness(float stiffness);
6264 float GetStiffness() const;
6265
6266 /// Access damping
6267 void SetDamping(float damping);
6268 float GetDamping() const;
6269
6270 /// Dump to b2Log
6271 void Dump() override;
6272
6273 ///
6274 void Draw(b2Draw* draw) const override;
6275
6276protected:
6277
6278 friend class b2Joint;
6279 b2WheelJoint(const b2WheelJointDef* def);
6280
6281 void InitVelocityConstraints(const b2SolverData& data) override;
6282 void SolveVelocityConstraints(const b2SolverData& data) override;
6283 bool SolvePositionConstraints(const b2SolverData& data) override;
6284
6285 b2Vec2 m_localAnchorA;
6286 b2Vec2 m_localAnchorB;
6287 b2Vec2 m_localXAxisA;
6288 b2Vec2 m_localYAxisA;
6289
6290 float m_impulse;
6291 float m_motorImpulse;
6292 float m_springImpulse;
6293
6294 float m_lowerImpulse;
6295 float m_upperImpulse;
6296 float m_translation;
6297 float m_lowerTranslation;
6298 float m_upperTranslation;
6299
6300 float m_maxMotorTorque;
6301 float m_motorSpeed;
6302
6303 bool m_enableLimit;
6304 bool m_enableMotor;
6305
6306 float m_stiffness;
6307 float m_damping;
6308
6309 // Solver temp
6310 int32 m_indexA;
6311 int32 m_indexB;
6312 b2Vec2 m_localCenterA;
6313 b2Vec2 m_localCenterB;
6314 float m_invMassA;
6315 float m_invMassB;
6316 float m_invIA;
6317 float m_invIB;
6318
6319 b2Vec2 m_ax, m_ay;
6320 float m_sAx, m_sBx;
6321 float m_sAy, m_sBy;
6322
6323 float m_mass;
6324 float m_motorMass;
6325 float m_axialMass;
6326 float m_springMass;
6327
6328 float m_bias;
6329 float m_gamma;
6330
6331};
6332
6333inline float b2WheelJoint::GetMotorSpeed() const {
6334 return m_motorSpeed;
6335}
6336
6337inline float b2WheelJoint::GetMaxMotorTorque() const {
6338 return m_maxMotorTorque;
6339}
6340
6341#endif
6342// MIT License
6343
6344// Copyright (c) 2019 Erin Catto
6345
6346// Permission is hereby granted, free of charge, to any person obtaining a copy
6347// of this software and associated documentation files (the "Software"), to deal
6348// in the Software without restriction, including without limitation the rights
6349// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6350// copies of the Software, and to permit persons to whom the Software is
6351// furnished to do so, subject to the following conditions:
6352
6353// The above copyright notice and this permission notice shall be included in all
6354// copies or substantial portions of the Software.
6355
6356// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6357// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6358// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6359// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6360// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6361// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6362// SOFTWARE.
6363
6364#ifndef B2_WORLD_CALLBACKS_H
6365#define B2_WORLD_CALLBACKS_H
6366
6367//#include "b2_api.h"
6368//#include "b2_settings.h"
6369
6370struct b2Vec2;
6371struct b2Transform;
6372class b2Fixture;
6373class b2Body;
6374class b2Joint;
6375class b2Contact;
6376struct b2ContactResult;
6377struct b2Manifold;
6378
6379/// Joints and fixtures are destroyed when their associated
6380/// body is destroyed. Implement this listener so that you
6381/// may nullify references to these joints and shapes.
6382class B2_API b2DestructionListener {
6383public:
6384 virtual ~b2DestructionListener() {}
6385
6386 /// Called when any joint is about to be destroyed due
6387 /// to the destruction of one of its attached bodies.
6388 virtual void SayGoodbye(b2Joint* joint) = 0;
6389
6390 /// Called when any fixture is about to be destroyed due
6391 /// to the destruction of its parent body.
6392 virtual void SayGoodbye(b2Fixture* fixture) = 0;
6393};
6394
6395/// Implement this class to provide collision filtering. In other words, you can implement
6396/// this class if you want finer control over contact creation.
6397class B2_API b2ContactFilter {
6398public:
6399 virtual ~b2ContactFilter() {}
6400
6401 /// Return true if contact calculations should be performed between these two shapes.
6402 /// @warning for performance reasons this is only called when the AABBs begin to overlap.
6403 virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB);
6404};
6405
6406/// Contact impulses for reporting. Impulses are used instead of forces because
6407/// sub-step forces may approach infinity for rigid body collisions. These
6408/// match up one-to-one with the contact points in b2Manifold.
6409struct B2_API b2ContactImpulse {
6410 float normalImpulses[b2_maxManifoldPoints];
6411 float tangentImpulses[b2_maxManifoldPoints];
6412 int32 count;
6413};
6414
6415/// Implement this class to get contact information. You can use these results for
6416/// things like sounds and game logic. You can also get contact results by
6417/// traversing the contact lists after the time step. However, you might miss
6418/// some contacts because continuous physics leads to sub-stepping.
6419/// Additionally you may receive multiple callbacks for the same contact in a
6420/// single time step.
6421/// You should strive to make your callbacks efficient because there may be
6422/// many callbacks per time step.
6423/// @warning You cannot create/destroy Box2D entities inside these callbacks.
6424class B2_API b2ContactListener {
6425public:
6426 virtual ~b2ContactListener() {}
6427
6428 /// Called when two fixtures begin to touch.
6429 virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); }
6430
6431 /// Called when two fixtures cease to touch.
6432 virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); }
6433
6434 /// This is called after a contact is updated. This allows you to inspect a
6435 /// contact before it goes to the solver. If you are careful, you can modify the
6436 /// contact manifold (e.g. disable contact).
6437 /// A copy of the old manifold is provided so that you can detect changes.
6438 /// Note: this is called only for awake bodies.
6439 /// Note: this is called even when the number of contact points is zero.
6440 /// Note: this is not called for sensors.
6441 /// Note: if you set the number of contact points to zero, you will not
6442 /// get an EndContact callback. However, you may get a BeginContact callback
6443 /// the next step.
6444 virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) {
6445 B2_NOT_USED(contact);
6446 B2_NOT_USED(oldManifold);
6447 }
6448
6449 /// This lets you inspect a contact after the solver is finished. This is useful
6450 /// for inspecting impulses.
6451 /// Note: the contact manifold does not include time of impact impulses, which can be
6452 /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
6453 /// in a separate data structure.
6454 /// Note: this is only called for contacts that are touching, solid, and awake.
6455 virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) {
6456 B2_NOT_USED(contact);
6457 B2_NOT_USED(impulse);
6458 }
6459};
6460
6461/// Callback class for AABB queries.
6462/// See b2World::Query
6463class B2_API b2QueryCallback {
6464public:
6465 virtual ~b2QueryCallback() {}
6466
6467 /// Called for each fixture found in the query AABB.
6468 /// @return false to terminate the query.
6469 virtual bool ReportFixture(b2Fixture* fixture) = 0;
6470};
6471
6472/// Callback class for ray casts.
6473/// See b2World::RayCast
6474class B2_API b2RayCastCallback {
6475public:
6476 virtual ~b2RayCastCallback() {}
6477
6478 /// Called for each fixture found in the query. You control how the ray cast
6479 /// proceeds by returning a float:
6480 /// return -1: ignore this fixture and continue
6481 /// return 0: terminate the ray cast
6482 /// return fraction: clip the ray to this point
6483 /// return 1: don't clip the ray and continue
6484 /// @param fixture the fixture hit by the ray
6485 /// @param point the point of initial intersection
6486 /// @param normal the normal vector at the point of intersection
6487 /// @param fraction the fraction along the ray at the point of intersection
6488 /// @return -1 to filter, 0 to terminate, fraction to clip the ray for
6489 /// closest hit, 1 to continue
6490 virtual float ReportFixture(b2Fixture* fixture, const b2Vec2& point,
6491 const b2Vec2& normal, float fraction) = 0;
6492};
6493
6494#endif
6495// MIT License
6496
6497// Copyright (c) 2019 Erin Catto
6498
6499// Permission is hereby granted, free of charge, to any person obtaining a copy
6500// of this software and associated documentation files (the "Software"), to deal
6501// in the Software without restriction, including without limitation the rights
6502// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6503// copies of the Software, and to permit persons to whom the Software is
6504// furnished to do so, subject to the following conditions:
6505
6506// The above copyright notice and this permission notice shall be included in all
6507// copies or substantial portions of the Software.
6508
6509// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6510// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6511// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6512// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6513// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6514// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6515// SOFTWARE.
6516
6517#ifndef B2_WORLD_H
6518#define B2_WORLD_H
6519
6520//#include "b2_api.h"
6521//#include "b2_block_allocator.h"
6522//#include "b2_contact_manager.h"
6523//#include "b2_math.h"
6524//#include "b2_stack_allocator.h"
6525//#include "b2_time_step.h"
6526//#include "b2_world_callbacks.h"
6527
6528struct b2AABB;
6529struct b2BodyDef;
6530struct b2Color;
6531struct b2JointDef;
6532class b2Body;
6533class b2Draw;
6534class b2Fixture;
6535class b2Joint;
6536
6537/// The world class manages all physics entities, dynamic simulation,
6538/// and asynchronous queries. The world also contains efficient memory
6539/// management facilities.
6540class B2_API b2World {
6541public:
6542 /// Construct a world object.
6543 /// @param gravity the world gravity vector.
6544 b2World(const b2Vec2& gravity);
6545
6546 /// Destruct the world. All physics entities are destroyed and all heap memory is released.
6547 ~b2World();
6548
6549 /// Register a destruction listener. The listener is owned by you and must
6550 /// remain in scope.
6551 void SetDestructionListener(b2DestructionListener* listener);
6552
6553 /// Register a contact filter to provide specific control over collision.
6554 /// Otherwise the default filter is used (b2_defaultFilter). The listener is
6555 /// owned by you and must remain in scope.
6556 void SetContactFilter(b2ContactFilter* filter);
6557
6558 /// Register a contact event listener. The listener is owned by you and must
6559 /// remain in scope.
6560 void SetContactListener(b2ContactListener* listener);
6561
6562 /// Register a routine for debug drawing. The debug draw functions are called
6563 /// inside with b2World::DebugDraw method. The debug draw object is owned
6564 /// by you and must remain in scope.
6565 void SetDebugDraw(b2Draw* debugDraw);
6566
6567 /// Create a rigid body given a definition. No reference to the definition
6568 /// is retained.
6569 /// @warning This function is locked during callbacks.
6570 b2Body* CreateBody(const b2BodyDef* def);
6571
6572 /// Destroy a rigid body given a definition. No reference to the definition
6573 /// is retained. This function is locked during callbacks.
6574 /// @warning This automatically deletes all associated shapes and joints.
6575 /// @warning This function is locked during callbacks.
6576 void DestroyBody(b2Body* body);
6577
6578 /// Create a joint to constrain bodies together. No reference to the definition
6579 /// is retained. This may cause the connected bodies to cease colliding.
6580 /// @warning This function is locked during callbacks.
6581 b2Joint* CreateJoint(const b2JointDef* def);
6582
6583 /// Destroy a joint. This may cause the connected bodies to begin colliding.
6584 /// @warning This function is locked during callbacks.
6585 void DestroyJoint(b2Joint* joint);
6586
6587 /// Take a time step. This performs collision detection, integration,
6588 /// and constraint solution.
6589 /// @param timeStep the amount of time to simulate, this should not vary.
6590 /// @param velocityIterations for the velocity constraint solver.
6591 /// @param positionIterations for the position constraint solver.
6592 void Step(float timeStep,
6593 int32 velocityIterations,
6594 int32 positionIterations);
6595
6596 /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically
6597 /// after each call to Step. The default behavior is modified by calling SetAutoClearForces.
6598 /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain
6599 /// a fixed sized time step under a variable frame-rate.
6600 /// When you perform sub-stepping you will disable auto clearing of forces and instead call
6601 /// ClearForces after all sub-steps are complete in one pass of your game loop.
6602 /// @see SetAutoClearForces
6603 void ClearForces();
6604
6605 /// Call this to draw shapes and other debug draw data. This is intentionally non-const.
6606 void DebugDraw();
6607
6608 /// Query the world for all fixtures that potentially overlap the
6609 /// provided AABB.
6610 /// @param callback a user implemented callback class.
6611 /// @param aabb the query box.
6612 void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const;
6613
6614 /// Ray-cast the world for all fixtures in the path of the ray. Your callback
6615 /// controls whether you get the closest point, any point, or n-points.
6616 /// The ray-cast ignores shapes that contain the starting point.
6617 /// @param callback a user implemented callback class.
6618 /// @param point1 the ray starting point
6619 /// @param point2 the ray ending point
6620 void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;
6621
6622 /// Get the world body list. With the returned body, use b2Body::GetNext to get
6623 /// the next body in the world list. A nullptr body indicates the end of the list.
6624 /// @return the head of the world body list.
6625 b2Body* GetBodyList();
6626 const b2Body* GetBodyList() const;
6627
6628 /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
6629 /// the next joint in the world list. A nullptr joint indicates the end of the list.
6630 /// @return the head of the world joint list.
6631 b2Joint* GetJointList();
6632 const b2Joint* GetJointList() const;
6633
6634 /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
6635 /// the next contact in the world list. A nullptr contact indicates the end of the list.
6636 /// @return the head of the world contact list.
6637 /// @warning contacts are created and destroyed in the middle of a time step.
6638 /// Use b2ContactListener to avoid missing contacts.
6639 b2Contact* GetContactList();
6640 const b2Contact* GetContactList() const;
6641
6642 /// Enable/disable sleep.
6643 void SetAllowSleeping(bool flag);
6644 bool GetAllowSleeping() const { return m_allowSleep; }
6645
6646 /// Enable/disable warm starting. For testing.
6647 void SetWarmStarting(bool flag) { m_warmStarting = flag; }
6648 bool GetWarmStarting() const { return m_warmStarting; }
6649
6650 /// Enable/disable continuous physics. For testing.
6651 void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
6652 bool GetContinuousPhysics() const { return m_continuousPhysics; }
6653
6654 /// Enable/disable single stepped continuous physics. For testing.
6655 void SetSubStepping(bool flag) { m_subStepping = flag; }
6656 bool GetSubStepping() const { return m_subStepping; }
6657
6658 /// Get the number of broad-phase proxies.
6659 int32 GetProxyCount() const;
6660
6661 /// Get the number of bodies.
6662 int32 GetBodyCount() const;
6663
6664 /// Get the number of joints.
6665 int32 GetJointCount() const;
6666
6667 /// Get the number of contacts (each may have 0 or more contact points).
6668 int32 GetContactCount() const;
6669
6670 /// Get the height of the dynamic tree.
6671 int32 GetTreeHeight() const;
6672
6673 /// Get the balance of the dynamic tree.
6674 int32 GetTreeBalance() const;
6675
6676 /// Get the quality metric of the dynamic tree. The smaller the better.
6677 /// The minimum is 1.
6678 float GetTreeQuality() const;
6679
6680 /// Change the global gravity vector.
6681 void SetGravity(const b2Vec2& gravity);
6682
6683 /// Get the global gravity vector.
6684 b2Vec2 GetGravity() const;
6685
6686 /// Is the world locked (in the middle of a time step).
6687 bool IsLocked() const;
6688
6689 /// Set flag to control automatic clearing of forces after each time step.
6690 void SetAutoClearForces(bool flag);
6691
6692 /// Get the flag that controls automatic clearing of forces after each time step.
6693 bool GetAutoClearForces() const;
6694
6695 /// Shift the world origin. Useful for large worlds.
6696 /// The body shift formula is: position -= newOrigin
6697 /// @param newOrigin the new origin with respect to the old origin
6698 void ShiftOrigin(const b2Vec2& newOrigin);
6699
6700 /// Get the contact manager for testing.
6701 const b2ContactManager& GetContactManager() const;
6702
6703 /// Get the current profile.
6704 const b2Profile& GetProfile() const;
6705
6706 /// Dump the world into the log file.
6707 /// @warning this should be called outside of a time step.
6708 void Dump();
6709
6710private:
6711
6712 friend class b2Body;
6713 friend class b2Fixture;
6714 friend class b2ContactManager;
6715 friend class b2Controller;
6716
6717 void Solve(const b2TimeStep& step);
6718 void SolveTOI(const b2TimeStep& step);
6719
6720 void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);
6721
6722 b2BlockAllocator m_blockAllocator;
6723 b2StackAllocator m_stackAllocator;
6724
6725 b2ContactManager m_contactManager;
6726
6727 b2Body* m_bodyList;
6728 b2Joint* m_jointList;
6729
6730 int32 m_bodyCount;
6731 int32 m_jointCount;
6732
6733 b2Vec2 m_gravity;
6734 bool m_allowSleep;
6735
6736 b2DestructionListener* m_destructionListener;
6737 b2Draw* m_debugDraw;
6738
6739 // This is used to compute the time step ratio to
6740 // support a variable time step.
6741 float m_inv_dt0;
6742
6743 bool m_newContacts;
6744 bool m_locked;
6745 bool m_clearForces;
6746
6747 // These are for debugging the solver.
6748 bool m_warmStarting;
6749 bool m_continuousPhysics;
6750 bool m_subStepping;
6751
6752 bool m_stepComplete;
6753
6754 b2Profile m_profile;
6755};
6756
6757inline b2Body* b2World::GetBodyList() {
6758 return m_bodyList;
6759}
6760
6761inline const b2Body* b2World::GetBodyList() const {
6762 return m_bodyList;
6763}
6764
6765inline b2Joint* b2World::GetJointList() {
6766 return m_jointList;
6767}
6768
6769inline const b2Joint* b2World::GetJointList() const {
6770 return m_jointList;
6771}
6772
6773inline b2Contact* b2World::GetContactList() {
6774 return m_contactManager.m_contactList;
6775}
6776
6777inline const b2Contact* b2World::GetContactList() const {
6778 return m_contactManager.m_contactList;
6779}
6780
6781inline int32 b2World::GetBodyCount() const {
6782 return m_bodyCount;
6783}
6784
6785inline int32 b2World::GetJointCount() const {
6786 return m_jointCount;
6787}
6788
6789inline int32 b2World::GetContactCount() const {
6790 return m_contactManager.m_contactCount;
6791}
6792
6793inline void b2World::SetGravity(const b2Vec2& gravity) {
6794 m_gravity = gravity;
6795}
6796
6797inline b2Vec2 b2World::GetGravity() const {
6798 return m_gravity;
6799}
6800
6801inline bool b2World::IsLocked() const {
6802 return m_locked;
6803}
6804
6805inline void b2World::SetAutoClearForces(bool flag) {
6806 m_clearForces = flag;
6807}
6808
6809/// Get the flag that controls automatic clearing of forces after each time step.
6810inline bool b2World::GetAutoClearForces() const {
6811 return m_clearForces;
6812}
6813
6814inline const b2ContactManager& b2World::GetContactManager() const {
6815 return m_contactManager;
6816}
6817
6818inline const b2Profile& b2World::GetProfile() const {
6819 return m_profile;
6820}
6821
6822#endif
6823// MIT License
6824
6825// Copyright (c) 2019 Erin Catto
6826
6827// Permission is hereby granted, free of charge, to any person obtaining a copy
6828// of this software and associated documentation files (the "Software"), to deal
6829// in the Software without restriction, including without limitation the rights
6830// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6831// copies of the Software, and to permit persons to whom the Software is
6832// furnished to do so, subject to the following conditions:
6833
6834// The above copyright notice and this permission notice shall be included in all
6835// copies or substantial portions of the Software.
6836
6837// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6838// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6839// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6840// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6841// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6842// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6843// SOFTWARE.
6844
6845#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H
6846#define B2_CHAIN_AND_CIRCLE_CONTACT_H
6847
6848//#include "box2d/b2_contact.h"
6849
6850class b2BlockAllocator;
6851
6852class b2ChainAndCircleContact : public b2Contact {
6853public:
6854 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
6855 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
6856 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
6857
6858 b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
6859 ~b2ChainAndCircleContact() {}
6860
6861 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
6862};
6863
6864#endif
6865// MIT License
6866
6867// Copyright (c) 2019 Erin Catto
6868
6869// Permission is hereby granted, free of charge, to any person obtaining a copy
6870// of this software and associated documentation files (the "Software"), to deal
6871// in the Software without restriction, including without limitation the rights
6872// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6873// copies of the Software, and to permit persons to whom the Software is
6874// furnished to do so, subject to the following conditions:
6875
6876// The above copyright notice and this permission notice shall be included in all
6877// copies or substantial portions of the Software.
6878
6879// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6880// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6881// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6882// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6883// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6884// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6885// SOFTWARE.
6886
6887#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H
6888#define B2_CHAIN_AND_POLYGON_CONTACT_H
6889
6890//#include "box2d/b2_contact.h"
6891
6892class b2BlockAllocator;
6893
6894class b2ChainAndPolygonContact : public b2Contact {
6895public:
6896 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
6897 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
6898 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
6899
6900 b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
6901 ~b2ChainAndPolygonContact() {}
6902
6903 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
6904};
6905
6906#endif
6907// MIT License
6908
6909// Copyright (c) 2019 Erin Catto
6910
6911// Permission is hereby granted, free of charge, to any person obtaining a copy
6912// of this software and associated documentation files (the "Software"), to deal
6913// in the Software without restriction, including without limitation the rights
6914// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6915// copies of the Software, and to permit persons to whom the Software is
6916// furnished to do so, subject to the following conditions:
6917
6918// The above copyright notice and this permission notice shall be included in all
6919// copies or substantial portions of the Software.
6920
6921// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6922// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6923// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6924// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6925// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6926// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6927// SOFTWARE.
6928
6929#ifndef B2_CIRCLE_CONTACT_H
6930#define B2_CIRCLE_CONTACT_H
6931
6932//#include "box2d/b2_contact.h"
6933
6934class b2BlockAllocator;
6935
6936class b2CircleContact : public b2Contact {
6937public:
6938 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
6939 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
6940 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
6941
6942 b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
6943 ~b2CircleContact() {}
6944
6945 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
6946};
6947
6948#endif
6949// MIT License
6950
6951// Copyright (c) 2019 Erin Catto
6952
6953// Permission is hereby granted, free of charge, to any person obtaining a copy
6954// of this software and associated documentation files (the "Software"), to deal
6955// in the Software without restriction, including without limitation the rights
6956// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
6957// copies of the Software, and to permit persons to whom the Software is
6958// furnished to do so, subject to the following conditions:
6959
6960// The above copyright notice and this permission notice shall be included in all
6961// copies or substantial portions of the Software.
6962
6963// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
6964// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6965// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
6966// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
6967// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
6968// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
6969// SOFTWARE.
6970
6971#ifndef B2_CONTACT_SOLVER_H
6972#define B2_CONTACT_SOLVER_H
6973
6974//#include "box2d/b2_collision.h"
6975//#include "box2d/b2_math.h"
6976//#include "box2d/b2_time_step.h"
6977
6978class b2Contact;
6979class b2Body;
6980class b2StackAllocator;
6981struct b2ContactPositionConstraint;
6982
6983struct b2VelocityConstraintPoint {
6984 b2Vec2 rA;
6985 b2Vec2 rB;
6986 float normalImpulse;
6987 float tangentImpulse;
6988 float normalMass;
6989 float tangentMass;
6990 float velocityBias;
6991};
6992
6993struct b2ContactVelocityConstraint {
6994 b2VelocityConstraintPoint points[b2_maxManifoldPoints];
6995 b2Vec2 normal;
6996 b2Mat22 normalMass;
6997 b2Mat22 K;
6998 int32 indexA;
6999 int32 indexB;
7000 float invMassA, invMassB;
7001 float invIA, invIB;
7002 float friction;
7003 float restitution;
7004 float threshold;
7005 float tangentSpeed;
7006 int32 pointCount;
7007 int32 contactIndex;
7008};
7009
7010struct b2ContactSolverDef {
7011 b2TimeStep step;
7012 b2Contact** contacts;
7013 int32 count;
7014 b2Position* positions;
7015 b2Velocity* velocities;
7016 b2StackAllocator* allocator;
7017};
7018
7019class b2ContactSolver {
7020public:
7021 b2ContactSolver(b2ContactSolverDef* def);
7022 ~b2ContactSolver();
7023
7024 void InitializeVelocityConstraints();
7025
7026 void WarmStart();
7027 void SolveVelocityConstraints();
7028 void StoreImpulses();
7029
7030 bool SolvePositionConstraints();
7031 bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
7032
7033 b2TimeStep m_step;
7034 b2Position* m_positions;
7035 b2Velocity* m_velocities;
7036 b2StackAllocator* m_allocator;
7037 b2ContactPositionConstraint* m_positionConstraints;
7038 b2ContactVelocityConstraint* m_velocityConstraints;
7039 b2Contact** m_contacts;
7040 int m_count;
7041};
7042
7043#endif
7044
7045// MIT License
7046
7047// Copyright (c) 2019 Erin Catto
7048
7049// Permission is hereby granted, free of charge, to any person obtaining a copy
7050// of this software and associated documentation files (the "Software"), to deal
7051// in the Software without restriction, including without limitation the rights
7052// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7053// copies of the Software, and to permit persons to whom the Software is
7054// furnished to do so, subject to the following conditions:
7055
7056// The above copyright notice and this permission notice shall be included in all
7057// copies or substantial portions of the Software.
7058
7059// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7060// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7061// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7062// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7063// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7064// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7065// SOFTWARE.
7066
7067#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H
7068#define B2_EDGE_AND_CIRCLE_CONTACT_H
7069
7070//#include "box2d/b2_contact.h"
7071
7072class b2BlockAllocator;
7073
7074class b2EdgeAndCircleContact : public b2Contact {
7075public:
7076 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
7077 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
7078 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
7079
7080 b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
7081 ~b2EdgeAndCircleContact() {}
7082
7083 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
7084};
7085
7086#endif
7087// MIT License
7088
7089// Copyright (c) 2019 Erin Catto
7090
7091// Permission is hereby granted, free of charge, to any person obtaining a copy
7092// of this software and associated documentation files (the "Software"), to deal
7093// in the Software without restriction, including without limitation the rights
7094// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7095// copies of the Software, and to permit persons to whom the Software is
7096// furnished to do so, subject to the following conditions:
7097
7098// The above copyright notice and this permission notice shall be included in all
7099// copies or substantial portions of the Software.
7100
7101// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7102// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7103// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7104// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7105// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7106// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7107// SOFTWARE.
7108
7109#ifndef B2_EDGE_AND_POLYGON_CONTACT_H
7110#define B2_EDGE_AND_POLYGON_CONTACT_H
7111
7112//#include "box2d/b2_contact.h"
7113
7114class b2BlockAllocator;
7115
7116class b2EdgeAndPolygonContact : public b2Contact {
7117public:
7118 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
7119 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
7120 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
7121
7122 b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
7123 ~b2EdgeAndPolygonContact() {}
7124
7125 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
7126};
7127
7128#endif
7129// MIT License
7130
7131// Copyright (c) 2019 Erin Catto
7132
7133// Permission is hereby granted, free of charge, to any person obtaining a copy
7134// of this software and associated documentation files (the "Software"), to deal
7135// in the Software without restriction, including without limitation the rights
7136// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7137// copies of the Software, and to permit persons to whom the Software is
7138// furnished to do so, subject to the following conditions:
7139
7140// The above copyright notice and this permission notice shall be included in all
7141// copies or substantial portions of the Software.
7142
7143// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7144// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7145// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7146// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7147// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7148// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7149// SOFTWARE.
7150
7151#ifndef B2_ISLAND_H
7152#define B2_ISLAND_H
7153
7154//#include "box2d/b2_body.h"
7155//#include "box2d/b2_math.h"
7156//#include "box2d/b2_time_step.h"
7157
7158class b2Contact;
7159class b2Joint;
7160class b2StackAllocator;
7161class b2ContactListener;
7162struct b2ContactVelocityConstraint;
7163struct b2Profile;
7164
7165/// This is an internal class.
7166class b2Island {
7167public:
7168 b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity,
7169 b2StackAllocator* allocator, b2ContactListener* listener);
7170 ~b2Island();
7171
7172 void Clear() {
7173 m_bodyCount = 0;
7174 m_contactCount = 0;
7175 m_jointCount = 0;
7176 }
7177
7178 void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
7179
7180 void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB);
7181
7182 void Add(b2Body* body) {
7183 b2Assert(m_bodyCount < m_bodyCapacity);
7184 body->m_islandIndex = m_bodyCount;
7185 m_bodies[m_bodyCount] = body;
7186 ++m_bodyCount;
7187 }
7188
7189 void Add(b2Contact* contact) {
7190 b2Assert(m_contactCount < m_contactCapacity);
7191 m_contacts[m_contactCount++] = contact;
7192 }
7193
7194 void Add(b2Joint* joint) {
7195 b2Assert(m_jointCount < m_jointCapacity);
7196 m_joints[m_jointCount++] = joint;
7197 }
7198
7199 void Report(const b2ContactVelocityConstraint* constraints);
7200
7201 b2StackAllocator* m_allocator;
7202 b2ContactListener* m_listener;
7203
7204 b2Body** m_bodies;
7205 b2Contact** m_contacts;
7206 b2Joint** m_joints;
7207
7208 b2Position* m_positions;
7209 b2Velocity* m_velocities;
7210
7211 int32 m_bodyCount;
7212 int32 m_jointCount;
7213 int32 m_contactCount;
7214
7215 int32 m_bodyCapacity;
7216 int32 m_contactCapacity;
7217 int32 m_jointCapacity;
7218};
7219
7220#endif
7221// MIT License
7222
7223// Copyright (c) 2019 Erin Catto
7224
7225// Permission is hereby granted, free of charge, to any person obtaining a copy
7226// of this software and associated documentation files (the "Software"), to deal
7227// in the Software without restriction, including without limitation the rights
7228// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7229// copies of the Software, and to permit persons to whom the Software is
7230// furnished to do so, subject to the following conditions:
7231
7232// The above copyright notice and this permission notice shall be included in all
7233// copies or substantial portions of the Software.
7234
7235// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7236// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7237// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7238// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7239// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7240// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7241// SOFTWARE.
7242
7243#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H
7244#define B2_POLYGON_AND_CIRCLE_CONTACT_H
7245
7246//#include "box2d/b2_contact.h"
7247
7248class b2BlockAllocator;
7249
7250class b2PolygonAndCircleContact : public b2Contact {
7251public:
7252 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
7253 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
7254
7255 b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
7256 ~b2PolygonAndCircleContact() {}
7257
7258 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
7259};
7260
7261#endif
7262// MIT License
7263
7264// Copyright (c) 2019 Erin Catto
7265
7266// Permission is hereby granted, free of charge, to any person obtaining a copy
7267// of this software and associated documentation files (the "Software"), to deal
7268// in the Software without restriction, including without limitation the rights
7269// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7270// copies of the Software, and to permit persons to whom the Software is
7271// furnished to do so, subject to the following conditions:
7272
7273// The above copyright notice and this permission notice shall be included in all
7274// copies or substantial portions of the Software.
7275
7276// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7277// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7278// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7279// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7280// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7281// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7282// SOFTWARE.
7283
7284#ifndef B2_POLYGON_CONTACT_H
7285#define B2_POLYGON_CONTACT_H
7286
7287//#include "box2d/b2_contact.h"
7288
7289class b2BlockAllocator;
7290
7291class b2PolygonContact : public b2Contact {
7292public:
7293 static b2Contact* Create(b2Fixture* fixtureA, int32 indexA,
7294 b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
7295 static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
7296
7297 b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
7298 ~b2PolygonContact() {}
7299
7300 void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
7301};
7302
7303#endif
7304
7305#ifndef BOX2D_IMPL
7306#define BOX2D_IMPL
7307
7308// MIT License
7309
7310// Copyright (c) 2019 Erin Catto
7311
7312// Permission is hereby granted, free of charge, to any person obtaining a copy
7313// of this software and associated documentation files (the "Software"), to deal
7314// in the Software without restriction, including without limitation the rights
7315// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7316// copies of the Software, and to permit persons to whom the Software is
7317// furnished to do so, subject to the following conditions:
7318
7319// The above copyright notice and this permission notice shall be included in all
7320// copies or substantial portions of the Software.
7321
7322// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7323// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7324// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7325// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7326// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7327// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7328// SOFTWARE.
7329
7330#include "box2d/b2_broad_phase.h"
7331#include <string.h>
7332
7333b2BroadPhase::b2BroadPhase() {
7334 m_proxyCount = 0;
7335
7336 m_pairCapacity = 16;
7337 m_pairCount = 0;
7338 m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
7339
7340 m_moveCapacity = 16;
7341 m_moveCount = 0;
7342 m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
7343}
7344
7345b2BroadPhase::~b2BroadPhase() {
7346 b2Free(m_moveBuffer);
7347 b2Free(m_pairBuffer);
7348}
7349
7350int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) {
7351 int32 proxyId = m_tree.CreateProxy(aabb, userData);
7352 ++m_proxyCount;
7353 BufferMove(proxyId);
7354 return proxyId;
7355}
7356
7357void b2BroadPhase::DestroyProxy(int32 proxyId) {
7358 UnBufferMove(proxyId);
7359 --m_proxyCount;
7360 m_tree.DestroyProxy(proxyId);
7361}
7362
7363void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) {
7364 bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
7365 if (buffer) {
7366 BufferMove(proxyId);
7367 }
7368}
7369
7370void b2BroadPhase::TouchProxy(int32 proxyId) {
7371 BufferMove(proxyId);
7372}
7373
7374void b2BroadPhase::BufferMove(int32 proxyId) {
7375 if (m_moveCount == m_moveCapacity) {
7376 int32* oldBuffer = m_moveBuffer;
7377 m_moveCapacity *= 2;
7378 m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
7379 memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
7380 b2Free(oldBuffer);
7381 }
7382
7383 m_moveBuffer[m_moveCount] = proxyId;
7384 ++m_moveCount;
7385}
7386
7387void b2BroadPhase::UnBufferMove(int32 proxyId) {
7388 for (int32 i = 0; i < m_moveCount; ++i) {
7389 if (m_moveBuffer[i] == proxyId) {
7390 m_moveBuffer[i] = e_nullProxy;
7391 }
7392 }
7393}
7394
7395// This is called from b2DynamicTree::Query when we are gathering pairs.
7396bool b2BroadPhase::QueryCallback(int32 proxyId) {
7397 // A proxy cannot form a pair with itself.
7398 if (proxyId == m_queryProxyId) {
7399 return true;
7400 }
7401
7402 const bool moved = m_tree.WasMoved(proxyId);
7403 if (moved && proxyId > m_queryProxyId) {
7404 // Both proxies are moving. Avoid duplicate pairs.
7405 return true;
7406 }
7407
7408 // Grow the pair buffer as needed.
7409 if (m_pairCount == m_pairCapacity) {
7410 b2Pair* oldBuffer = m_pairBuffer;
7411 m_pairCapacity = m_pairCapacity + (m_pairCapacity >> 1);
7412 m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
7413 memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
7414 b2Free(oldBuffer);
7415 }
7416
7417 m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
7418 m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
7419 ++m_pairCount;
7420
7421 return true;
7422}
7423// MIT License
7424
7425// Copyright (c) 2019 Erin Catto
7426
7427// Permission is hereby granted, free of charge, to any person obtaining a copy
7428// of this software and associated documentation files (the "Software"), to deal
7429// in the Software without restriction, including without limitation the rights
7430// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7431// copies of the Software, and to permit persons to whom the Software is
7432// furnished to do so, subject to the following conditions:
7433
7434// The above copyright notice and this permission notice shall be included in all
7435// copies or substantial portions of the Software.
7436
7437// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7438// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7439// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7440// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7441// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7442// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7443// SOFTWARE.
7444
7445#include "box2d/b2_chain_shape.h"
7446#include "box2d/b2_edge_shape.h"
7447
7448#include "box2d/b2_block_allocator.h"
7449
7450#include <new>
7451#include <string.h>
7452
7453b2ChainShape::~b2ChainShape() {
7454 Clear();
7455}
7456
7457void b2ChainShape::Clear() {
7458 b2Free(m_vertices);
7459 m_vertices = nullptr;
7460 m_count = 0;
7461}
7462
7463void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) {
7464 b2Assert(m_vertices == nullptr && m_count == 0);
7465 b2Assert(count >= 3);
7466 if (count < 3) {
7467 return;
7468 }
7469
7470 for (int32 i = 1; i < count; ++i) {
7471 b2Vec2 v1 = vertices[i - 1];
7472 b2Vec2 v2 = vertices[i];
7473 // If the code crashes here, it means your vertices are too close together.
7474 b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
7475 }
7476
7477 m_count = count + 1;
7478 m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
7479 memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
7480 m_vertices[count] = m_vertices[0];
7481 m_prevVertex = m_vertices[m_count - 2];
7482 m_nextVertex = m_vertices[1];
7483}
7484
7485void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count, const b2Vec2& prevVertex, const b2Vec2& nextVertex) {
7486 b2Assert(m_vertices == nullptr && m_count == 0);
7487 b2Assert(count >= 2);
7488 for (int32 i = 1; i < count; ++i) {
7489 // If the code crashes here, it means your vertices are too close together.
7490 b2Assert(b2DistanceSquared(vertices[i - 1], vertices[i]) > b2_linearSlop * b2_linearSlop);
7491 }
7492
7493 m_count = count;
7494 m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
7495 memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
7496
7497 m_prevVertex = prevVertex;
7498 m_nextVertex = nextVertex;
7499}
7500
7501b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const {
7502 void* mem = allocator->Allocate(sizeof(b2ChainShape));
7503 b2ChainShape* clone = new (mem) b2ChainShape;
7504 clone->CreateChain(m_vertices, m_count, m_prevVertex, m_nextVertex);
7505 return clone;
7506}
7507
7508int32 b2ChainShape::GetChildCount() const {
7509 // edge count = vertex count - 1
7510 return m_count - 1;
7511}
7512
7513void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const {
7514 b2Assert(0 <= index && index < m_count - 1);
7515 edge->m_type = b2Shape::e_edge;
7516 edge->m_radius = m_radius;
7517
7518 edge->m_vertex1 = m_vertices[index + 0];
7519 edge->m_vertex2 = m_vertices[index + 1];
7520 edge->m_oneSided = true;
7521
7522 if (index > 0) {
7523 edge->m_vertex0 = m_vertices[index - 1];
7524 } else {
7525 edge->m_vertex0 = m_prevVertex;
7526 }
7527
7528 if (index < m_count - 2) {
7529 edge->m_vertex3 = m_vertices[index + 2];
7530 } else {
7531 edge->m_vertex3 = m_nextVertex;
7532 }
7533}
7534
7535bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const {
7536 B2_NOT_USED(xf);
7537 B2_NOT_USED(p);
7538 return false;
7539}
7540
7541bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
7542 const b2Transform& xf, int32 childIndex) const {
7543 b2Assert(childIndex < m_count);
7544
7545 b2EdgeShape edgeShape;
7546
7547 int32 i1 = childIndex;
7548 int32 i2 = childIndex + 1;
7549 if (i2 == m_count) {
7550 i2 = 0;
7551 }
7552
7553 edgeShape.m_vertex1 = m_vertices[i1];
7554 edgeShape.m_vertex2 = m_vertices[i2];
7555
7556 return edgeShape.RayCast(output, input, xf, 0);
7557}
7558
7559void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const {
7560 b2Assert(childIndex < m_count);
7561
7562 int32 i1 = childIndex;
7563 int32 i2 = childIndex + 1;
7564 if (i2 == m_count) {
7565 i2 = 0;
7566 }
7567
7568 b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
7569 b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
7570
7571 b2Vec2 lower = b2Min(v1, v2);
7572 b2Vec2 upper = b2Max(v1, v2);
7573
7574 b2Vec2 r(m_radius, m_radius);
7575 aabb->lowerBound = lower - r;
7576 aabb->upperBound = upper + r;
7577}
7578
7579void b2ChainShape::ComputeMass(b2MassData* massData, float density) const {
7580 B2_NOT_USED(density);
7581
7582 massData->mass = 0.0f;
7583 massData->center.SetZero();
7584 massData->I = 0.0f;
7585}
7586// MIT License
7587
7588// Copyright (c) 2019 Erin Catto
7589
7590// Permission is hereby granted, free of charge, to any person obtaining a copy
7591// of this software and associated documentation files (the "Software"), to deal
7592// in the Software without restriction, including without limitation the rights
7593// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7594// copies of the Software, and to permit persons to whom the Software is
7595// furnished to do so, subject to the following conditions:
7596
7597// The above copyright notice and this permission notice shall be included in all
7598// copies or substantial portions of the Software.
7599
7600// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7601// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7602// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7603// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7604// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7605// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7606// SOFTWARE.
7607
7608#include "box2d/b2_circle_shape.h"
7609#include "box2d/b2_block_allocator.h"
7610
7611#include <new>
7612
7613b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const {
7614 void* mem = allocator->Allocate(sizeof(b2CircleShape));
7615 b2CircleShape* clone = new (mem) b2CircleShape;
7616 *clone = *this;
7617 return clone;
7618}
7619
7620int32 b2CircleShape::GetChildCount() const {
7621 return 1;
7622}
7623
7624bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const {
7625 b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
7626 b2Vec2 d = p - center;
7627 return b2Dot(d, d) <= m_radius * m_radius;
7628}
7629
7630// Collision Detection in Interactive 3D Environments by Gino van den Bergen
7631// From Section 3.1.2
7632// x = s + a * r
7633// norm(x) = radius
7634bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
7635 const b2Transform& transform, int32 childIndex) const {
7636 B2_NOT_USED(childIndex);
7637
7638 b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
7639 b2Vec2 s = input.p1 - position;
7640 float b = b2Dot(s, s) - m_radius * m_radius;
7641
7642 // Solve quadratic equation.
7643 b2Vec2 r = input.p2 - input.p1;
7644 float c = b2Dot(s, r);
7645 float rr = b2Dot(r, r);
7646 float sigma = c * c - rr * b;
7647
7648 // Check for negative discriminant and short segment.
7649 if (sigma < 0.0f || rr < b2_epsilon) {
7650 return false;
7651 }
7652
7653 // Find the point of intersection of the line with the circle.
7654 float a = -(c + b2Sqrt(sigma));
7655
7656 // Is the intersection point on the segment?
7657 if (0.0f <= a && a <= input.maxFraction * rr) {
7658 a /= rr;
7659 output->fraction = a;
7660 output->normal = s + a * r;
7661 output->normal.Normalize();
7662 return true;
7663 }
7664
7665 return false;
7666}
7667
7668void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const {
7669 B2_NOT_USED(childIndex);
7670
7671 b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
7672 aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
7673 aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
7674}
7675
7676void b2CircleShape::ComputeMass(b2MassData* massData, float density) const {
7677 massData->mass = density * b2_pi * m_radius * m_radius;
7678 massData->center = m_p;
7679
7680 // inertia about the local origin
7681 massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
7682}
7683// MIT License
7684
7685// Copyright (c) 2019 Erin Catto
7686
7687// Permission is hereby granted, free of charge, to any person obtaining a copy
7688// of this software and associated documentation files (the "Software"), to deal
7689// in the Software without restriction, including without limitation the rights
7690// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7691// copies of the Software, and to permit persons to whom the Software is
7692// furnished to do so, subject to the following conditions:
7693
7694// The above copyright notice and this permission notice shall be included in all
7695// copies or substantial portions of the Software.
7696
7697// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7698// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7699// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7700// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7701// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7702// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7703// SOFTWARE.
7704
7705#include "box2d/b2_collision.h"
7706#include "box2d/b2_circle_shape.h"
7707#include "box2d/b2_polygon_shape.h"
7708
7709void b2CollideCircles(
7710 b2Manifold* manifold,
7711 const b2CircleShape* circleA, const b2Transform& xfA,
7712 const b2CircleShape* circleB, const b2Transform& xfB) {
7713 manifold->pointCount = 0;
7714
7715 b2Vec2 pA = b2Mul(xfA, circleA->m_p);
7716 b2Vec2 pB = b2Mul(xfB, circleB->m_p);
7717
7718 b2Vec2 d = pB - pA;
7719 float distSqr = b2Dot(d, d);
7720 float rA = circleA->m_radius, rB = circleB->m_radius;
7721 float radius = rA + rB;
7722 if (distSqr > radius * radius) {
7723 return;
7724 }
7725
7726 manifold->type = b2Manifold::e_circles;
7727 manifold->localPoint = circleA->m_p;
7728 manifold->localNormal.SetZero();
7729 manifold->pointCount = 1;
7730
7731 manifold->points[0].localPoint = circleB->m_p;
7732 manifold->points[0].id.key = 0;
7733}
7734
7735void b2CollidePolygonAndCircle(
7736 b2Manifold* manifold,
7737 const b2PolygonShape* polygonA, const b2Transform& xfA,
7738 const b2CircleShape* circleB, const b2Transform& xfB) {
7739 manifold->pointCount = 0;
7740
7741 // Compute circle position in the frame of the polygon.
7742 b2Vec2 c = b2Mul(xfB, circleB->m_p);
7743 b2Vec2 cLocal = b2MulT(xfA, c);
7744
7745 // Find the min separating edge.
7746 int32 normalIndex = 0;
7747 float separation = -b2_maxFloat;
7748 float radius = polygonA->m_radius + circleB->m_radius;
7749 int32 vertexCount = polygonA->m_count;
7750 const b2Vec2* vertices = polygonA->m_vertices;
7751 const b2Vec2* normals = polygonA->m_normals;
7752
7753 for (int32 i = 0; i < vertexCount; ++i) {
7754 float s = b2Dot(normals[i], cLocal - vertices[i]);
7755
7756 if (s > radius) {
7757 // Early out.
7758 return;
7759 }
7760
7761 if (s > separation) {
7762 separation = s;
7763 normalIndex = i;
7764 }
7765 }
7766
7767 // Vertices that subtend the incident face.
7768 int32 vertIndex1 = normalIndex;
7769 int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
7770 b2Vec2 v1 = vertices[vertIndex1];
7771 b2Vec2 v2 = vertices[vertIndex2];
7772
7773 // If the center is inside the polygon ...
7774 if (separation < b2_epsilon) {
7775 manifold->pointCount = 1;
7776 manifold->type = b2Manifold::e_faceA;
7777 manifold->localNormal = normals[normalIndex];
7778 manifold->localPoint = 0.5f * (v1 + v2);
7779 manifold->points[0].localPoint = circleB->m_p;
7780 manifold->points[0].id.key = 0;
7781 return;
7782 }
7783
7784 // Compute barycentric coordinates
7785 float u1 = b2Dot(cLocal - v1, v2 - v1);
7786 float u2 = b2Dot(cLocal - v2, v1 - v2);
7787 if (u1 <= 0.0f) {
7788 if (b2DistanceSquared(cLocal, v1) > radius * radius) {
7789 return;
7790 }
7791
7792 manifold->pointCount = 1;
7793 manifold->type = b2Manifold::e_faceA;
7794 manifold->localNormal = cLocal - v1;
7795 manifold->localNormal.Normalize();
7796 manifold->localPoint = v1;
7797 manifold->points[0].localPoint = circleB->m_p;
7798 manifold->points[0].id.key = 0;
7799 } else if (u2 <= 0.0f) {
7800 if (b2DistanceSquared(cLocal, v2) > radius * radius) {
7801 return;
7802 }
7803
7804 manifold->pointCount = 1;
7805 manifold->type = b2Manifold::e_faceA;
7806 manifold->localNormal = cLocal - v2;
7807 manifold->localNormal.Normalize();
7808 manifold->localPoint = v2;
7809 manifold->points[0].localPoint = circleB->m_p;
7810 manifold->points[0].id.key = 0;
7811 } else {
7812 b2Vec2 faceCenter = 0.5f * (v1 + v2);
7813 float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
7814 if (s > radius) {
7815 return;
7816 }
7817
7818 manifold->pointCount = 1;
7819 manifold->type = b2Manifold::e_faceA;
7820 manifold->localNormal = normals[vertIndex1];
7821 manifold->localPoint = faceCenter;
7822 manifold->points[0].localPoint = circleB->m_p;
7823 manifold->points[0].id.key = 0;
7824 }
7825}
7826// MIT License
7827
7828// Copyright (c) 2019 Erin Catto
7829
7830// Permission is hereby granted, free of charge, to any person obtaining a copy
7831// of this software and associated documentation files (the "Software"), to deal
7832// in the Software without restriction, including without limitation the rights
7833// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7834// copies of the Software, and to permit persons to whom the Software is
7835// furnished to do so, subject to the following conditions:
7836
7837// The above copyright notice and this permission notice shall be included in all
7838// copies or substantial portions of the Software.
7839
7840// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
7841// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
7842// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7843// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
7844// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
7845// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
7846// SOFTWARE.
7847
7848#include "box2d/b2_collision.h"
7849#include "box2d/b2_circle_shape.h"
7850#include "box2d/b2_edge_shape.h"
7851#include "box2d/b2_polygon_shape.h"
7852
7853
7854// Compute contact points for edge versus circle.
7855// This accounts for edge connectivity.
7856void b2CollideEdgeAndCircle(b2Manifold* manifold,
7857 const b2EdgeShape* edgeA, const b2Transform& xfA,
7858 const b2CircleShape* circleB, const b2Transform& xfB) {
7859 manifold->pointCount = 0;
7860
7861 // Compute circle in frame of edge
7862 b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
7863
7864 b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
7865 b2Vec2 e = B - A;
7866
7867 // Normal points to the right for a CCW winding
7868 b2Vec2 n(e.y, -e.x);
7869 float offset = b2Dot(n, Q - A);
7870
7871 bool oneSided = edgeA->m_oneSided;
7872 if (oneSided && offset < 0.0f) {
7873 return;
7874 }
7875
7876 // Barycentric coordinates
7877 float u = b2Dot(e, B - Q);
7878 float v = b2Dot(e, Q - A);
7879
7880 float radius = edgeA->m_radius + circleB->m_radius;
7881
7882 b2ContactFeature cf;
7883 cf.indexB = 0;
7884 cf.typeB = b2ContactFeature::e_vertex;
7885
7886 // Region A
7887 if (v <= 0.0f) {
7888 b2Vec2 P = A;
7889 b2Vec2 d = Q - P;
7890 float dd = b2Dot(d, d);
7891 if (dd > radius * radius) {
7892 return;
7893 }
7894
7895 // Is there an edge connected to A?
7896 if (edgeA->m_oneSided) {
7897 b2Vec2 A1 = edgeA->m_vertex0;
7898 b2Vec2 B1 = A;
7899 b2Vec2 e1 = B1 - A1;
7900 float u1 = b2Dot(e1, B1 - Q);
7901
7902 // Is the circle in Region AB of the previous edge?
7903 if (u1 > 0.0f) {
7904 return;
7905 }
7906 }
7907
7908 cf.indexA = 0;
7909 cf.typeA = b2ContactFeature::e_vertex;
7910 manifold->pointCount = 1;
7911 manifold->type = b2Manifold::e_circles;
7912 manifold->localNormal.SetZero();
7913 manifold->localPoint = P;
7914 manifold->points[0].id.key = 0;
7915 manifold->points[0].id.cf = cf;
7916 manifold->points[0].localPoint = circleB->m_p;
7917 return;
7918 }
7919
7920 // Region B
7921 if (u <= 0.0f) {
7922 b2Vec2 P = B;
7923 b2Vec2 d = Q - P;
7924 float dd = b2Dot(d, d);
7925 if (dd > radius * radius) {
7926 return;
7927 }
7928
7929 // Is there an edge connected to B?
7930 if (edgeA->m_oneSided) {
7931 b2Vec2 B2 = edgeA->m_vertex3;
7932 b2Vec2 A2 = B;
7933 b2Vec2 e2 = B2 - A2;
7934 float v2 = b2Dot(e2, Q - A2);
7935
7936 // Is the circle in Region AB of the next edge?
7937 if (v2 > 0.0f) {
7938 return;
7939 }
7940 }
7941
7942 cf.indexA = 1;
7943 cf.typeA = b2ContactFeature::e_vertex;
7944 manifold->pointCount = 1;
7945 manifold->type = b2Manifold::e_circles;
7946 manifold->localNormal.SetZero();
7947 manifold->localPoint = P;
7948 manifold->points[0].id.key = 0;
7949 manifold->points[0].id.cf = cf;
7950 manifold->points[0].localPoint = circleB->m_p;
7951 return;
7952 }
7953
7954 // Region AB
7955 float den = b2Dot(e, e);
7956 b2Assert(den > 0.0f);
7957 b2Vec2 P = (1.0f / den) * (u * A + v * B);
7958 b2Vec2 d = Q - P;
7959 float dd = b2Dot(d, d);
7960 if (dd > radius * radius) {
7961 return;
7962 }
7963
7964 if (offset < 0.0f) {
7965 n.Set(-n.x, -n.y);
7966 }
7967 n.Normalize();
7968
7969 cf.indexA = 0;
7970 cf.typeA = b2ContactFeature::e_face;
7971 manifold->pointCount = 1;
7972 manifold->type = b2Manifold::e_faceA;
7973 manifold->localNormal = n;
7974 manifold->localPoint = A;
7975 manifold->points[0].id.key = 0;
7976 manifold->points[0].id.cf = cf;
7977 manifold->points[0].localPoint = circleB->m_p;
7978}
7979
7980// This structure is used to keep track of the best separating axis.
7981struct b2EPAxis {
7982 enum Type {
7983 e_unknown,
7984 e_edgeA,
7985 e_edgeB
7986 };
7987
7988 b2Vec2 normal;
7989 Type type;
7990 int32 index;
7991 float separation;
7992};
7993
7994// This holds polygon B expressed in frame A.
7995struct b2TempPolygon {
7996 b2Vec2 vertices[b2_maxPolygonVertices];
7997 b2Vec2 normals[b2_maxPolygonVertices];
7998 int32 count;
7999};
8000
8001// Reference face used for clipping
8002struct b2ReferenceFace {
8003 int32 i1, i2;
8004 b2Vec2 v1, v2;
8005 b2Vec2 normal;
8006
8007 b2Vec2 sideNormal1;
8008 float sideOffset1;
8009
8010 b2Vec2 sideNormal2;
8011 float sideOffset2;
8012};
8013
8014static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1) {
8015 b2EPAxis axis;
8016 axis.type = b2EPAxis::e_edgeA;
8017 axis.index = -1;
8018 axis.separation = -FLT_MAX;
8019 axis.normal.SetZero();
8020
8021 b2Vec2 axes[2] = { normal1, -normal1 };
8022
8023 // Find axis with least overlap (min-max problem)
8024 for (int32 j = 0; j < 2; ++j) {
8025 float sj = FLT_MAX;
8026
8027 // Find deepest polygon vertex along axis j
8028 for (int32 i = 0; i < polygonB.count; ++i) {
8029 float si = b2Dot(axes[j], polygonB.vertices[i] - v1);
8030 if (si < sj) {
8031 sj = si;
8032 }
8033 }
8034
8035 if (sj > axis.separation) {
8036 axis.index = j;
8037 axis.separation = sj;
8038 axis.normal = axes[j];
8039 }
8040 }
8041
8042 return axis;
8043}
8044
8045static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2) {
8046 b2EPAxis axis;
8047 axis.type = b2EPAxis::e_unknown;
8048 axis.index = -1;
8049 axis.separation = -FLT_MAX;
8050 axis.normal.SetZero();
8051
8052 for (int32 i = 0; i < polygonB.count; ++i) {
8053 b2Vec2 n = -polygonB.normals[i];
8054
8055 float s1 = b2Dot(n, polygonB.vertices[i] - v1);
8056 float s2 = b2Dot(n, polygonB.vertices[i] - v2);
8057 float s = b2Min(s1, s2);
8058
8059 if (s > axis.separation) {
8060 axis.type = b2EPAxis::e_edgeB;
8061 axis.index = i;
8062 axis.separation = s;
8063 axis.normal = n;
8064 }
8065 }
8066
8067 return axis;
8068}
8069
8070void b2CollideEdgeAndPolygon(b2Manifold* manifold,
8071 const b2EdgeShape* edgeA, const b2Transform& xfA,
8072 const b2PolygonShape* polygonB, const b2Transform& xfB) {
8073 manifold->pointCount = 0;
8074
8075 b2Transform xf = b2MulT(xfA, xfB);
8076
8077 b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid);
8078
8079 b2Vec2 v1 = edgeA->m_vertex1;
8080 b2Vec2 v2 = edgeA->m_vertex2;
8081
8082 b2Vec2 edge1 = v2 - v1;
8083 edge1.Normalize();
8084
8085 // Normal points to the right for a CCW winding
8086 b2Vec2 normal1(edge1.y, -edge1.x);
8087 float offset1 = b2Dot(normal1, centroidB - v1);
8088
8089 bool oneSided = edgeA->m_oneSided;
8090 if (oneSided && offset1 < 0.0f) {
8091 return;
8092 }
8093
8094 // Get polygonB in frameA
8095 b2TempPolygon tempPolygonB;
8096 tempPolygonB.count = polygonB->m_count;
8097 for (int32 i = 0; i < polygonB->m_count; ++i) {
8098 tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]);
8099 tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]);
8100 }
8101
8102 float radius = polygonB->m_radius + edgeA->m_radius;
8103
8104 b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1);
8105 if (edgeAxis.separation > radius) {
8106 return;
8107 }
8108
8109 b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2);
8110 if (polygonAxis.separation > radius) {
8111 return;
8112 }
8113
8114 // Use hysteresis for jitter reduction.
8115 const float k_relativeTol = 0.98f;
8116 const float k_absoluteTol = 0.001f;
8117
8118 b2EPAxis primaryAxis;
8119 if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol) {
8120 primaryAxis = polygonAxis;
8121 } else {
8122 primaryAxis = edgeAxis;
8123 }
8124
8125 if (oneSided) {
8126 // Smooth collision
8127 // See https://box2d.org/posts/2020/06/ghost-collisions/
8128
8129 b2Vec2 edge0 = v1 - edgeA->m_vertex0;
8130 edge0.Normalize();
8131 b2Vec2 normal0(edge0.y, -edge0.x);
8132 bool convex1 = b2Cross(edge0, edge1) >= 0.0f;
8133
8134 b2Vec2 edge2 = edgeA->m_vertex3 - v2;
8135 edge2.Normalize();
8136 b2Vec2 normal2(edge2.y, -edge2.x);
8137 bool convex2 = b2Cross(edge1, edge2) >= 0.0f;
8138
8139 const float sinTol = 0.1f;
8140 bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f;
8141
8142 // Check Gauss Map
8143 if (side1) {
8144 if (convex1) {
8145 if (b2Cross(primaryAxis.normal, normal0) > sinTol) {
8146 // Skip region
8147 return;
8148 }
8149
8150 // Admit region
8151 } else {
8152 // Snap region
8153 primaryAxis = edgeAxis;
8154 }
8155 } else {
8156 if (convex2) {
8157 if (b2Cross(normal2, primaryAxis.normal) > sinTol) {
8158 // Skip region
8159 return;
8160 }
8161
8162 // Admit region
8163 } else {
8164 // Snap region
8165 primaryAxis = edgeAxis;
8166 }
8167 }
8168 }
8169
8170 b2ClipVertex clipPoints[2];
8171 b2ReferenceFace ref;
8172 if (primaryAxis.type == b2EPAxis::e_edgeA) {
8173 manifold->type = b2Manifold::e_faceA;
8174
8175 // Search for the polygon normal that is most anti-parallel to the edge normal.
8176 int32 bestIndex = 0;
8177 float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]);
8178 for (int32 i = 1; i < tempPolygonB.count; ++i) {
8179 float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]);
8180 if (value < bestValue) {
8181 bestValue = value;
8182 bestIndex = i;
8183 }
8184 }
8185
8186 int32 i1 = bestIndex;
8187 int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0;
8188
8189 clipPoints[0].v = tempPolygonB.vertices[i1];
8190 clipPoints[0].id.cf.indexA = 0;
8191 clipPoints[0].id.cf.indexB = static_cast<uint8>(i1);
8192 clipPoints[0].id.cf.typeA = b2ContactFeature::e_face;
8193 clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex;
8194
8195 clipPoints[1].v = tempPolygonB.vertices[i2];
8196 clipPoints[1].id.cf.indexA = 0;
8197 clipPoints[1].id.cf.indexB = static_cast<uint8>(i2);
8198 clipPoints[1].id.cf.typeA = b2ContactFeature::e_face;
8199 clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex;
8200
8201 ref.i1 = 0;
8202 ref.i2 = 1;
8203 ref.v1 = v1;
8204 ref.v2 = v2;
8205 ref.normal = primaryAxis.normal;
8206 ref.sideNormal1 = -edge1;
8207 ref.sideNormal2 = edge1;
8208 } else {
8209 manifold->type = b2Manifold::e_faceB;
8210
8211 clipPoints[0].v = v2;
8212 clipPoints[0].id.cf.indexA = 1;
8213 clipPoints[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
8214 clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex;
8215 clipPoints[0].id.cf.typeB = b2ContactFeature::e_face;
8216
8217 clipPoints[1].v = v1;
8218 clipPoints[1].id.cf.indexA = 0;
8219 clipPoints[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
8220 clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex;
8221 clipPoints[1].id.cf.typeB = b2ContactFeature::e_face;
8222
8223 ref.i1 = primaryAxis.index;
8224 ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0;
8225 ref.v1 = tempPolygonB.vertices[ref.i1];
8226 ref.v2 = tempPolygonB.vertices[ref.i2];
8227 ref.normal = tempPolygonB.normals[ref.i1];
8228
8229 // CCW winding
8230 ref.sideNormal1.Set(ref.normal.y, -ref.normal.x);
8231 ref.sideNormal2 = -ref.sideNormal1;
8232 }
8233
8234 ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1);
8235 ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2);
8236
8237 // Clip incident edge against reference face side planes
8238 b2ClipVertex clipPoints1[2];
8239 b2ClipVertex clipPoints2[2];
8240 int32 np;
8241
8242 // Clip to side 1
8243 np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1);
8244
8245 if (np < b2_maxManifoldPoints) {
8246 return;
8247 }
8248
8249 // Clip to side 2
8250 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2);
8251
8252 if (np < b2_maxManifoldPoints) {
8253 return;
8254 }
8255
8256 // Now clipPoints2 contains the clipped points.
8257 if (primaryAxis.type == b2EPAxis::e_edgeA) {
8258 manifold->localNormal = ref.normal;
8259 manifold->localPoint = ref.v1;
8260 } else {
8261 manifold->localNormal = polygonB->m_normals[ref.i1];
8262 manifold->localPoint = polygonB->m_vertices[ref.i1];
8263 }
8264
8265 int32 pointCount = 0;
8266 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) {
8267 float separation;
8268
8269 separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1);
8270
8271 if (separation <= radius) {
8272 b2ManifoldPoint* cp = manifold->points + pointCount;
8273
8274 if (primaryAxis.type == b2EPAxis::e_edgeA) {
8275 cp->localPoint = b2MulT(xf, clipPoints2[i].v);
8276 cp->id = clipPoints2[i].id;
8277 } else {
8278 cp->localPoint = clipPoints2[i].v;
8279 cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
8280 cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
8281 cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
8282 cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
8283 }
8284
8285 ++pointCount;
8286 }
8287 }
8288
8289 manifold->pointCount = pointCount;
8290}
8291// MIT License
8292
8293// Copyright (c) 2019 Erin Catto
8294
8295// Permission is hereby granted, free of charge, to any person obtaining a copy
8296// of this software and associated documentation files (the "Software"), to deal
8297// in the Software without restriction, including without limitation the rights
8298// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8299// copies of the Software, and to permit persons to whom the Software is
8300// furnished to do so, subject to the following conditions:
8301
8302// The above copyright notice and this permission notice shall be included in all
8303// copies or substantial portions of the Software.
8304
8305// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
8306// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
8307// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
8308// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
8309// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
8310// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
8311// SOFTWARE.
8312
8313#include "box2d/b2_collision.h"
8314#include "box2d/b2_polygon_shape.h"
8315
8316// Find the max separation between poly1 and poly2 using edge normals from poly1.
8317static float b2FindMaxSeparation(int32* edgeIndex,
8318 const b2PolygonShape* poly1, const b2Transform& xf1,
8319 const b2PolygonShape* poly2, const b2Transform& xf2) {
8320 int32 count1 = poly1->m_count;
8321 int32 count2 = poly2->m_count;
8322 const b2Vec2* n1s = poly1->m_normals;
8323 const b2Vec2* v1s = poly1->m_vertices;
8324 const b2Vec2* v2s = poly2->m_vertices;
8325 b2Transform xf = b2MulT(xf2, xf1);
8326
8327 int32 bestIndex = 0;
8328 float maxSeparation = -b2_maxFloat;
8329 for (int32 i = 0; i < count1; ++i) {
8330 // Get poly1 normal in frame2.
8331 b2Vec2 n = b2Mul(xf.q, n1s[i]);
8332 b2Vec2 v1 = b2Mul(xf, v1s[i]);
8333
8334 // Find deepest point for normal i.
8335 float si = b2_maxFloat;
8336 for (int32 j = 0; j < count2; ++j) {
8337 float sij = b2Dot(n, v2s[j] - v1);
8338 if (sij < si) {
8339 si = sij;
8340 }
8341 }
8342
8343 if (si > maxSeparation) {
8344 maxSeparation = si;
8345 bestIndex = i;
8346 }
8347 }
8348
8349 *edgeIndex = bestIndex;
8350 return maxSeparation;
8351}
8352
8353static void b2FindIncidentEdge(b2ClipVertex c[2],
8354 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
8355 const b2PolygonShape* poly2, const b2Transform& xf2) {
8356 const b2Vec2* normals1 = poly1->m_normals;
8357
8358 int32 count2 = poly2->m_count;
8359 const b2Vec2* vertices2 = poly2->m_vertices;
8360 const b2Vec2* normals2 = poly2->m_normals;
8361
8362 b2Assert(0 <= edge1 && edge1 < poly1->m_count);
8363
8364 // Get the normal of the reference edge in poly2's frame.
8365 b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
8366
8367 // Find the incident edge on poly2.
8368 int32 index = 0;
8369 float minDot = b2_maxFloat;
8370 for (int32 i = 0; i < count2; ++i) {
8371 float dot = b2Dot(normal1, normals2[i]);
8372 if (dot < minDot) {
8373 minDot = dot;
8374 index = i;
8375 }
8376 }
8377
8378 // Build the clip vertices for the incident edge.
8379 int32 i1 = index;
8380 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
8381
8382 c[0].v = b2Mul(xf2, vertices2[i1]);
8383 c[0].id.cf.indexA = (uint8)edge1;
8384 c[0].id.cf.indexB = (uint8)i1;
8385 c[0].id.cf.typeA = b2ContactFeature::e_face;
8386 c[0].id.cf.typeB = b2ContactFeature::e_vertex;
8387
8388 c[1].v = b2Mul(xf2, vertices2[i2]);
8389 c[1].id.cf.indexA = (uint8)edge1;
8390 c[1].id.cf.indexB = (uint8)i2;
8391 c[1].id.cf.typeA = b2ContactFeature::e_face;
8392 c[1].id.cf.typeB = b2ContactFeature::e_vertex;
8393}
8394
8395// Find edge normal of max separation on A - return if separating axis is found
8396// Find edge normal of max separation on B - return if separation axis is found
8397// Choose reference edge as min(minA, minB)
8398// Find incident edge
8399// Clip
8400
8401// The normal points from 1 to 2
8402void b2CollidePolygons(b2Manifold* manifold,
8403 const b2PolygonShape* polyA, const b2Transform& xfA,
8404 const b2PolygonShape* polyB, const b2Transform& xfB) {
8405 manifold->pointCount = 0;
8406 float totalRadius = polyA->m_radius + polyB->m_radius;
8407
8408 int32 edgeA = 0;
8409 float separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
8410 if (separationA > totalRadius)
8411 return;
8412
8413 int32 edgeB = 0;
8414 float separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
8415 if (separationB > totalRadius)
8416 return;
8417
8418 const b2PolygonShape* poly1; // reference polygon
8419 const b2PolygonShape* poly2; // incident polygon
8420 b2Transform xf1, xf2;
8421 int32 edge1; // reference edge
8422 uint8 flip;
8423 const float k_tol = 0.1f * b2_linearSlop;
8424
8425 if (separationB > separationA + k_tol) {
8426 poly1 = polyB;
8427 poly2 = polyA;
8428 xf1 = xfB;
8429 xf2 = xfA;
8430 edge1 = edgeB;
8431 manifold->type = b2Manifold::e_faceB;
8432 flip = 1;
8433 } else {
8434 poly1 = polyA;
8435 poly2 = polyB;
8436 xf1 = xfA;
8437 xf2 = xfB;
8438 edge1 = edgeA;
8439 manifold->type = b2Manifold::e_faceA;
8440 flip = 0;
8441 }
8442
8443 b2ClipVertex incidentEdge[2];
8444 b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
8445
8446 int32 count1 = poly1->m_count;
8447 const b2Vec2* vertices1 = poly1->m_vertices;
8448
8449 int32 iv1 = edge1;
8450 int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
8451
8452 b2Vec2 v11 = vertices1[iv1];
8453 b2Vec2 v12 = vertices1[iv2];
8454
8455 b2Vec2 localTangent = v12 - v11;
8456 localTangent.Normalize();
8457
8458 b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
8459 b2Vec2 planePoint = 0.5f * (v11 + v12);
8460
8461 b2Vec2 tangent = b2Mul(xf1.q, localTangent);
8462 b2Vec2 normal = b2Cross(tangent, 1.0f);
8463
8464 v11 = b2Mul(xf1, v11);
8465 v12 = b2Mul(xf1, v12);
8466
8467 // Face offset.
8468 float frontOffset = b2Dot(normal, v11);
8469
8470 // Side offsets, extended by polytope skin thickness.
8471 float sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
8472 float sideOffset2 = b2Dot(tangent, v12) + totalRadius;
8473
8474 // Clip incident edge against extruded edge1 side edges.
8475 b2ClipVertex clipPoints1[2];
8476 b2ClipVertex clipPoints2[2];
8477 int np;
8478
8479 // Clip to box side 1
8480 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
8481
8482 if (np < 2)
8483 return;
8484
8485 // Clip to negative box side 1
8486 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
8487
8488 if (np < 2) {
8489 return;
8490 }
8491
8492 // Now clipPoints2 contains the clipped points.
8493 manifold->localNormal = localNormal;
8494 manifold->localPoint = planePoint;
8495
8496 int32 pointCount = 0;
8497 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) {
8498 float separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
8499
8500 if (separation <= totalRadius) {
8501 b2ManifoldPoint* cp = manifold->points + pointCount;
8502 cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
8503 cp->id = clipPoints2[i].id;
8504 if (flip) {
8505 // Swap features
8506 b2ContactFeature cf = cp->id.cf;
8507 cp->id.cf.indexA = cf.indexB;
8508 cp->id.cf.indexB = cf.indexA;
8509 cp->id.cf.typeA = cf.typeB;
8510 cp->id.cf.typeB = cf.typeA;
8511 }
8512 ++pointCount;
8513 }
8514 }
8515
8516 manifold->pointCount = pointCount;
8517}
8518// MIT License
8519
8520// Copyright (c) 2019 Erin Catto
8521
8522// Permission is hereby granted, free of charge, to any person obtaining a copy
8523// of this software and associated documentation files (the "Software"), to deal
8524// in the Software without restriction, including without limitation the rights
8525// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8526// copies of the Software, and to permit persons to whom the Software is
8527// furnished to do so, subject to the following conditions:
8528
8529// The above copyright notice and this permission notice shall be included in all
8530// copies or substantial portions of the Software.
8531
8532// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
8533// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
8534// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
8535// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
8536// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
8537// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
8538// SOFTWARE.
8539
8540#include "box2d/b2_collision.h"
8541#include "box2d/b2_distance.h"
8542
8543void b2WorldManifold::Initialize(const b2Manifold* manifold,
8544 const b2Transform& xfA, float radiusA,
8545 const b2Transform& xfB, float radiusB) {
8546 if (manifold->pointCount == 0) {
8547 return;
8548 }
8549
8550 switch (manifold->type) {
8551 case b2Manifold::e_circles:
8552 {
8553 normal.Set(1.0f, 0.0f);
8554 b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
8555 b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
8556 if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) {
8557 normal = pointB - pointA;
8558 normal.Normalize();
8559 }
8560
8561 b2Vec2 cA = pointA + radiusA * normal;
8562 b2Vec2 cB = pointB - radiusB * normal;
8563 points[0] = 0.5f * (cA + cB);
8564 separations[0] = b2Dot(cB - cA, normal);
8565 }
8566 break;
8567
8568 case b2Manifold::e_faceA:
8569 {
8570 normal = b2Mul(xfA.q, manifold->localNormal);
8571 b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
8572
8573 for (int32 i = 0; i < manifold->pointCount; ++i) {
8574 b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
8575 b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
8576 b2Vec2 cB = clipPoint - radiusB * normal;
8577 points[i] = 0.5f * (cA + cB);
8578 separations[i] = b2Dot(cB - cA, normal);
8579 }
8580 }
8581 break;
8582
8583 case b2Manifold::e_faceB:
8584 {
8585 normal = b2Mul(xfB.q, manifold->localNormal);
8586 b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
8587
8588 for (int32 i = 0; i < manifold->pointCount; ++i) {
8589 b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
8590 b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
8591 b2Vec2 cA = clipPoint - radiusA * normal;
8592 points[i] = 0.5f * (cA + cB);
8593 separations[i] = b2Dot(cA - cB, normal);
8594 }
8595
8596 // Ensure normal points from A to B.
8597 normal = -normal;
8598 }
8599 break;
8600 }
8601}
8602
8603void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
8604 const b2Manifold* manifold1, const b2Manifold* manifold2) {
8605 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) {
8606 state1[i] = b2_nullState;
8607 state2[i] = b2_nullState;
8608 }
8609
8610 // Detect persists and removes.
8611 for (int32 i = 0; i < manifold1->pointCount; ++i) {
8612 b2ContactID id = manifold1->points[i].id;
8613
8614 state1[i] = b2_removeState;
8615
8616 for (int32 j = 0; j < manifold2->pointCount; ++j) {
8617 if (manifold2->points[j].id.key == id.key) {
8618 state1[i] = b2_persistState;
8619 break;
8620 }
8621 }
8622 }
8623
8624 // Detect persists and adds.
8625 for (int32 i = 0; i < manifold2->pointCount; ++i) {
8626 b2ContactID id = manifold2->points[i].id;
8627
8628 state2[i] = b2_addState;
8629
8630 for (int32 j = 0; j < manifold1->pointCount; ++j) {
8631 if (manifold1->points[j].id.key == id.key) {
8632 state2[i] = b2_persistState;
8633 break;
8634 }
8635 }
8636 }
8637}
8638
8639// From Real-time Collision Detection, p179.
8640bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const {
8641 float tmin = -b2_maxFloat;
8642 float tmax = b2_maxFloat;
8643
8644 b2Vec2 p = input.p1;
8645 b2Vec2 d = input.p2 - input.p1;
8646 b2Vec2 absD = b2Abs(d);
8647
8648 b2Vec2 normal;
8649
8650 for (int32 i = 0; i < 2; ++i) {
8651 if (absD(i) < b2_epsilon) {
8652 // Parallel.
8653 if (p(i) < lowerBound(i) || upperBound(i) < p(i)) {
8654 return false;
8655 }
8656 } else {
8657 float inv_d = 1.0f / d(i);
8658 float t1 = (lowerBound(i) - p(i)) * inv_d;
8659 float t2 = (upperBound(i) - p(i)) * inv_d;
8660
8661 // Sign of the normal vector.
8662 float s = -1.0f;
8663
8664 if (t1 > t2) {
8665 b2Swap(t1, t2);
8666 s = 1.0f;
8667 }
8668
8669 // Push the min up
8670 if (t1 > tmin) {
8671 normal.SetZero();
8672 normal(i) = s;
8673 tmin = t1;
8674 }
8675
8676 // Pull the max down
8677 tmax = b2Min(tmax, t2);
8678
8679 if (tmin > tmax) {
8680 return false;
8681 }
8682 }
8683 }
8684
8685 // Does the ray start inside the box?
8686 // Does the ray intersect beyond the max fraction?
8687 if (tmin < 0.0f || input.maxFraction < tmin) {
8688 return false;
8689 }
8690
8691 // Intersection.
8692 output->fraction = tmin;
8693 output->normal = normal;
8694 return true;
8695}
8696
8697// Sutherland-Hodgman clipping.
8698int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
8699 const b2Vec2& normal, float offset, int32 vertexIndexA) {
8700 // Start with no output points
8701 int32 count = 0;
8702
8703 // Calculate the distance of end points to the line
8704 float distance0 = b2Dot(normal, vIn[0].v) - offset;
8705 float distance1 = b2Dot(normal, vIn[1].v) - offset;
8706
8707 // If the points are behind the plane
8708 if (distance0 <= 0.0f) vOut[count++] = vIn[0];
8709 if (distance1 <= 0.0f) vOut[count++] = vIn[1];
8710
8711 // If the points are on different sides of the plane
8712 if (distance0 * distance1 < 0.0f) {
8713 // Find intersection point of edge and plane
8714 float interp = distance0 / (distance0 - distance1);
8715 vOut[count].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
8716
8717 // VertexA is hitting edgeB.
8718 vOut[count].id.cf.indexA = static_cast<uint8>(vertexIndexA);
8719 vOut[count].id.cf.indexB = vIn[0].id.cf.indexB;
8720 vOut[count].id.cf.typeA = b2ContactFeature::e_vertex;
8721 vOut[count].id.cf.typeB = b2ContactFeature::e_face;
8722 ++count;
8723
8724 b2Assert(count == 2);
8725 }
8726
8727 return count;
8728}
8729
8730bool b2TestOverlap(const b2Shape* shapeA, int32 indexA,
8731 const b2Shape* shapeB, int32 indexB,
8732 const b2Transform& xfA, const b2Transform& xfB) {
8733 b2DistanceInput input;
8734 input.proxyA.Set(shapeA, indexA);
8735 input.proxyB.Set(shapeB, indexB);
8736 input.transformA = xfA;
8737 input.transformB = xfB;
8738 input.useRadii = true;
8739
8740 b2SimplexCache cache;
8741 cache.count = 0;
8742
8743 b2DistanceOutput output;
8744
8745 b2Distance(&output, &cache, &input);
8746
8747 return output.distance < 10.0f * b2_epsilon;
8748}
8749// MIT License
8750
8751// Copyright (c) 2019 Erin Catto
8752
8753// Permission is hereby granted, free of charge, to any person obtaining a copy
8754// of this software and associated documentation files (the "Software"), to deal
8755// in the Software without restriction, including without limitation the rights
8756// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8757// copies of the Software, and to permit persons to whom the Software is
8758// furnished to do so, subject to the following conditions:
8759
8760// The above copyright notice and this permission notice shall be included in all
8761// copies or substantial portions of the Software.
8762
8763// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
8764// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
8765// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
8766// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
8767// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
8768// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
8769// SOFTWARE.
8770
8771#include "box2d/b2_circle_shape.h"
8772#include "box2d/b2_distance.h"
8773#include "box2d/b2_edge_shape.h"
8774#include "box2d/b2_chain_shape.h"
8775#include "box2d/b2_polygon_shape.h"
8776
8777// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
8778B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
8779
8780void b2DistanceProxy::Set(const b2Shape* shape, int32 index) {
8781 switch (shape->GetType()) {
8782 case b2Shape::e_circle:
8783 {
8784 const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
8785 m_vertices = &circle->m_p;
8786 m_count = 1;
8787 m_radius = circle->m_radius;
8788 }
8789 break;
8790
8791 case b2Shape::e_polygon:
8792 {
8793 const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
8794 m_vertices = polygon->m_vertices;
8795 m_count = polygon->m_count;
8796 m_radius = polygon->m_radius;
8797 }
8798 break;
8799
8800 case b2Shape::e_chain:
8801 {
8802 const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
8803 b2Assert(0 <= index && index < chain->m_count);
8804
8805 m_buffer[0] = chain->m_vertices[index];
8806 if (index + 1 < chain->m_count) {
8807 m_buffer[1] = chain->m_vertices[index + 1];
8808 } else {
8809 m_buffer[1] = chain->m_vertices[0];
8810 }
8811
8812 m_vertices = m_buffer;
8813 m_count = 2;
8814 m_radius = chain->m_radius;
8815 }
8816 break;
8817
8818 case b2Shape::e_edge:
8819 {
8820 const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
8821 m_vertices = &edge->m_vertex1;
8822 m_count = 2;
8823 m_radius = edge->m_radius;
8824 }
8825 break;
8826
8827 default:
8828 b2Assert(false);
8829 }
8830}
8831
8832void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float radius) {
8833 m_vertices = vertices;
8834 m_count = count;
8835 m_radius = radius;
8836}
8837
8838struct b2SimplexVertex {
8839 b2Vec2 wA; // support point in proxyA
8840 b2Vec2 wB; // support point in proxyB
8841 b2Vec2 w; // wB - wA
8842 float a; // barycentric coordinate for closest point
8843 int32 indexA; // wA index
8844 int32 indexB; // wB index
8845};
8846
8847struct b2Simplex {
8848 void ReadCache(const b2SimplexCache* cache,
8849 const b2DistanceProxy* proxyA, const b2Transform& transformA,
8850 const b2DistanceProxy* proxyB, const b2Transform& transformB) {
8851 b2Assert(cache->count <= 3);
8852
8853 // Copy data from cache.
8854 m_count = cache->count;
8855 b2SimplexVertex* vertices = &m_v1;
8856 for (int32 i = 0; i < m_count; ++i) {
8857 b2SimplexVertex* v = vertices + i;
8858 v->indexA = cache->indexA[i];
8859 v->indexB = cache->indexB[i];
8860 b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
8861 b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
8862 v->wA = b2Mul(transformA, wALocal);
8863 v->wB = b2Mul(transformB, wBLocal);
8864 v->w = v->wB - v->wA;
8865 v->a = 0.0f;
8866 }
8867
8868 // Compute the new simplex metric, if it is substantially different than
8869 // old metric then flush the simplex.
8870 if (m_count > 1) {
8871 float metric1 = cache->metric;
8872 float metric2 = GetMetric();
8873 if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) {
8874 // Reset the simplex.
8875 m_count = 0;
8876 }
8877 }
8878
8879 // If the cache is empty or invalid ...
8880 if (m_count == 0) {
8881 b2SimplexVertex* v = vertices + 0;
8882 v->indexA = 0;
8883 v->indexB = 0;
8884 b2Vec2 wALocal = proxyA->GetVertex(0);
8885 b2Vec2 wBLocal = proxyB->GetVertex(0);
8886 v->wA = b2Mul(transformA, wALocal);
8887 v->wB = b2Mul(transformB, wBLocal);
8888 v->w = v->wB - v->wA;
8889 v->a = 1.0f;
8890 m_count = 1;
8891 }
8892 }
8893
8894 void WriteCache(b2SimplexCache* cache) const {
8895 cache->metric = GetMetric();
8896 cache->count = uint16(m_count);
8897 const b2SimplexVertex* vertices = &m_v1;
8898 for (int32 i = 0; i < m_count; ++i) {
8899 cache->indexA[i] = uint8(vertices[i].indexA);
8900 cache->indexB[i] = uint8(vertices[i].indexB);
8901 }
8902 }
8903
8904 b2Vec2 GetSearchDirection() const {
8905 switch (m_count) {
8906 case 1:
8907 return -m_v1.w;
8908
8909 case 2:
8910 {
8911 b2Vec2 e12 = m_v2.w - m_v1.w;
8912 float sgn = b2Cross(e12, -m_v1.w);
8913 if (sgn > 0.0f) {
8914 // Origin is left of e12.
8915 return b2Cross(1.0f, e12);
8916 } else {
8917 // Origin is right of e12.
8918 return b2Cross(e12, 1.0f);
8919 }
8920 }
8921
8922 default:
8923 b2Assert(false);
8924 return b2Vec2_zero;
8925 }
8926 }
8927
8928 b2Vec2 GetClosestPoint() const {
8929 switch (m_count) {
8930 case 0:
8931 b2Assert(false);
8932 return b2Vec2_zero;
8933
8934 case 1:
8935 return m_v1.w;
8936
8937 case 2:
8938 return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
8939
8940 case 3:
8941 return b2Vec2_zero;
8942
8943 default:
8944 b2Assert(false);
8945 return b2Vec2_zero;
8946 }
8947 }
8948
8949 void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const {
8950 switch (m_count) {
8951 case 0:
8952 b2Assert(false);
8953 break;
8954
8955 case 1:
8956 *pA = m_v1.wA;
8957 *pB = m_v1.wB;
8958 break;
8959
8960 case 2:
8961 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
8962 *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
8963 break;
8964
8965 case 3:
8966 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
8967 *pB = *pA;
8968 break;
8969
8970 default:
8971 b2Assert(false);
8972 break;
8973 }
8974 }
8975
8976 float GetMetric() const {
8977 switch (m_count) {
8978 case 0:
8979 b2Assert(false);
8980 return 0.0f;
8981
8982 case 1:
8983 return 0.0f;
8984
8985 case 2:
8986 return b2Distance(m_v1.w, m_v2.w);
8987
8988 case 3:
8989 return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
8990
8991 default:
8992 b2Assert(false);
8993 return 0.0f;
8994 }
8995 }
8996
8997 void Solve2();
8998 void Solve3();
8999
9000 b2SimplexVertex m_v1, m_v2, m_v3;
9001 int32 m_count;
9002};
9003
9004
9005// Solve a line segment using barycentric coordinates.
9006//
9007// p = a1 * w1 + a2 * w2
9008// a1 + a2 = 1
9009//
9010// The vector from the origin to the closest point on the line is
9011// perpendicular to the line.
9012// e12 = w2 - w1
9013// dot(p, e) = 0
9014// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
9015//
9016// 2-by-2 linear system
9017// [1 1 ][a1] = [1]
9018// [w1.e12 w2.e12][a2] = [0]
9019//
9020// Define
9021// d12_1 = dot(w2, e12)
9022// d12_2 = -dot(w1, e12)
9023// d12 = d12_1 + d12_2
9024//
9025// Solution
9026// a1 = d12_1 / d12
9027// a2 = d12_2 / d12
9028void b2Simplex::Solve2() {
9029 b2Vec2 w1 = m_v1.w;
9030 b2Vec2 w2 = m_v2.w;
9031 b2Vec2 e12 = w2 - w1;
9032
9033 // w1 region
9034 float d12_2 = -b2Dot(w1, e12);
9035 if (d12_2 <= 0.0f) {
9036 // a2 <= 0, so we clamp it to 0
9037 m_v1.a = 1.0f;
9038 m_count = 1;
9039 return;
9040 }
9041
9042 // w2 region
9043 float d12_1 = b2Dot(w2, e12);
9044 if (d12_1 <= 0.0f) {
9045 // a1 <= 0, so we clamp it to 0
9046 m_v2.a = 1.0f;
9047 m_count = 1;
9048 m_v1 = m_v2;
9049 return;
9050 }
9051
9052 // Must be in e12 region.
9053 float inv_d12 = 1.0f / (d12_1 + d12_2);
9054 m_v1.a = d12_1 * inv_d12;
9055 m_v2.a = d12_2 * inv_d12;
9056 m_count = 2;
9057}
9058
9059// Possible regions:
9060// - points[2]
9061// - edge points[0]-points[2]
9062// - edge points[1]-points[2]
9063// - inside the triangle
9064void b2Simplex::Solve3() {
9065 b2Vec2 w1 = m_v1.w;
9066 b2Vec2 w2 = m_v2.w;
9067 b2Vec2 w3 = m_v3.w;
9068
9069 // Edge12
9070 // [1 1 ][a1] = [1]
9071 // [w1.e12 w2.e12][a2] = [0]
9072 // a3 = 0
9073 b2Vec2 e12 = w2 - w1;
9074 float w1e12 = b2Dot(w1, e12);
9075 float w2e12 = b2Dot(w2, e12);
9076 float d12_1 = w2e12;
9077 float d12_2 = -w1e12;
9078
9079 // Edge13
9080 // [1 1 ][a1] = [1]
9081 // [w1.e13 w3.e13][a3] = [0]
9082 // a2 = 0
9083 b2Vec2 e13 = w3 - w1;
9084 float w1e13 = b2Dot(w1, e13);
9085 float w3e13 = b2Dot(w3, e13);
9086 float d13_1 = w3e13;
9087 float d13_2 = -w1e13;
9088
9089 // Edge23
9090 // [1 1 ][a2] = [1]
9091 // [w2.e23 w3.e23][a3] = [0]
9092 // a1 = 0
9093 b2Vec2 e23 = w3 - w2;
9094 float w2e23 = b2Dot(w2, e23);
9095 float w3e23 = b2Dot(w3, e23);
9096 float d23_1 = w3e23;
9097 float d23_2 = -w2e23;
9098
9099 // Triangle123
9100 float n123 = b2Cross(e12, e13);
9101
9102 float d123_1 = n123 * b2Cross(w2, w3);
9103 float d123_2 = n123 * b2Cross(w3, w1);
9104 float d123_3 = n123 * b2Cross(w1, w2);
9105
9106 // w1 region
9107 if (d12_2 <= 0.0f && d13_2 <= 0.0f) {
9108 m_v1.a = 1.0f;
9109 m_count = 1;
9110 return;
9111 }
9112
9113 // e12
9114 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) {
9115 float inv_d12 = 1.0f / (d12_1 + d12_2);
9116 m_v1.a = d12_1 * inv_d12;
9117 m_v2.a = d12_2 * inv_d12;
9118 m_count = 2;
9119 return;
9120 }
9121
9122 // e13
9123 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) {
9124 float inv_d13 = 1.0f / (d13_1 + d13_2);
9125 m_v1.a = d13_1 * inv_d13;
9126 m_v3.a = d13_2 * inv_d13;
9127 m_count = 2;
9128 m_v2 = m_v3;
9129 return;
9130 }
9131
9132 // w2 region
9133 if (d12_1 <= 0.0f && d23_2 <= 0.0f) {
9134 m_v2.a = 1.0f;
9135 m_count = 1;
9136 m_v1 = m_v2;
9137 return;
9138 }
9139
9140 // w3 region
9141 if (d13_1 <= 0.0f && d23_1 <= 0.0f) {
9142 m_v3.a = 1.0f;
9143 m_count = 1;
9144 m_v1 = m_v3;
9145 return;
9146 }
9147
9148 // e23
9149 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) {
9150 float inv_d23 = 1.0f / (d23_1 + d23_2);
9151 m_v2.a = d23_1 * inv_d23;
9152 m_v3.a = d23_2 * inv_d23;
9153 m_count = 2;
9154 m_v1 = m_v3;
9155 return;
9156 }
9157
9158 // Must be in triangle123
9159 float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
9160 m_v1.a = d123_1 * inv_d123;
9161 m_v2.a = d123_2 * inv_d123;
9162 m_v3.a = d123_3 * inv_d123;
9163 m_count = 3;
9164}
9165
9166void b2Distance(b2DistanceOutput* output,
9167 b2SimplexCache* cache,
9168 const b2DistanceInput* input) {
9169 ++b2_gjkCalls;
9170
9171 const b2DistanceProxy* proxyA = &input->proxyA;
9172 const b2DistanceProxy* proxyB = &input->proxyB;
9173
9174 b2Transform transformA = input->transformA;
9175 b2Transform transformB = input->transformB;
9176
9177 // Initialize the simplex.
9178 b2Simplex simplex;
9179 simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
9180
9181 // Get simplex vertices as an array.
9182 b2SimplexVertex* vertices = &simplex.m_v1;
9183 const int32 k_maxIters = 20;
9184
9185 // These store the vertices of the last simplex so that we
9186 // can check for duplicates and prevent cycling.
9187 int32 saveA[3], saveB[3];
9188 int32 saveCount = 0;
9189
9190 // Main iteration loop.
9191 int32 iter = 0;
9192 while (iter < k_maxIters) {
9193 // Copy simplex so we can identify duplicates.
9194 saveCount = simplex.m_count;
9195 for (int32 i = 0; i < saveCount; ++i) {
9196 saveA[i] = vertices[i].indexA;
9197 saveB[i] = vertices[i].indexB;
9198 }
9199
9200 switch (simplex.m_count) {
9201 case 1:
9202 break;
9203
9204 case 2:
9205 simplex.Solve2();
9206 break;
9207
9208 case 3:
9209 simplex.Solve3();
9210 break;
9211
9212 default:
9213 b2Assert(false);
9214 }
9215
9216 // If we have 3 points, then the origin is in the corresponding triangle.
9217 if (simplex.m_count == 3) {
9218 break;
9219 }
9220
9221 // Get search direction.
9222 b2Vec2 d = simplex.GetSearchDirection();
9223
9224 // Ensure the search direction is numerically fit.
9225 if (d.LengthSquared() < b2_epsilon * b2_epsilon) {
9226 // The origin is probably contained by a line segment
9227 // or triangle. Thus the shapes are overlapped.
9228
9229 // We can't return zero here even though there may be overlap.
9230 // In case the simplex is a point, segment, or triangle it is difficult
9231 // to determine if the origin is contained in the CSO or very close to it.
9232 break;
9233 }
9234
9235 // Compute a tentative new simplex vertex using support points.
9236 b2SimplexVertex* vertex = vertices + simplex.m_count;
9237 vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
9238 vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
9239 vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
9240 vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
9241 vertex->w = vertex->wB - vertex->wA;
9242
9243 // Iteration count is equated to the number of support point calls.
9244 ++iter;
9245 ++b2_gjkIters;
9246
9247 // Check for duplicate support points. This is the main termination criteria.
9248 bool duplicate = false;
9249 for (int32 i = 0; i < saveCount; ++i) {
9250 if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) {
9251 duplicate = true;
9252 break;
9253 }
9254 }
9255
9256 // If we found a duplicate support point we must exit to avoid cycling.
9257 if (duplicate) {
9258 break;
9259 }
9260
9261 // New vertex is ok and needed.
9262 ++simplex.m_count;
9263 }
9264
9265 b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
9266
9267 // Prepare output.
9268 simplex.GetWitnessPoints(&output->pointA, &output->pointB);
9269 output->distance = b2Distance(output->pointA, output->pointB);
9270 output->iterations = iter;
9271
9272 // Cache the simplex.
9273 simplex.WriteCache(cache);
9274
9275 // Apply radii if requested.
9276 if (input->useRadii) {
9277 float rA = proxyA->m_radius;
9278 float rB = proxyB->m_radius;
9279
9280 if (output->distance > rA + rB && output->distance > b2_epsilon) {
9281 // Shapes are still no overlapped.
9282 // Move the witness points to the outer surface.
9283 output->distance -= rA + rB;
9284 b2Vec2 normal = output->pointB - output->pointA;
9285 normal.Normalize();
9286 output->pointA += rA * normal;
9287 output->pointB -= rB * normal;
9288 } else {
9289 // Shapes are overlapped when radii are considered.
9290 // Move the witness points to the middle.
9291 b2Vec2 p = 0.5f * (output->pointA + output->pointB);
9292 output->pointA = p;
9293 output->pointB = p;
9294 output->distance = 0.0f;
9295 }
9296 }
9297}
9298
9299// GJK-raycast
9300// Algorithm by Gino van den Bergen.
9301// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
9302bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input) {
9303 output->iterations = 0;
9304 output->lambda = 1.0f;
9305 output->normal.SetZero();
9306 output->point.SetZero();
9307
9308 const b2DistanceProxy* proxyA = &input->proxyA;
9309 const b2DistanceProxy* proxyB = &input->proxyB;
9310
9311 float radiusA = b2Max(proxyA->m_radius, b2_polygonRadius);
9312 float radiusB = b2Max(proxyB->m_radius, b2_polygonRadius);
9313 float radius = radiusA + radiusB;
9314
9315 b2Transform xfA = input->transformA;
9316 b2Transform xfB = input->transformB;
9317
9318 b2Vec2 r = input->translationB;
9319 b2Vec2 n(0.0f, 0.0f);
9320 float lambda = 0.0f;
9321
9322 // Initial simplex
9323 b2Simplex simplex;
9324 simplex.m_count = 0;
9325
9326 // Get simplex vertices as an array.
9327 b2SimplexVertex* vertices = &simplex.m_v1;
9328
9329 // Get support point in -r direction
9330 int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r));
9331 b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
9332 int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r));
9333 b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
9334 b2Vec2 v = wA - wB;
9335
9336 // Sigma is the target distance between polygons
9337 float sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius);
9338 const float tolerance = 0.5f * b2_linearSlop;
9339
9340 // Main iteration loop.
9341 const int32 k_maxIters = 20;
9342 int32 iter = 0;
9343 while (iter < k_maxIters && v.Length() - sigma > tolerance) {
9344 b2Assert(simplex.m_count < 3);
9345
9346 output->iterations += 1;
9347
9348 // Support in direction -v (A - B)
9349 indexA = proxyA->GetSupport(b2MulT(xfA.q, -v));
9350 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
9351 indexB = proxyB->GetSupport(b2MulT(xfB.q, v));
9352 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
9353 b2Vec2 p = wA - wB;
9354
9355 // -v is a normal at p
9356 v.Normalize();
9357
9358 // Intersect ray with plane
9359 float vp = b2Dot(v, p);
9360 float vr = b2Dot(v, r);
9361 if (vp - sigma > lambda * vr) {
9362 if (vr <= 0.0f) {
9363 return false;
9364 }
9365
9366 lambda = (vp - sigma) / vr;
9367 if (lambda > 1.0f) {
9368 return false;
9369 }
9370
9371 n = -v;
9372 simplex.m_count = 0;
9373 }
9374
9375 // Reverse simplex since it works with B - A.
9376 // Shift by lambda * r because we want the closest point to the current clip point.
9377 // Note that the support point p is not shifted because we want the plane equation
9378 // to be formed in unshifted space.
9379 b2SimplexVertex* vertex = vertices + simplex.m_count;
9380 vertex->indexA = indexB;
9381 vertex->wA = wB + lambda * r;
9382 vertex->indexB = indexA;
9383 vertex->wB = wA;
9384 vertex->w = vertex->wB - vertex->wA;
9385 vertex->a = 1.0f;
9386 simplex.m_count += 1;
9387
9388 switch (simplex.m_count) {
9389 case 1:
9390 break;
9391
9392 case 2:
9393 simplex.Solve2();
9394 break;
9395
9396 case 3:
9397 simplex.Solve3();
9398 break;
9399
9400 default:
9401 b2Assert(false);
9402 }
9403
9404 // If we have 3 points, then the origin is in the corresponding triangle.
9405 if (simplex.m_count == 3) {
9406 // Overlap
9407 return false;
9408 }
9409
9410 // Get search direction.
9411 v = simplex.GetClosestPoint();
9412
9413 // Iteration count is equated to the number of support point calls.
9414 ++iter;
9415 }
9416
9417 if (iter == 0) {
9418 // Initial overlap
9419 return false;
9420 }
9421
9422 // Prepare output.
9423 b2Vec2 pointA, pointB;
9424 simplex.GetWitnessPoints(&pointB, &pointA);
9425
9426 if (v.LengthSquared() > 0.0f) {
9427 n = -v;
9428 n.Normalize();
9429 }
9430
9431 output->point = pointA + radiusA * n;
9432 output->normal = n;
9433 output->lambda = lambda;
9434 output->iterations = iter;
9435 return true;
9436}
9437// MIT License
9438
9439// Copyright (c) 2019 Erin Catto
9440
9441// Permission is hereby granted, free of charge, to any person obtaining a copy
9442// of this software and associated documentation files (the "Software"), to deal
9443// in the Software without restriction, including without limitation the rights
9444// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9445// copies of the Software, and to permit persons to whom the Software is
9446// furnished to do so, subject to the following conditions:
9447
9448// The above copyright notice and this permission notice shall be included in all
9449// copies or substantial portions of the Software.
9450
9451// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
9452// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
9453// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
9454// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
9455// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
9456// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
9457// SOFTWARE.
9458#include "box2d/b2_dynamic_tree.h"
9459#include <string.h>
9460
9461b2DynamicTree::b2DynamicTree() {
9462 m_root = b2_nullNode;
9463
9464 m_nodeCapacity = 16;
9465 m_nodeCount = 0;
9466 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
9467 memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
9468
9469 // Build a linked list for the free list.
9470 for (int32 i = 0; i < m_nodeCapacity - 1; ++i) {
9471 m_nodes[i].next = i + 1;
9472 m_nodes[i].height = -1;
9473 }
9474 m_nodes[m_nodeCapacity - 1].next = b2_nullNode;
9475 m_nodes[m_nodeCapacity - 1].height = -1;
9476 m_freeList = 0;
9477
9478 m_insertionCount = 0;
9479}
9480
9481b2DynamicTree::~b2DynamicTree() {
9482 // This frees the entire tree in one shot.
9483 b2Free(m_nodes);
9484}
9485
9486// Allocate a node from the pool. Grow the pool if necessary.
9487int32 b2DynamicTree::AllocateNode() {
9488 // Expand the node pool as needed.
9489 if (m_freeList == b2_nullNode) {
9490 b2Assert(m_nodeCount == m_nodeCapacity);
9491
9492 // The free list is empty. Rebuild a bigger pool.
9493 b2TreeNode* oldNodes = m_nodes;
9494 m_nodeCapacity *= 2;
9495 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
9496 memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
9497 b2Free(oldNodes);
9498
9499 // Build a linked list for the free list. The parent
9500 // pointer becomes the "next" pointer.
9501 for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) {
9502 m_nodes[i].next = i + 1;
9503 m_nodes[i].height = -1;
9504 }
9505 m_nodes[m_nodeCapacity - 1].next = b2_nullNode;
9506 m_nodes[m_nodeCapacity - 1].height = -1;
9507 m_freeList = m_nodeCount;
9508 }
9509
9510 // Peel a node off the free list.
9511 int32 nodeId = m_freeList;
9512 m_freeList = m_nodes[nodeId].next;
9513 m_nodes[nodeId].parent = b2_nullNode;
9514 m_nodes[nodeId].child1 = b2_nullNode;
9515 m_nodes[nodeId].child2 = b2_nullNode;
9516 m_nodes[nodeId].height = 0;
9517 m_nodes[nodeId].userData = nullptr;
9518 m_nodes[nodeId].moved = false;
9519 ++m_nodeCount;
9520 return nodeId;
9521}
9522
9523// Return a node to the pool.
9524void b2DynamicTree::FreeNode(int32 nodeId) {
9525 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
9526 b2Assert(0 < m_nodeCount);
9527 m_nodes[nodeId].next = m_freeList;
9528 m_nodes[nodeId].height = -1;
9529 m_freeList = nodeId;
9530 --m_nodeCount;
9531}
9532
9533// Create a proxy in the tree as a leaf node. We return the index
9534// of the node instead of a pointer so that we can grow
9535// the node pool.
9536int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) {
9537 int32 proxyId = AllocateNode();
9538
9539 // Fatten the aabb.
9540 b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
9541 m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
9542 m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
9543 m_nodes[proxyId].userData = userData;
9544 m_nodes[proxyId].height = 0;
9545 m_nodes[proxyId].moved = true;
9546
9547 InsertLeaf(proxyId);
9548
9549 return proxyId;
9550}
9551
9552void b2DynamicTree::DestroyProxy(int32 proxyId) {
9553 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
9554 b2Assert(m_nodes[proxyId].IsLeaf());
9555
9556 RemoveLeaf(proxyId);
9557 FreeNode(proxyId);
9558}
9559
9560bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) {
9561 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
9562
9563 b2Assert(m_nodes[proxyId].IsLeaf());
9564
9565 // Extend AABB
9566 b2AABB fatAABB;
9567 b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
9568 fatAABB.lowerBound = aabb.lowerBound - r;
9569 fatAABB.upperBound = aabb.upperBound + r;
9570
9571 // Predict AABB movement
9572 b2Vec2 d = b2_aabbMultiplier * displacement;
9573
9574 if (d.x < 0.0f) {
9575 fatAABB.lowerBound.x += d.x;
9576 } else {
9577 fatAABB.upperBound.x += d.x;
9578 }
9579
9580 if (d.y < 0.0f) {
9581 fatAABB.lowerBound.y += d.y;
9582 } else {
9583 fatAABB.upperBound.y += d.y;
9584 }
9585
9586 const b2AABB& treeAABB = m_nodes[proxyId].aabb;
9587 if (treeAABB.Contains(aabb)) {
9588 // The tree AABB still contains the object, but it might be too large.
9589 // Perhaps the object was moving fast but has since gone to sleep.
9590 // The huge AABB is larger than the new fat AABB.
9591 b2AABB hugeAABB;
9592 hugeAABB.lowerBound = fatAABB.lowerBound - 4.0f * r;
9593 hugeAABB.upperBound = fatAABB.upperBound + 4.0f * r;
9594
9595 if (hugeAABB.Contains(treeAABB)) {
9596 // The tree AABB contains the object AABB and the tree AABB is
9597 // not too large. No tree update needed.
9598 return false;
9599 }
9600
9601 // Otherwise the tree AABB is huge and needs to be shrunk
9602 }
9603
9604 RemoveLeaf(proxyId);
9605
9606 m_nodes[proxyId].aabb = fatAABB;
9607
9608 InsertLeaf(proxyId);
9609
9610 m_nodes[proxyId].moved = true;
9611
9612 return true;
9613}
9614
9615void b2DynamicTree::InsertLeaf(int32 leaf) {
9616 ++m_insertionCount;
9617
9618 if (m_root == b2_nullNode) {
9619 m_root = leaf;
9620 m_nodes[m_root].parent = b2_nullNode;
9621 return;
9622 }
9623
9624 // Find the best sibling for this node
9625 b2AABB leafAABB = m_nodes[leaf].aabb;
9626 int32 index = m_root;
9627 while (m_nodes[index].IsLeaf() == false) {
9628 int32 child1 = m_nodes[index].child1;
9629 int32 child2 = m_nodes[index].child2;
9630
9631 float area = m_nodes[index].aabb.GetPerimeter();
9632
9633 b2AABB combinedAABB;
9634 combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
9635 float combinedArea = combinedAABB.GetPerimeter();
9636
9637 // Cost of creating a new parent for this node and the new leaf
9638 float cost = 2.0f * combinedArea;
9639
9640 // Minimum cost of pushing the leaf further down the tree
9641 float inheritanceCost = 2.0f * (combinedArea - area);
9642
9643 // Cost of descending into child1
9644 float cost1;
9645 if (m_nodes[child1].IsLeaf()) {
9646 b2AABB aabb;
9647 aabb.Combine(leafAABB, m_nodes[child1].aabb);
9648 cost1 = aabb.GetPerimeter() + inheritanceCost;
9649 } else {
9650 b2AABB aabb;
9651 aabb.Combine(leafAABB, m_nodes[child1].aabb);
9652 float oldArea = m_nodes[child1].aabb.GetPerimeter();
9653 float newArea = aabb.GetPerimeter();
9654 cost1 = (newArea - oldArea) + inheritanceCost;
9655 }
9656
9657 // Cost of descending into child2
9658 float cost2;
9659 if (m_nodes[child2].IsLeaf()) {
9660 b2AABB aabb;
9661 aabb.Combine(leafAABB, m_nodes[child2].aabb);
9662 cost2 = aabb.GetPerimeter() + inheritanceCost;
9663 } else {
9664 b2AABB aabb;
9665 aabb.Combine(leafAABB, m_nodes[child2].aabb);
9666 float oldArea = m_nodes[child2].aabb.GetPerimeter();
9667 float newArea = aabb.GetPerimeter();
9668 cost2 = newArea - oldArea + inheritanceCost;
9669 }
9670
9671 // Descend according to the minimum cost.
9672 if (cost < cost1 && cost < cost2) {
9673 break;
9674 }
9675
9676 // Descend
9677 if (cost1 < cost2) {
9678 index = child1;
9679 } else {
9680 index = child2;
9681 }
9682 }
9683
9684 int32 sibling = index;
9685
9686 // Create a new parent.
9687 int32 oldParent = m_nodes[sibling].parent;
9688 int32 newParent = AllocateNode();
9689 m_nodes[newParent].parent = oldParent;
9690 m_nodes[newParent].userData = nullptr;
9691 m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
9692 m_nodes[newParent].height = m_nodes[sibling].height + 1;
9693
9694 if (oldParent != b2_nullNode) {
9695 // The sibling was not the root.
9696 if (m_nodes[oldParent].child1 == sibling) {
9697 m_nodes[oldParent].child1 = newParent;
9698 } else {
9699 m_nodes[oldParent].child2 = newParent;
9700 }
9701
9702 m_nodes[newParent].child1 = sibling;
9703 m_nodes[newParent].child2 = leaf;
9704 m_nodes[sibling].parent = newParent;
9705 m_nodes[leaf].parent = newParent;
9706 } else {
9707 // The sibling was the root.
9708 m_nodes[newParent].child1 = sibling;
9709 m_nodes[newParent].child2 = leaf;
9710 m_nodes[sibling].parent = newParent;
9711 m_nodes[leaf].parent = newParent;
9712 m_root = newParent;
9713 }
9714
9715 // Walk back up the tree fixing heights and AABBs
9716 index = m_nodes[leaf].parent;
9717 while (index != b2_nullNode) {
9718 index = Balance(index);
9719
9720 int32 child1 = m_nodes[index].child1;
9721 int32 child2 = m_nodes[index].child2;
9722
9723 b2Assert(child1 != b2_nullNode);
9724 b2Assert(child2 != b2_nullNode);
9725
9726 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
9727 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
9728
9729 index = m_nodes[index].parent;
9730 }
9731
9732 //Validate();
9733}
9734
9735void b2DynamicTree::RemoveLeaf(int32 leaf) {
9736 if (leaf == m_root) {
9737 m_root = b2_nullNode;
9738 return;
9739 }
9740
9741 int32 parent = m_nodes[leaf].parent;
9742 int32 grandParent = m_nodes[parent].parent;
9743 int32 sibling;
9744 if (m_nodes[parent].child1 == leaf) {
9745 sibling = m_nodes[parent].child2;
9746 } else {
9747 sibling = m_nodes[parent].child1;
9748 }
9749
9750 if (grandParent != b2_nullNode) {
9751 // Destroy parent and connect sibling to grandParent.
9752 if (m_nodes[grandParent].child1 == parent) {
9753 m_nodes[grandParent].child1 = sibling;
9754 } else {
9755 m_nodes[grandParent].child2 = sibling;
9756 }
9757 m_nodes[sibling].parent = grandParent;
9758 FreeNode(parent);
9759
9760 // Adjust ancestor bounds.
9761 int32 index = grandParent;
9762 while (index != b2_nullNode) {
9763 index = Balance(index);
9764
9765 int32 child1 = m_nodes[index].child1;
9766 int32 child2 = m_nodes[index].child2;
9767
9768 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
9769 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
9770
9771 index = m_nodes[index].parent;
9772 }
9773 } else {
9774 m_root = sibling;
9775 m_nodes[sibling].parent = b2_nullNode;
9776 FreeNode(parent);
9777 }
9778
9779 //Validate();
9780}
9781
9782// Perform a left or right rotation if node A is imbalanced.
9783// Returns the new root index.
9784int32 b2DynamicTree::Balance(int32 iA) {
9785 b2Assert(iA != b2_nullNode);
9786
9787 b2TreeNode* A = m_nodes + iA;
9788 if (A->IsLeaf() || A->height < 2) {
9789 return iA;
9790 }
9791
9792 int32 iB = A->child1;
9793 int32 iC = A->child2;
9794 b2Assert(0 <= iB && iB < m_nodeCapacity);
9795 b2Assert(0 <= iC && iC < m_nodeCapacity);
9796
9797 b2TreeNode* B = m_nodes + iB;
9798 b2TreeNode* C = m_nodes + iC;
9799
9800 int32 balance = C->height - B->height;
9801
9802 // Rotate C up
9803 if (balance > 1) {
9804 int32 iF = C->child1;
9805 int32 iG = C->child2;
9806 b2TreeNode* F = m_nodes + iF;
9807 b2TreeNode* G = m_nodes + iG;
9808 b2Assert(0 <= iF && iF < m_nodeCapacity);
9809 b2Assert(0 <= iG && iG < m_nodeCapacity);
9810
9811 // Swap A and C
9812 C->child1 = iA;
9813 C->parent = A->parent;
9814 A->parent = iC;
9815
9816 // A's old parent should point to C
9817 if (C->parent != b2_nullNode) {
9818 if (m_nodes[C->parent].child1 == iA) {
9819 m_nodes[C->parent].child1 = iC;
9820 } else {
9821 b2Assert(m_nodes[C->parent].child2 == iA);
9822 m_nodes[C->parent].child2 = iC;
9823 }
9824 } else {
9825 m_root = iC;
9826 }
9827
9828 // Rotate
9829 if (F->height > G->height) {
9830 C->child2 = iF;
9831 A->child2 = iG;
9832 G->parent = iA;
9833 A->aabb.Combine(B->aabb, G->aabb);
9834 C->aabb.Combine(A->aabb, F->aabb);
9835
9836 A->height = 1 + b2Max(B->height, G->height);
9837 C->height = 1 + b2Max(A->height, F->height);
9838 } else {
9839 C->child2 = iG;
9840 A->child2 = iF;
9841 F->parent = iA;
9842 A->aabb.Combine(B->aabb, F->aabb);
9843 C->aabb.Combine(A->aabb, G->aabb);
9844
9845 A->height = 1 + b2Max(B->height, F->height);
9846 C->height = 1 + b2Max(A->height, G->height);
9847 }
9848
9849 return iC;
9850 }
9851
9852 // Rotate B up
9853 if (balance < -1) {
9854 int32 iD = B->child1;
9855 int32 iE = B->child2;
9856 b2TreeNode* D = m_nodes + iD;
9857 b2TreeNode* E = m_nodes + iE;
9858 b2Assert(0 <= iD && iD < m_nodeCapacity);
9859 b2Assert(0 <= iE && iE < m_nodeCapacity);
9860
9861 // Swap A and B
9862 B->child1 = iA;
9863 B->parent = A->parent;
9864 A->parent = iB;
9865
9866 // A's old parent should point to B
9867 if (B->parent != b2_nullNode) {
9868 if (m_nodes[B->parent].child1 == iA) {
9869 m_nodes[B->parent].child1 = iB;
9870 } else {
9871 b2Assert(m_nodes[B->parent].child2 == iA);
9872 m_nodes[B->parent].child2 = iB;
9873 }
9874 } else {
9875 m_root = iB;
9876 }
9877
9878 // Rotate
9879 if (D->height > E->height) {
9880 B->child2 = iD;
9881 A->child1 = iE;
9882 E->parent = iA;
9883 A->aabb.Combine(C->aabb, E->aabb);
9884 B->aabb.Combine(A->aabb, D->aabb);
9885
9886 A->height = 1 + b2Max(C->height, E->height);
9887 B->height = 1 + b2Max(A->height, D->height);
9888 } else {
9889 B->child2 = iE;
9890 A->child1 = iD;
9891 D->parent = iA;
9892 A->aabb.Combine(C->aabb, D->aabb);
9893 B->aabb.Combine(A->aabb, E->aabb);
9894
9895 A->height = 1 + b2Max(C->height, D->height);
9896 B->height = 1 + b2Max(A->height, E->height);
9897 }
9898
9899 return iB;
9900 }
9901
9902 return iA;
9903}
9904
9905int32 b2DynamicTree::GetHeight() const {
9906 if (m_root == b2_nullNode) {
9907 return 0;
9908 }
9909
9910 return m_nodes[m_root].height;
9911}
9912
9913//
9914float b2DynamicTree::GetAreaRatio() const {
9915 if (m_root == b2_nullNode) {
9916 return 0.0f;
9917 }
9918
9919 const b2TreeNode* root = m_nodes + m_root;
9920 float rootArea = root->aabb.GetPerimeter();
9921
9922 float totalArea = 0.0f;
9923 for (int32 i = 0; i < m_nodeCapacity; ++i) {
9924 const b2TreeNode* node = m_nodes + i;
9925 if (node->height < 0) {
9926 // Free node in pool
9927 continue;
9928 }
9929
9930 totalArea += node->aabb.GetPerimeter();
9931 }
9932
9933 return totalArea / rootArea;
9934}
9935
9936// Compute the height of a sub-tree.
9937int32 b2DynamicTree::ComputeHeight(int32 nodeId) const {
9938 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
9939 b2TreeNode* node = m_nodes + nodeId;
9940
9941 if (node->IsLeaf()) {
9942 return 0;
9943 }
9944
9945 int32 height1 = ComputeHeight(node->child1);
9946 int32 height2 = ComputeHeight(node->child2);
9947 return 1 + b2Max(height1, height2);
9948}
9949
9950int32 b2DynamicTree::ComputeHeight() const {
9951 int32 height = ComputeHeight(m_root);
9952 return height;
9953}
9954
9955void b2DynamicTree::ValidateStructure(int32 index) const {
9956 if (index == b2_nullNode) {
9957 return;
9958 }
9959
9960 if (index == m_root) {
9961 b2Assert(m_nodes[index].parent == b2_nullNode);
9962 }
9963
9964 const b2TreeNode* node = m_nodes + index;
9965
9966 int32 child1 = node->child1;
9967 int32 child2 = node->child2;
9968
9969 if (node->IsLeaf()) {
9970 b2Assert(child1 == b2_nullNode);
9971 b2Assert(child2 == b2_nullNode);
9972 b2Assert(node->height == 0);
9973 return;
9974 }
9975
9976 b2Assert(0 <= child1 && child1 < m_nodeCapacity);
9977 b2Assert(0 <= child2 && child2 < m_nodeCapacity);
9978
9979 b2Assert(m_nodes[child1].parent == index);
9980 b2Assert(m_nodes[child2].parent == index);
9981
9982 ValidateStructure(child1);
9983 ValidateStructure(child2);
9984}
9985
9986void b2DynamicTree::ValidateMetrics(int32 index) const {
9987 if (index == b2_nullNode) {
9988 return;
9989 }
9990
9991 const b2TreeNode* node = m_nodes + index;
9992
9993 int32 child1 = node->child1;
9994 int32 child2 = node->child2;
9995
9996 if (node->IsLeaf()) {
9997 b2Assert(child1 == b2_nullNode);
9998 b2Assert(child2 == b2_nullNode);
9999 b2Assert(node->height == 0);
10000 return;
10001 }
10002
10003 b2Assert(0 <= child1 && child1 < m_nodeCapacity);
10004 b2Assert(0 <= child2 && child2 < m_nodeCapacity);
10005
10006 int32 height1 = m_nodes[child1].height;
10007 int32 height2 = m_nodes[child2].height;
10008 int32 height;
10009 height = 1 + b2Max(height1, height2);
10010 b2Assert(node->height == height);
10011
10012 b2AABB aabb;
10013 aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
10014
10015 b2Assert(aabb.lowerBound == node->aabb.lowerBound);
10016 b2Assert(aabb.upperBound == node->aabb.upperBound);
10017
10018 ValidateMetrics(child1);
10019 ValidateMetrics(child2);
10020}
10021
10022void b2DynamicTree::Validate() const {
10023#if defined(b2DEBUG)
10024 ValidateStructure(m_root);
10025 ValidateMetrics(m_root);
10026
10027 int32 freeCount = 0;
10028 int32 freeIndex = m_freeList;
10029 while (freeIndex != b2_nullNode) {
10030 b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
10031 freeIndex = m_nodes[freeIndex].next;
10032 ++freeCount;
10033 }
10034
10035 b2Assert(GetHeight() == ComputeHeight());
10036
10037 b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
10038#endif
10039}
10040
10041int32 b2DynamicTree::GetMaxBalance() const {
10042 int32 maxBalance = 0;
10043 for (int32 i = 0; i < m_nodeCapacity; ++i) {
10044 const b2TreeNode* node = m_nodes + i;
10045 if (node->height <= 1) {
10046 continue;
10047 }
10048
10049 b2Assert(node->IsLeaf() == false);
10050
10051 int32 child1 = node->child1;
10052 int32 child2 = node->child2;
10053 int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
10054 maxBalance = b2Max(maxBalance, balance);
10055 }
10056
10057 return maxBalance;
10058}
10059
10060void b2DynamicTree::RebuildBottomUp() {
10061 int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
10062 int32 count = 0;
10063
10064 // Build array of leaves. Free the rest.
10065 for (int32 i = 0; i < m_nodeCapacity; ++i) {
10066 if (m_nodes[i].height < 0) {
10067 // free node in pool
10068 continue;
10069 }
10070
10071 if (m_nodes[i].IsLeaf()) {
10072 m_nodes[i].parent = b2_nullNode;
10073 nodes[count] = i;
10074 ++count;
10075 } else {
10076 FreeNode(i);
10077 }
10078 }
10079
10080 while (count > 1) {
10081 float minCost = b2_maxFloat;
10082 int32 iMin = -1, jMin = -1;
10083 for (int32 i = 0; i < count; ++i) {
10084 b2AABB aabbi = m_nodes[nodes[i]].aabb;
10085
10086 for (int32 j = i + 1; j < count; ++j) {
10087 b2AABB aabbj = m_nodes[nodes[j]].aabb;
10088 b2AABB b;
10089 b.Combine(aabbi, aabbj);
10090 float cost = b.GetPerimeter();
10091 if (cost < minCost) {
10092 iMin = i;
10093 jMin = j;
10094 minCost = cost;
10095 }
10096 }
10097 }
10098
10099 int32 index1 = nodes[iMin];
10100 int32 index2 = nodes[jMin];
10101 b2TreeNode* child1 = m_nodes + index1;
10102 b2TreeNode* child2 = m_nodes + index2;
10103
10104 int32 parentIndex = AllocateNode();
10105 b2TreeNode* parent = m_nodes + parentIndex;
10106 parent->child1 = index1;
10107 parent->child2 = index2;
10108 parent->height = 1 + b2Max(child1->height, child2->height);
10109 parent->aabb.Combine(child1->aabb, child2->aabb);
10110 parent->parent = b2_nullNode;
10111
10112 child1->parent = parentIndex;
10113 child2->parent = parentIndex;
10114
10115 nodes[jMin] = nodes[count - 1];
10116 nodes[iMin] = parentIndex;
10117 --count;
10118 }
10119
10120 m_root = nodes[0];
10121 b2Free(nodes);
10122
10123 Validate();
10124}
10125
10126void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) {
10127 // Build array of leaves. Free the rest.
10128 for (int32 i = 0; i < m_nodeCapacity; ++i) {
10129 m_nodes[i].aabb.lowerBound -= newOrigin;
10130 m_nodes[i].aabb.upperBound -= newOrigin;
10131 }
10132}
10133// MIT License
10134
10135// Copyright (c) 2019 Erin Catto
10136
10137// Permission is hereby granted, free of charge, to any person obtaining a copy
10138// of this software and associated documentation files (the "Software"), to deal
10139// in the Software without restriction, including without limitation the rights
10140// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10141// copies of the Software, and to permit persons to whom the Software is
10142// furnished to do so, subject to the following conditions:
10143
10144// The above copyright notice and this permission notice shall be included in all
10145// copies or substantial portions of the Software.
10146
10147// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
10148// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
10149// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
10150// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
10151// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
10152// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
10153// SOFTWARE.
10154
10155#include "box2d/b2_edge_shape.h"
10156#include "box2d/b2_block_allocator.h"
10157#include <new>
10158
10159void b2EdgeShape::SetOneSided(const b2Vec2& v0, const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& v3) {
10160 m_vertex0 = v0;
10161 m_vertex1 = v1;
10162 m_vertex2 = v2;
10163 m_vertex3 = v3;
10164 m_oneSided = true;
10165}
10166
10167void b2EdgeShape::SetTwoSided(const b2Vec2& v1, const b2Vec2& v2) {
10168 m_vertex1 = v1;
10169 m_vertex2 = v2;
10170 m_oneSided = false;
10171}
10172
10173b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const {
10174 void* mem = allocator->Allocate(sizeof(b2EdgeShape));
10175 b2EdgeShape* clone = new (mem) b2EdgeShape;
10176 *clone = *this;
10177 return clone;
10178}
10179
10180int32 b2EdgeShape::GetChildCount() const {
10181 return 1;
10182}
10183
10184bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const {
10185 B2_NOT_USED(xf);
10186 B2_NOT_USED(p);
10187 return false;
10188}
10189
10190// p = p1 + t * d
10191// v = v1 + s * e
10192// p1 + t * d = v1 + s * e
10193// s * e - t * d = p1 - v1
10194bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
10195 const b2Transform& xf, int32 childIndex) const {
10196 B2_NOT_USED(childIndex);
10197
10198 // Put the ray into the edge's frame of reference.
10199 b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
10200 b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
10201 b2Vec2 d = p2 - p1;
10202
10203 b2Vec2 v1 = m_vertex1;
10204 b2Vec2 v2 = m_vertex2;
10205 b2Vec2 e = v2 - v1;
10206
10207 // Normal points to the right, looking from v1 at v2
10208 b2Vec2 normal(e.y, -e.x);
10209 normal.Normalize();
10210
10211 // q = p1 + t * d
10212 // dot(normal, q - v1) = 0
10213 // dot(normal, p1 - v1) + t * dot(normal, d) = 0
10214 float numerator = b2Dot(normal, v1 - p1);
10215 if (m_oneSided && numerator > 0.0f) {
10216 return false;
10217 }
10218
10219 float denominator = b2Dot(normal, d);
10220
10221 if (denominator == 0.0f) {
10222 return false;
10223 }
10224
10225 float t = numerator / denominator;
10226 if (t < 0.0f || input.maxFraction < t) {
10227 return false;
10228 }
10229
10230 b2Vec2 q = p1 + t * d;
10231
10232 // q = v1 + s * r
10233 // s = dot(q - v1, r) / dot(r, r)
10234 b2Vec2 r = v2 - v1;
10235 float rr = b2Dot(r, r);
10236 if (rr == 0.0f) {
10237 return false;
10238 }
10239
10240 float s = b2Dot(q - v1, r) / rr;
10241 if (s < 0.0f || 1.0f < s) {
10242 return false;
10243 }
10244
10245 output->fraction = t;
10246 if (numerator > 0.0f) {
10247 output->normal = -b2Mul(xf.q, normal);
10248 } else {
10249 output->normal = b2Mul(xf.q, normal);
10250 }
10251 return true;
10252}
10253
10254void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const {
10255 B2_NOT_USED(childIndex);
10256
10257 b2Vec2 v1 = b2Mul(xf, m_vertex1);
10258 b2Vec2 v2 = b2Mul(xf, m_vertex2);
10259
10260 b2Vec2 lower = b2Min(v1, v2);
10261 b2Vec2 upper = b2Max(v1, v2);
10262
10263 b2Vec2 r(m_radius, m_radius);
10264 aabb->lowerBound = lower - r;
10265 aabb->upperBound = upper + r;
10266}
10267
10268void b2EdgeShape::ComputeMass(b2MassData* massData, float density) const {
10269 B2_NOT_USED(density);
10270
10271 massData->mass = 0.0f;
10272 massData->center = 0.5f * (m_vertex1 + m_vertex2);
10273 massData->I = 0.0f;
10274}
10275// MIT License
10276
10277// Copyright (c) 2019 Erin Catto
10278
10279// Permission is hereby granted, free of charge, to any person obtaining a copy
10280// of this software and associated documentation files (the "Software"), to deal
10281// in the Software without restriction, including without limitation the rights
10282// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10283// copies of the Software, and to permit persons to whom the Software is
10284// furnished to do so, subject to the following conditions:
10285
10286// The above copyright notice and this permission notice shall be included in all
10287// copies or substantial portions of the Software.
10288
10289// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
10290// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
10291// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
10292// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
10293// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
10294// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
10295// SOFTWARE.
10296
10297#include "box2d/b2_polygon_shape.h"
10298#include "box2d/b2_block_allocator.h"
10299
10300#include <new>
10301
10302b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const {
10303 void* mem = allocator->Allocate(sizeof(b2PolygonShape));
10304 b2PolygonShape* clone = new (mem) b2PolygonShape;
10305 *clone = *this;
10306 return clone;
10307}
10308
10309void b2PolygonShape::SetAsBox(float hx, float hy) {
10310 m_count = 4;
10311 m_vertices[0].Set(-hx, -hy);
10312 m_vertices[1].Set(hx, -hy);
10313 m_vertices[2].Set(hx, hy);
10314 m_vertices[3].Set(-hx, hy);
10315 m_normals[0].Set(0.0f, -1.0f);
10316 m_normals[1].Set(1.0f, 0.0f);
10317 m_normals[2].Set(0.0f, 1.0f);
10318 m_normals[3].Set(-1.0f, 0.0f);
10319 m_centroid.SetZero();
10320}
10321
10322void b2PolygonShape::SetAsBox(float hx, float hy, const b2Vec2& center, float angle) {
10323 m_count = 4;
10324 m_vertices[0].Set(-hx, -hy);
10325 m_vertices[1].Set(hx, -hy);
10326 m_vertices[2].Set(hx, hy);
10327 m_vertices[3].Set(-hx, hy);
10328 m_normals[0].Set(0.0f, -1.0f);
10329 m_normals[1].Set(1.0f, 0.0f);
10330 m_normals[2].Set(0.0f, 1.0f);
10331 m_normals[3].Set(-1.0f, 0.0f);
10332 m_centroid = center;
10333
10334 b2Transform xf;
10335 xf.p = center;
10336 xf.q.Set(angle);
10337
10338 // Transform vertices and normals.
10339 for (int32 i = 0; i < m_count; ++i) {
10340 m_vertices[i] = b2Mul(xf, m_vertices[i]);
10341 m_normals[i] = b2Mul(xf.q, m_normals[i]);
10342 }
10343}
10344
10345int32 b2PolygonShape::GetChildCount() const {
10346 return 1;
10347}
10348
10349static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) {
10350 b2Assert(count >= 3);
10351
10352 b2Vec2 c(0.0f, 0.0f);
10353 float area = 0.0f;
10354
10355 // Get a reference point for forming triangles.
10356 // Use the first vertex to reduce round-off errors.
10357 b2Vec2 s = vs[0];
10358
10359 const float inv3 = 1.0f / 3.0f;
10360
10361 for (int32 i = 0; i < count; ++i) {
10362 // Triangle vertices.
10363 b2Vec2 p1 = vs[0] - s;
10364 b2Vec2 p2 = vs[i] - s;
10365 b2Vec2 p3 = i + 1 < count ? vs[i + 1] - s : vs[0] - s;
10366
10367 b2Vec2 e1 = p2 - p1;
10368 b2Vec2 e2 = p3 - p1;
10369
10370 float D = b2Cross(e1, e2);
10371
10372 float triangleArea = 0.5f * D;
10373 area += triangleArea;
10374
10375 // Area weighted centroid
10376 c += triangleArea * inv3 * (p1 + p2 + p3);
10377 }
10378
10379 // Centroid
10380 b2Assert(area > b2_epsilon);
10381 c = (1.0f / area) * c + s;
10382 return c;
10383}
10384
10385void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) {
10386 b2Assert(3 <= count && count <= b2_maxPolygonVertices);
10387 if (count < 3) {
10388 SetAsBox(1.0f, 1.0f);
10389 return;
10390 }
10391
10392 int32 n = b2Min(count, b2_maxPolygonVertices);
10393
10394 // Perform welding and copy vertices into local buffer.
10395 b2Vec2 ps[b2_maxPolygonVertices];
10396 int32 tempCount = 0;
10397 for (int32 i = 0; i < n; ++i) {
10398 b2Vec2 v = vertices[i];
10399
10400 bool unique = true;
10401 for (int32 j = 0; j < tempCount; ++j) {
10402 if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop))) {
10403 unique = false;
10404 break;
10405 }
10406 }
10407
10408 if (unique) {
10409 ps[tempCount++] = v;
10410 }
10411 }
10412
10413 n = tempCount;
10414 if (n < 3) {
10415 // Polygon is degenerate.
10416 b2Assert(false);
10417 SetAsBox(1.0f, 1.0f);
10418 return;
10419 }
10420
10421 // Create the convex hull using the Gift wrapping algorithm
10422 // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
10423
10424 // Find the right most point on the hull
10425 int32 i0 = 0;
10426 float x0 = ps[0].x;
10427 for (int32 i = 1; i < n; ++i) {
10428 float x = ps[i].x;
10429 if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) {
10430 i0 = i;
10431 x0 = x;
10432 }
10433 }
10434
10435 int32 hull[b2_maxPolygonVertices];
10436 int32 m = 0;
10437 int32 ih = i0;
10438
10439 for (;;) {
10440 b2Assert(m < b2_maxPolygonVertices);
10441 hull[m] = ih;
10442
10443 int32 ie = 0;
10444 for (int32 j = 1; j < n; ++j) {
10445 if (ie == ih) {
10446 ie = j;
10447 continue;
10448 }
10449
10450 b2Vec2 r = ps[ie] - ps[hull[m]];
10451 b2Vec2 v = ps[j] - ps[hull[m]];
10452 float c = b2Cross(r, v);
10453 if (c < 0.0f) {
10454 ie = j;
10455 }
10456
10457 // Collinearity check
10458 if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) {
10459 ie = j;
10460 }
10461 }
10462
10463 ++m;
10464 ih = ie;
10465
10466 if (ie == i0) {
10467 break;
10468 }
10469 }
10470
10471 if (m < 3) {
10472 // Polygon is degenerate.
10473 b2Assert(false);
10474 SetAsBox(1.0f, 1.0f);
10475 return;
10476 }
10477
10478 m_count = m;
10479
10480 // Copy vertices.
10481 for (int32 i = 0; i < m; ++i) {
10482 m_vertices[i] = ps[hull[i]];
10483 }
10484
10485 // Compute normals. Ensure the edges have non-zero length.
10486 for (int32 i = 0; i < m; ++i) {
10487 int32 i1 = i;
10488 int32 i2 = i + 1 < m ? i + 1 : 0;
10489 b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
10490 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
10491 m_normals[i] = b2Cross(edge, 1.0f);
10492 m_normals[i].Normalize();
10493 }
10494
10495 // Compute the polygon centroid.
10496 m_centroid = ComputeCentroid(m_vertices, m);
10497}
10498
10499bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const {
10500 b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
10501
10502 for (int32 i = 0; i < m_count; ++i) {
10503 float dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
10504 if (dot > 0.0f) {
10505 return false;
10506 }
10507 }
10508
10509 return true;
10510}
10511
10512bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
10513 const b2Transform& xf, int32 childIndex) const {
10514 B2_NOT_USED(childIndex);
10515
10516 // Put the ray into the polygon's frame of reference.
10517 b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
10518 b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
10519 b2Vec2 d = p2 - p1;
10520
10521 float lower = 0.0f, upper = input.maxFraction;
10522
10523 int32 index = -1;
10524
10525 for (int32 i = 0; i < m_count; ++i) {
10526 // p = p1 + a * d
10527 // dot(normal, p - v) = 0
10528 // dot(normal, p1 - v) + a * dot(normal, d) = 0
10529 float numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
10530 float denominator = b2Dot(m_normals[i], d);
10531
10532 if (denominator == 0.0f) {
10533 if (numerator < 0.0f) {
10534 return false;
10535 }
10536 } else {
10537 // Note: we want this predicate without division:
10538 // lower < numerator / denominator, where denominator < 0
10539 // Since denominator < 0, we have to flip the inequality:
10540 // lower < numerator / denominator <==> denominator * lower > numerator.
10541 if (denominator < 0.0f && numerator < lower * denominator) {
10542 // Increase lower.
10543 // The segment enters this half-space.
10544 lower = numerator / denominator;
10545 index = i;
10546 } else if (denominator > 0.0f && numerator < upper * denominator) {
10547 // Decrease upper.
10548 // The segment exits this half-space.
10549 upper = numerator / denominator;
10550 }
10551 }
10552
10553 // The use of epsilon here causes the assert on lower to trip
10554 // in some cases. Apparently the use of epsilon was to make edge
10555 // shapes work, but now those are handled separately.
10556 //if (upper < lower - b2_epsilon)
10557 if (upper < lower) {
10558 return false;
10559 }
10560 }
10561
10562 b2Assert(0.0f <= lower && lower <= input.maxFraction);
10563
10564 if (index >= 0) {
10565 output->fraction = lower;
10566 output->normal = b2Mul(xf.q, m_normals[index]);
10567 return true;
10568 }
10569
10570 return false;
10571}
10572
10573void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const {
10574 B2_NOT_USED(childIndex);
10575
10576 b2Vec2 lower = b2Mul(xf, m_vertices[0]);
10577 b2Vec2 upper = lower;
10578
10579 for (int32 i = 1; i < m_count; ++i) {
10580 b2Vec2 v = b2Mul(xf, m_vertices[i]);
10581 lower = b2Min(lower, v);
10582 upper = b2Max(upper, v);
10583 }
10584
10585 b2Vec2 r(m_radius, m_radius);
10586 aabb->lowerBound = lower - r;
10587 aabb->upperBound = upper + r;
10588}
10589
10590void b2PolygonShape::ComputeMass(b2MassData* massData, float density) const {
10591 // Polygon mass, centroid, and inertia.
10592 // Let rho be the polygon density in mass per unit area.
10593 // Then:
10594 // mass = rho * int(dA)
10595 // centroid.x = (1/mass) * rho * int(x * dA)
10596 // centroid.y = (1/mass) * rho * int(y * dA)
10597 // I = rho * int((x*x + y*y) * dA)
10598 //
10599 // We can compute these integrals by summing all the integrals
10600 // for each triangle of the polygon. To evaluate the integral
10601 // for a single triangle, we make a change of variables to
10602 // the (u,v) coordinates of the triangle:
10603 // x = x0 + e1x * u + e2x * v
10604 // y = y0 + e1y * u + e2y * v
10605 // where 0 <= u && 0 <= v && u + v <= 1.
10606 //
10607 // We integrate u from [0,1-v] and then v from [0,1].
10608 // We also need to use the Jacobian of the transformation:
10609 // D = cross(e1, e2)
10610 //
10611 // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
10612 //
10613 // The rest of the derivation is handled by computer algebra.
10614
10615 b2Assert(m_count >= 3);
10616
10617 b2Vec2 center(0.0f, 0.0f);
10618 float area = 0.0f;
10619 float I = 0.0f;
10620
10621 // Get a reference point for forming triangles.
10622 // Use the first vertex to reduce round-off errors.
10623 b2Vec2 s = m_vertices[0];
10624
10625 const float k_inv3 = 1.0f / 3.0f;
10626
10627 for (int32 i = 0; i < m_count; ++i) {
10628 // Triangle vertices.
10629 b2Vec2 e1 = m_vertices[i] - s;
10630 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i + 1] - s : m_vertices[0] - s;
10631
10632 float D = b2Cross(e1, e2);
10633
10634 float triangleArea = 0.5f * D;
10635 area += triangleArea;
10636
10637 // Area weighted centroid
10638 center += triangleArea * k_inv3 * (e1 + e2);
10639
10640 float ex1 = e1.x, ey1 = e1.y;
10641 float ex2 = e2.x, ey2 = e2.y;
10642
10643 float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
10644 float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
10645
10646 I += (0.25f * k_inv3 * D) * (intx2 + inty2);
10647 }
10648
10649 // Total mass
10650 massData->mass = density * area;
10651
10652 // Center of mass
10653 b2Assert(area > b2_epsilon);
10654 center *= 1.0f / area;
10655 massData->center = center + s;
10656
10657 // Inertia tensor relative to the local origin (point s).
10658 massData->I = density * I;
10659
10660 // Shift to center of mass then to original body origin.
10661 massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
10662}
10663
10664bool b2PolygonShape::Validate() const {
10665 for (int32 i = 0; i < m_count; ++i) {
10666 int32 i1 = i;
10667 int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
10668 b2Vec2 p = m_vertices[i1];
10669 b2Vec2 e = m_vertices[i2] - p;
10670
10671 for (int32 j = 0; j < m_count; ++j) {
10672 if (j == i1 || j == i2) {
10673 continue;
10674 }
10675
10676 b2Vec2 v = m_vertices[j] - p;
10677 float c = b2Cross(e, v);
10678 if (c < 0.0f) {
10679 return false;
10680 }
10681 }
10682 }
10683
10684 return true;
10685}
10686// MIT License
10687
10688// Copyright (c) 2019 Erin Catto
10689
10690// Permission is hereby granted, free of charge, to any person obtaining a copy
10691// of this software and associated documentation files (the "Software"), to deal
10692// in the Software without restriction, including without limitation the rights
10693// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10694// copies of the Software, and to permit persons to whom the Software is
10695// furnished to do so, subject to the following conditions:
10696
10697// The above copyright notice and this permission notice shall be included in all
10698// copies or substantial portions of the Software.
10699
10700// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
10701// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
10702// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
10703// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
10704// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
10705// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
10706// SOFTWARE.
10707
10708#include "box2d/b2_collision.h"
10709#include "box2d/b2_distance.h"
10710#include "box2d/b2_circle_shape.h"
10711#include "box2d/b2_polygon_shape.h"
10712#include "box2d/b2_time_of_impact.h"
10713#include "box2d/b2_timer.h"
10714
10715#include <stdio.h>
10716
10717B2_API float b2_toiTime, b2_toiMaxTime;
10718B2_API int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
10719B2_API int32 b2_toiRootIters, b2_toiMaxRootIters;
10720
10721//
10722struct b2SeparationFunction {
10723 enum Type {
10724 e_points,
10725 e_faceA,
10726 e_faceB
10727 };
10728
10729 // TODO_ERIN might not need to return the separation
10730
10731 float Initialize(const b2SimplexCache* cache,
10732 const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
10733 const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
10734 float t1) {
10735 m_proxyA = proxyA;
10736 m_proxyB = proxyB;
10737 int32 count = cache->count;
10738 b2Assert(0 < count && count < 3);
10739
10740 m_sweepA = sweepA;
10741 m_sweepB = sweepB;
10742
10743 b2Transform xfA, xfB;
10744 m_sweepA.GetTransform(&xfA, t1);
10745 m_sweepB.GetTransform(&xfB, t1);
10746
10747 if (count == 1) {
10748 m_type = e_points;
10749 b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
10750 b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
10751 b2Vec2 pointA = b2Mul(xfA, localPointA);
10752 b2Vec2 pointB = b2Mul(xfB, localPointB);
10753 m_axis = pointB - pointA;
10754 float s = m_axis.Normalize();
10755 return s;
10756 } else if (cache->indexA[0] == cache->indexA[1]) {
10757 // Two points on B and one on A.
10758 m_type = e_faceB;
10759 b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
10760 b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
10761
10762 m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
10763 m_axis.Normalize();
10764 b2Vec2 normal = b2Mul(xfB.q, m_axis);
10765
10766 m_localPoint = 0.5f * (localPointB1 + localPointB2);
10767 b2Vec2 pointB = b2Mul(xfB, m_localPoint);
10768
10769 b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
10770 b2Vec2 pointA = b2Mul(xfA, localPointA);
10771
10772 float s = b2Dot(pointA - pointB, normal);
10773 if (s < 0.0f) {
10774 m_axis = -m_axis;
10775 s = -s;
10776 }
10777 return s;
10778 } else {
10779 // Two points on A and one or two points on B.
10780 m_type = e_faceA;
10781 b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
10782 b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
10783
10784 m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
10785 m_axis.Normalize();
10786 b2Vec2 normal = b2Mul(xfA.q, m_axis);
10787
10788 m_localPoint = 0.5f * (localPointA1 + localPointA2);
10789 b2Vec2 pointA = b2Mul(xfA, m_localPoint);
10790
10791 b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
10792 b2Vec2 pointB = b2Mul(xfB, localPointB);
10793
10794 float s = b2Dot(pointB - pointA, normal);
10795 if (s < 0.0f) {
10796 m_axis = -m_axis;
10797 s = -s;
10798 }
10799 return s;
10800 }
10801 }
10802
10803 //
10804 float FindMinSeparation(int32* indexA, int32* indexB, float t) const {
10805 b2Transform xfA, xfB;
10806 m_sweepA.GetTransform(&xfA, t);
10807 m_sweepB.GetTransform(&xfB, t);
10808
10809 switch (m_type) {
10810 case e_points:
10811 {
10812 b2Vec2 axisA = b2MulT(xfA.q, m_axis);
10813 b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
10814
10815 *indexA = m_proxyA->GetSupport(axisA);
10816 *indexB = m_proxyB->GetSupport(axisB);
10817
10818 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
10819 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
10820
10821 b2Vec2 pointA = b2Mul(xfA, localPointA);
10822 b2Vec2 pointB = b2Mul(xfB, localPointB);
10823
10824 float separation = b2Dot(pointB - pointA, m_axis);
10825 return separation;
10826 }
10827
10828 case e_faceA:
10829 {
10830 b2Vec2 normal = b2Mul(xfA.q, m_axis);
10831 b2Vec2 pointA = b2Mul(xfA, m_localPoint);
10832
10833 b2Vec2 axisB = b2MulT(xfB.q, -normal);
10834
10835 *indexA = -1;
10836 *indexB = m_proxyB->GetSupport(axisB);
10837
10838 b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
10839 b2Vec2 pointB = b2Mul(xfB, localPointB);
10840
10841 float separation = b2Dot(pointB - pointA, normal);
10842 return separation;
10843 }
10844
10845 case e_faceB:
10846 {
10847 b2Vec2 normal = b2Mul(xfB.q, m_axis);
10848 b2Vec2 pointB = b2Mul(xfB, m_localPoint);
10849
10850 b2Vec2 axisA = b2MulT(xfA.q, -normal);
10851
10852 *indexB = -1;
10853 *indexA = m_proxyA->GetSupport(axisA);
10854
10855 b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
10856 b2Vec2 pointA = b2Mul(xfA, localPointA);
10857
10858 float separation = b2Dot(pointA - pointB, normal);
10859 return separation;
10860 }
10861
10862 default:
10863 b2Assert(false);
10864 *indexA = -1;
10865 *indexB = -1;
10866 return 0.0f;
10867 }
10868 }
10869
10870 //
10871 float Evaluate(int32 indexA, int32 indexB, float t) const {
10872 b2Transform xfA, xfB;
10873 m_sweepA.GetTransform(&xfA, t);
10874 m_sweepB.GetTransform(&xfB, t);
10875
10876 switch (m_type) {
10877 case e_points:
10878 {
10879 b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
10880 b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
10881
10882 b2Vec2 pointA = b2Mul(xfA, localPointA);
10883 b2Vec2 pointB = b2Mul(xfB, localPointB);
10884 float separation = b2Dot(pointB - pointA, m_axis);
10885
10886 return separation;
10887 }
10888
10889 case e_faceA:
10890 {
10891 b2Vec2 normal = b2Mul(xfA.q, m_axis);
10892 b2Vec2 pointA = b2Mul(xfA, m_localPoint);
10893
10894 b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
10895 b2Vec2 pointB = b2Mul(xfB, localPointB);
10896
10897 float separation = b2Dot(pointB - pointA, normal);
10898 return separation;
10899 }
10900
10901 case e_faceB:
10902 {
10903 b2Vec2 normal = b2Mul(xfB.q, m_axis);
10904 b2Vec2 pointB = b2Mul(xfB, m_localPoint);
10905
10906 b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
10907 b2Vec2 pointA = b2Mul(xfA, localPointA);
10908
10909 float separation = b2Dot(pointA - pointB, normal);
10910 return separation;
10911 }
10912
10913 default:
10914 b2Assert(false);
10915 return 0.0f;
10916 }
10917 }
10918
10919 const b2DistanceProxy* m_proxyA;
10920 const b2DistanceProxy* m_proxyB;
10921 b2Sweep m_sweepA, m_sweepB;
10922 Type m_type;
10923 b2Vec2 m_localPoint;
10924 b2Vec2 m_axis;
10925};
10926
10927// CCD via the local separating axis method. This seeks progression
10928// by computing the largest time at which separation is maintained.
10929void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) {
10930 b2Timer timer;
10931
10932 ++b2_toiCalls;
10933
10934 output->state = b2TOIOutput::e_unknown;
10935 output->t = input->tMax;
10936
10937 const b2DistanceProxy* proxyA = &input->proxyA;
10938 const b2DistanceProxy* proxyB = &input->proxyB;
10939
10940 b2Sweep sweepA = input->sweepA;
10941 b2Sweep sweepB = input->sweepB;
10942
10943 // Large rotations can make the root finder fail, so we normalize the
10944 // sweep angles.
10945 sweepA.Normalize();
10946 sweepB.Normalize();
10947
10948 float tMax = input->tMax;
10949
10950 float totalRadius = proxyA->m_radius + proxyB->m_radius;
10951 float target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
10952 float tolerance = 0.25f * b2_linearSlop;
10953 b2Assert(target > tolerance);
10954
10955 float t1 = 0.0f;
10956 const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
10957 int32 iter = 0;
10958
10959 // Prepare input for distance query.
10960 b2SimplexCache cache;
10961 cache.count = 0;
10962 b2DistanceInput distanceInput;
10963 distanceInput.proxyA = input->proxyA;
10964 distanceInput.proxyB = input->proxyB;
10965 distanceInput.useRadii = false;
10966
10967 // The outer loop progressively attempts to compute new separating axes.
10968 // This loop terminates when an axis is repeated (no progress is made).
10969 for (;;) {
10970 b2Transform xfA, xfB;
10971 sweepA.GetTransform(&xfA, t1);
10972 sweepB.GetTransform(&xfB, t1);
10973
10974 // Get the distance between shapes. We can also use the results
10975 // to get a separating axis.
10976 distanceInput.transformA = xfA;
10977 distanceInput.transformB = xfB;
10978 b2DistanceOutput distanceOutput;
10979 b2Distance(&distanceOutput, &cache, &distanceInput);
10980
10981 // If the shapes are overlapped, we give up on continuous collision.
10982 if (distanceOutput.distance <= 0.0f) {
10983 // Failure!
10984 output->state = b2TOIOutput::e_overlapped;
10985 output->t = 0.0f;
10986 break;
10987 }
10988
10989 if (distanceOutput.distance < target + tolerance) {
10990 // Victory!
10991 output->state = b2TOIOutput::e_touching;
10992 output->t = t1;
10993 break;
10994 }
10995
10996 // Initialize the separating axis.
10997 b2SeparationFunction fcn;
10998 fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
10999#if 0
11000 // Dump the curve seen by the root finder
11001 {
11002 const int32 N = 100;
11003 float dx = 1.0f / N;
11004 float xs[N + 1];
11005 float fs[N + 1];
11006
11007 float x = 0.0f;
11008
11009 for (int32 i = 0; i <= N; ++i) {
11010 sweepA.GetTransform(&xfA, x);
11011 sweepB.GetTransform(&xfB, x);
11012 float f = fcn.Evaluate(xfA, xfB) - target;
11013
11014 printf("%g %g\n", x, f);
11015
11016 xs[i] = x;
11017 fs[i] = f;
11018
11019 x += dx;
11020 }
11021 }
11022#endif
11023
11024 // Compute the TOI on the separating axis. We do this by successively
11025 // resolving the deepest point. This loop is bounded by the number of vertices.
11026 bool done = false;
11027 float t2 = tMax;
11028 int32 pushBackIter = 0;
11029 for (;;) {
11030 // Find the deepest point at t2. Store the witness point indices.
11031 int32 indexA, indexB;
11032 float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
11033
11034 // Is the final configuration separated?
11035 if (s2 > target + tolerance) {
11036 // Victory!
11037 output->state = b2TOIOutput::e_separated;
11038 output->t = tMax;
11039 done = true;
11040 break;
11041 }
11042
11043 // Has the separation reached tolerance?
11044 if (s2 > target - tolerance) {
11045 // Advance the sweeps
11046 t1 = t2;
11047 break;
11048 }
11049
11050 // Compute the initial separation of the witness points.
11051 float s1 = fcn.Evaluate(indexA, indexB, t1);
11052
11053 // Check for initial overlap. This might happen if the root finder
11054 // runs out of iterations.
11055 if (s1 < target - tolerance) {
11056 output->state = b2TOIOutput::e_failed;
11057 output->t = t1;
11058 done = true;
11059 break;
11060 }
11061
11062 // Check for touching
11063 if (s1 <= target + tolerance) {
11064 // Victory! t1 should hold the TOI (could be 0.0).
11065 output->state = b2TOIOutput::e_touching;
11066 output->t = t1;
11067 done = true;
11068 break;
11069 }
11070
11071 // Compute 1D root of: f(x) - target = 0
11072 int32 rootIterCount = 0;
11073 float a1 = t1, a2 = t2;
11074 for (;;) {
11075 // Use a mix of the secant rule and bisection.
11076 float t;
11077 if (rootIterCount & 1) {
11078 // Secant rule to improve convergence.
11079 t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
11080 } else {
11081 // Bisection to guarantee progress.
11082 t = 0.5f * (a1 + a2);
11083 }
11084
11085 ++rootIterCount;
11086 ++b2_toiRootIters;
11087
11088 float s = fcn.Evaluate(indexA, indexB, t);
11089
11090 if (b2Abs(s - target) < tolerance) {
11091 // t2 holds a tentative value for t1
11092 t2 = t;
11093 break;
11094 }
11095
11096 // Ensure we continue to bracket the root.
11097 if (s > target) {
11098 a1 = t;
11099 s1 = s;
11100 } else {
11101 a2 = t;
11102 s2 = s;
11103 }
11104
11105 if (rootIterCount == 50) {
11106 break;
11107 }
11108 }
11109
11110 b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
11111
11112 ++pushBackIter;
11113
11114 if (pushBackIter == b2_maxPolygonVertices) {
11115 break;
11116 }
11117 }
11118
11119 ++iter;
11120 ++b2_toiIters;
11121
11122 if (done) {
11123 break;
11124 }
11125
11126 if (iter == k_maxIterations) {
11127 // Root finder got stuck. Semi-victory.
11128 output->state = b2TOIOutput::e_failed;
11129 output->t = t1;
11130 break;
11131 }
11132 }
11133
11134 b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
11135
11136 float time = timer.GetMilliseconds();
11137 b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
11138 b2_toiTime += time;
11139}
11140// MIT License
11141
11142// Copyright (c) 2019 Erin Catto
11143
11144// Permission is hereby granted, free of charge, to any person obtaining a copy
11145// of this software and associated documentation files (the "Software"), to deal
11146// in the Software without restriction, including without limitation the rights
11147// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11148// copies of the Software, and to permit persons to whom the Software is
11149// furnished to do so, subject to the following conditions:
11150
11151// The above copyright notice and this permission notice shall be included in all
11152// copies or substantial portions of the Software.
11153
11154// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11155// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11156// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11157// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11158// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11159// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11160// SOFTWARE.
11161
11162#include "box2d/b2_block_allocator.h"
11163#include <limits.h>
11164#include <string.h>
11165#include <stddef.h>
11166
11167static const int32 b2_chunkSize = 16 * 1024;
11168static const int32 b2_maxBlockSize = 640;
11169static const int32 b2_chunkArrayIncrement = 128;
11170
11171// These are the supported object sizes. Actual allocations are rounded up the next size.
11172static const int32 b2_blockSizes[b2_blockSizeCount] =
11173{
11174 16, // 0
11175 32, // 1
11176 64, // 2
11177 96, // 3
11178 128, // 4
11179 160, // 5
11180 192, // 6
11181 224, // 7
11182 256, // 8
11183 320, // 9
11184 384, // 10
11185 448, // 11
11186 512, // 12
11187 640, // 13
11188};
11189
11190// This maps an arbitrary allocation size to a suitable slot in b2_blockSizes.
11191struct b2SizeMap {
11192 b2SizeMap() {
11193 int32 j = 0;
11194 values[0] = 0;
11195 for (int32 i = 1; i <= b2_maxBlockSize; ++i) {
11196 b2Assert(j < b2_blockSizeCount);
11197 if (i <= b2_blockSizes[j]) {
11198 values[i] = (uint8)j;
11199 } else {
11200 ++j;
11201 values[i] = (uint8)j;
11202 }
11203 }
11204 }
11205
11206 uint8 values[b2_maxBlockSize + 1];
11207};
11208
11209static const b2SizeMap b2_sizeMap;
11210
11211struct b2Chunk {
11212 int32 blockSize;
11213 b2Block* blocks;
11214};
11215
11216struct b2Block {
11217 b2Block* next;
11218};
11219
11220b2BlockAllocator::b2BlockAllocator() {
11221 b2Assert(b2_blockSizeCount < UCHAR_MAX);
11222
11223 m_chunkSpace = b2_chunkArrayIncrement;
11224 m_chunkCount = 0;
11225 m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
11226
11227 memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
11228 memset(m_freeLists, 0, sizeof(m_freeLists));
11229}
11230
11231b2BlockAllocator::~b2BlockAllocator() {
11232 for (int32 i = 0; i < m_chunkCount; ++i) {
11233 b2Free(m_chunks[i].blocks);
11234 }
11235
11236 b2Free(m_chunks);
11237}
11238
11239void* b2BlockAllocator::Allocate(int32 size) {
11240 if (size == 0) {
11241 return nullptr;
11242 }
11243
11244 b2Assert(0 < size);
11245
11246 if (size > b2_maxBlockSize) {
11247 return b2Alloc(size);
11248 }
11249
11250 int32 index = b2_sizeMap.values[size];
11251 b2Assert(0 <= index && index < b2_blockSizeCount);
11252
11253 if (m_freeLists[index]) {
11254 b2Block* block = m_freeLists[index];
11255 m_freeLists[index] = block->next;
11256 return block;
11257 } else {
11258 if (m_chunkCount == m_chunkSpace) {
11259 b2Chunk* oldChunks = m_chunks;
11260 m_chunkSpace += b2_chunkArrayIncrement;
11261 m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
11262 memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
11263 memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
11264 b2Free(oldChunks);
11265 }
11266
11267 b2Chunk* chunk = m_chunks + m_chunkCount;
11268 chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
11269#if defined(_DEBUG)
11270 memset(chunk->blocks, 0xcd, b2_chunkSize);
11271#endif
11272 int32 blockSize = b2_blockSizes[index];
11273 chunk->blockSize = blockSize;
11274 int32 blockCount = b2_chunkSize / blockSize;
11275 b2Assert(blockCount * blockSize <= b2_chunkSize);
11276 for (int32 i = 0; i < blockCount - 1; ++i) {
11277 b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
11278 b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
11279 block->next = next;
11280 }
11281 b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
11282 last->next = nullptr;
11283
11284 m_freeLists[index] = chunk->blocks->next;
11285 ++m_chunkCount;
11286
11287 return chunk->blocks;
11288 }
11289}
11290
11291void b2BlockAllocator::Free(void* p, int32 size) {
11292 if (size == 0) {
11293 return;
11294 }
11295
11296 b2Assert(0 < size);
11297
11298 if (size > b2_maxBlockSize) {
11299 b2Free(p);
11300 return;
11301 }
11302
11303 int32 index = b2_sizeMap.values[size];
11304 b2Assert(0 <= index && index < b2_blockSizeCount);
11305
11306#if defined(_DEBUG)
11307 // Verify the memory address and size is valid.
11308 int32 blockSize = b2_blockSizes[index];
11309 bool found = false;
11310 for (int32 i = 0; i < m_chunkCount; ++i) {
11311 b2Chunk* chunk = m_chunks + i;
11312 if (chunk->blockSize != blockSize) {
11313 b2Assert((int8*)p + blockSize <= (int8*)chunk->blocks ||
11314 (int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
11315 } else {
11316 if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) {
11317 found = true;
11318 }
11319 }
11320 }
11321
11322 b2Assert(found);
11323
11324 memset(p, 0xfd, blockSize);
11325#endif
11326
11327 b2Block* block = (b2Block*)p;
11328 block->next = m_freeLists[index];
11329 m_freeLists[index] = block;
11330}
11331
11332void b2BlockAllocator::Clear() {
11333 for (int32 i = 0; i < m_chunkCount; ++i) {
11334 b2Free(m_chunks[i].blocks);
11335 }
11336
11337 m_chunkCount = 0;
11338 memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
11339 memset(m_freeLists, 0, sizeof(m_freeLists));
11340}
11341// MIT License
11342
11343// Copyright (c) 2019 Erin Catto
11344
11345// Permission is hereby granted, free of charge, to any person obtaining a copy
11346// of this software and associated documentation files (the "Software"), to deal
11347// in the Software without restriction, including without limitation the rights
11348// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11349// copies of the Software, and to permit persons to whom the Software is
11350// furnished to do so, subject to the following conditions:
11351
11352// The above copyright notice and this permission notice shall be included in all
11353// copies or substantial portions of the Software.
11354
11355// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11356// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11357// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11358// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11359// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11360// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11361// SOFTWARE.
11362#include "box2d/b2_draw.h"
11363
11364b2Draw::b2Draw() {
11365 m_drawFlags = 0;
11366}
11367
11368void b2Draw::SetFlags(uint32 flags) {
11369 m_drawFlags = flags;
11370}
11371
11372uint32 b2Draw::GetFlags() const {
11373 return m_drawFlags;
11374}
11375
11376void b2Draw::AppendFlags(uint32 flags) {
11377 m_drawFlags |= flags;
11378}
11379
11380void b2Draw::ClearFlags(uint32 flags) {
11381 m_drawFlags &= ~flags;
11382}
11383// MIT License
11384
11385// Copyright (c) 2019 Erin Catto
11386
11387// Permission is hereby granted, free of charge, to any person obtaining a copy
11388// of this software and associated documentation files (the "Software"), to deal
11389// in the Software without restriction, including without limitation the rights
11390// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11391// copies of the Software, and to permit persons to whom the Software is
11392// furnished to do so, subject to the following conditions:
11393
11394// The above copyright notice and this permission notice shall be included in all
11395// copies or substantial portions of the Software.
11396
11397// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11398// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11399// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11400// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11401// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11402// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11403// SOFTWARE.
11404
11405#include "box2d/b2_math.h"
11406
11407const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
11408
11409/// Solve A * x = b, where b is a column vector. This is more efficient
11410/// than computing the inverse in one-shot cases.
11411b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const {
11412 float det = b2Dot(ex, b2Cross(ey, ez));
11413 if (det != 0.0f) {
11414 det = 1.0f / det;
11415 }
11416 b2Vec3 x;
11417 x.x = det * b2Dot(b, b2Cross(ey, ez));
11418 x.y = det * b2Dot(ex, b2Cross(b, ez));
11419 x.z = det * b2Dot(ex, b2Cross(ey, b));
11420 return x;
11421}
11422
11423/// Solve A * x = b, where b is a column vector. This is more efficient
11424/// than computing the inverse in one-shot cases.
11425b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const {
11426 float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
11427 float det = a11 * a22 - a12 * a21;
11428 if (det != 0.0f) {
11429 det = 1.0f / det;
11430 }
11431 b2Vec2 x;
11432 x.x = det * (a22 * b.x - a12 * b.y);
11433 x.y = det * (a11 * b.y - a21 * b.x);
11434 return x;
11435}
11436
11437///
11438void b2Mat33::GetInverse22(b2Mat33* M) const {
11439 float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
11440 float det = a * d - b * c;
11441 if (det != 0.0f) {
11442 det = 1.0f / det;
11443 }
11444
11445 M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f;
11446 M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f;
11447 M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
11448}
11449
11450/// Returns the zero matrix if singular.
11451void b2Mat33::GetSymInverse33(b2Mat33* M) const {
11452 float det = b2Dot(ex, b2Cross(ey, ez));
11453 if (det != 0.0f) {
11454 det = 1.0f / det;
11455 }
11456
11457 float a11 = ex.x, a12 = ey.x, a13 = ez.x;
11458 float a22 = ey.y, a23 = ez.y;
11459 float a33 = ez.z;
11460
11461 M->ex.x = det * (a22 * a33 - a23 * a23);
11462 M->ex.y = det * (a13 * a23 - a12 * a33);
11463 M->ex.z = det * (a12 * a23 - a13 * a22);
11464
11465 M->ey.x = M->ex.y;
11466 M->ey.y = det * (a11 * a33 - a13 * a13);
11467 M->ey.z = det * (a13 * a12 - a11 * a23);
11468
11469 M->ez.x = M->ex.z;
11470 M->ez.y = M->ey.z;
11471 M->ez.z = det * (a11 * a22 - a12 * a12);
11472}
11473// MIT License
11474
11475// Copyright (c) 2019 Erin Catto
11476
11477// Permission is hereby granted, free of charge, to any person obtaining a copy
11478// of this software and associated documentation files (the "Software"), to deal
11479// in the Software without restriction, including without limitation the rights
11480// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11481// copies of the Software, and to permit persons to whom the Software is
11482// furnished to do so, subject to the following conditions:
11483
11484// The above copyright notice and this permission notice shall be included in all
11485// copies or substantial portions of the Software.
11486
11487// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11488// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11489// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11490// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11491// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11492// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11493// SOFTWARE.
11494
11495#define _CRT_SECURE_NO_WARNINGS
11496
11497#include "box2d/b2_settings.h"
11498#include <stdio.h>
11499#include <stdarg.h>
11500#include <stdlib.h>
11501
11502b2Version b2_version = { 2, 4, 0 };
11503
11504// Memory allocators. Modify these to use your own allocator.
11505void* b2Alloc_Default(int32 size) {
11506 return malloc(size);
11507}
11508
11509void b2Free_Default(void* mem) {
11510 free(mem);
11511}
11512
11513// You can modify this to use your logging facility.
11514void b2Log_Default(const char* string, va_list args) {
11515 vprintf(string, args);
11516}
11517
11518FILE* b2_dumpFile = nullptr;
11519
11520void b2OpenDump(const char* fileName) {
11521 b2Assert(b2_dumpFile == nullptr);
11522 b2_dumpFile = fopen(fileName, "w");
11523}
11524
11525void b2Dump(const char* string, ...) {
11526 if (b2_dumpFile == nullptr) {
11527 return;
11528 }
11529
11530 va_list args;
11531 va_start(args, string);
11532 vfprintf(b2_dumpFile, string, args);
11533 va_end(args);
11534}
11535
11536void b2CloseDump() {
11537 fclose(b2_dumpFile);
11538 b2_dumpFile = nullptr;
11539}
11540// MIT License
11541
11542// Copyright (c) 2019 Erin Catto
11543
11544// Permission is hereby granted, free of charge, to any person obtaining a copy
11545// of this software and associated documentation files (the "Software"), to deal
11546// in the Software without restriction, including without limitation the rights
11547// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11548// copies of the Software, and to permit persons to whom the Software is
11549// furnished to do so, subject to the following conditions:
11550
11551// The above copyright notice and this permission notice shall be included in all
11552// copies or substantial portions of the Software.
11553
11554// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11555// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11556// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11557// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11558// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11559// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11560// SOFTWARE.
11561
11562#include "box2d/b2_stack_allocator.h"
11563#include "box2d/b2_math.h"
11564
11565b2StackAllocator::b2StackAllocator() {
11566 m_index = 0;
11567 m_allocation = 0;
11568 m_maxAllocation = 0;
11569 m_entryCount = 0;
11570}
11571
11572b2StackAllocator::~b2StackAllocator() {
11573 b2Assert(m_index == 0);
11574 b2Assert(m_entryCount == 0);
11575}
11576
11577void* b2StackAllocator::Allocate(int32 size) {
11578 b2Assert(m_entryCount < b2_maxStackEntries);
11579
11580 b2StackEntry* entry = m_entries + m_entryCount;
11581 entry->size = size;
11582 if (m_index + size > b2_stackSize) {
11583 entry->data = (char*)b2Alloc(size);
11584 entry->usedMalloc = true;
11585 } else {
11586 entry->data = m_data + m_index;
11587 entry->usedMalloc = false;
11588 m_index += size;
11589 }
11590
11591 m_allocation += size;
11592 m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
11593 ++m_entryCount;
11594
11595 return entry->data;
11596}
11597
11598void b2StackAllocator::Free(void* p) {
11599 b2Assert(m_entryCount > 0);
11600 b2StackEntry* entry = m_entries + m_entryCount - 1;
11601 b2Assert(p == entry->data);
11602 if (entry->usedMalloc) {
11603 b2Free(p);
11604 } else {
11605 m_index -= entry->size;
11606 }
11607 m_allocation -= entry->size;
11608 --m_entryCount;
11609
11610 p = nullptr;
11611}
11612
11613int32 b2StackAllocator::GetMaxAllocation() const {
11614 return m_maxAllocation;
11615}
11616// MIT License
11617
11618// Copyright (c) 2019 Erin Catto
11619
11620// Permission is hereby granted, free of charge, to any person obtaining a copy
11621// of this software and associated documentation files (the "Software"), to deal
11622// in the Software without restriction, including without limitation the rights
11623// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11624// copies of the Software, and to permit persons to whom the Software is
11625// furnished to do so, subject to the following conditions:
11626
11627// The above copyright notice and this permission notice shall be included in all
11628// copies or substantial portions of the Software.
11629
11630// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11631// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11632// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11633// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11634// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11635// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11636// SOFTWARE.
11637
11638#include "box2d/b2_timer.h"
11639
11640#if defined(_WIN32)
11641
11642double b2Timer::s_invFrequency = 0.0;
11643
11644#ifndef WIN32_LEAN_AND_MEAN
11645#define WIN32_LEAN_AND_MEAN
11646#endif
11647
11648#include <windows.h>
11649
11650b2Timer::b2Timer() {
11651 LARGE_INTEGER largeInteger;
11652
11653 if (s_invFrequency == 0.0) {
11654 QueryPerformanceFrequency(&largeInteger);
11655 s_invFrequency = double(largeInteger.QuadPart);
11656 if (s_invFrequency > 0.0) {
11657 s_invFrequency = 1000.0 / s_invFrequency;
11658 }
11659 }
11660
11661 QueryPerformanceCounter(&largeInteger);
11662 m_start = double(largeInteger.QuadPart);
11663}
11664
11665void b2Timer::Reset() {
11666 LARGE_INTEGER largeInteger;
11667 QueryPerformanceCounter(&largeInteger);
11668 m_start = double(largeInteger.QuadPart);
11669}
11670
11671float b2Timer::GetMilliseconds() const {
11672 LARGE_INTEGER largeInteger;
11673 QueryPerformanceCounter(&largeInteger);
11674 double count = double(largeInteger.QuadPart);
11675 float ms = float(s_invFrequency * (count - m_start));
11676 return ms;
11677}
11678
11679#elif defined(__linux__) || defined (__APPLE__)
11680
11681#include <sys/time.h>
11682
11683b2Timer::b2Timer() {
11684 Reset();
11685}
11686
11687void b2Timer::Reset() {
11688 timeval t;
11689 gettimeofday(&t, 0);
11690 m_start_sec = t.tv_sec;
11691 m_start_usec = t.tv_usec;
11692}
11693
11694float b2Timer::GetMilliseconds() const {
11695 timeval t;
11696 gettimeofday(&t, 0);
11697 time_t start_sec = m_start_sec;
11698 suseconds_t start_usec = m_start_usec;
11699
11700 // http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
11701 if (t.tv_usec < start_usec) {
11702 int nsec = (start_usec - t.tv_usec) / 1000000 + 1;
11703 start_usec -= 1000000 * nsec;
11704 start_sec += nsec;
11705 }
11706
11707 if (t.tv_usec - start_usec > 1000000) {
11708 int nsec = (t.tv_usec - start_usec) / 1000000;
11709 start_usec += 1000000 * nsec;
11710 start_sec -= nsec;
11711 }
11712 return 1000.0f * (t.tv_sec - start_sec) + 0.001f * (t.tv_usec - start_usec);
11713}
11714
11715#else
11716
11717b2Timer::b2Timer() {
11718}
11719
11720void b2Timer::Reset() {
11721}
11722
11723float b2Timer::GetMilliseconds() const {
11724 return 0.0f;
11725}
11726
11727#endif
11728// MIT License
11729
11730// Copyright (c) 2019 Erin Catto
11731
11732// Permission is hereby granted, free of charge, to any person obtaining a copy
11733// of this software and associated documentation files (the "Software"), to deal
11734// in the Software without restriction, including without limitation the rights
11735// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11736// copies of the Software, and to permit persons to whom the Software is
11737// furnished to do so, subject to the following conditions:
11738
11739// The above copyright notice and this permission notice shall be included in all
11740// copies or substantial portions of the Software.
11741
11742// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11743// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11744// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11745// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
11746// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
11747// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
11748// SOFTWARE.
11749
11750#include "box2d/b2_body.h"
11751#include "box2d/b2_contact.h"
11752#include "box2d/b2_fixture.h"
11753#include "box2d/b2_joint.h"
11754#include "box2d/b2_world.h"
11755
11756#include <new>
11757
11758b2Body::b2Body(const b2BodyDef* bd, b2World* world) {
11759 b2Assert(bd->position.IsValid());
11760 b2Assert(bd->linearVelocity.IsValid());
11761 b2Assert(b2IsValid(bd->angle));
11762 b2Assert(b2IsValid(bd->angularVelocity));
11763 b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
11764 b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
11765
11766 m_flags = 0;
11767
11768 if (bd->bullet) {
11769 m_flags |= e_bulletFlag;
11770 }
11771 if (bd->fixedRotation) {
11772 m_flags |= e_fixedRotationFlag;
11773 }
11774 if (bd->allowSleep) {
11775 m_flags |= e_autoSleepFlag;
11776 }
11777 if (bd->awake && bd->type != b2_staticBody) {
11778 m_flags |= e_awakeFlag;
11779 }
11780 if (bd->enabled) {
11781 m_flags |= e_enabledFlag;
11782 }
11783
11784 m_world = world;
11785
11786 m_xf.p = bd->position;
11787 m_xf.q.Set(bd->angle);
11788
11789 m_sweep.localCenter.SetZero();
11790 m_sweep.c0 = m_xf.p;
11791 m_sweep.c = m_xf.p;
11792 m_sweep.a0 = bd->angle;
11793 m_sweep.a = bd->angle;
11794 m_sweep.alpha0 = 0.0f;
11795
11796 m_jointList = nullptr;
11797 m_contactList = nullptr;
11798 m_prev = nullptr;
11799 m_next = nullptr;
11800
11801 m_linearVelocity = bd->linearVelocity;
11802 m_angularVelocity = bd->angularVelocity;
11803
11804 m_linearDamping = bd->linearDamping;
11805 m_angularDamping = bd->angularDamping;
11806 m_gravityScale = bd->gravityScale;
11807
11808 m_force.SetZero();
11809 m_torque = 0.0f;
11810
11811 m_sleepTime = 0.0f;
11812
11813 m_type = bd->type;
11814
11815 m_mass = 0.0f;
11816 m_invMass = 0.0f;
11817
11818 m_I = 0.0f;
11819 m_invI = 0.0f;
11820
11821 m_userData = bd->userData;
11822
11823 m_fixtureList = nullptr;
11824 m_fixtureCount = 0;
11825}
11826
11827b2Body::~b2Body() {
11828 // shapes and joints are destroyed in b2World::Destroy
11829}
11830
11831void b2Body::SetType(b2BodyType type) {
11832 b2Assert(m_world->IsLocked() == false);
11833 if (m_world->IsLocked() == true) {
11834 return;
11835 }
11836
11837 if (m_type == type) {
11838 return;
11839 }
11840
11841 m_type = type;
11842
11843 ResetMassData();
11844
11845 if (m_type == b2_staticBody) {
11846 m_linearVelocity.SetZero();
11847 m_angularVelocity = 0.0f;
11848 m_sweep.a0 = m_sweep.a;
11849 m_sweep.c0 = m_sweep.c;
11850 m_flags &= ~e_awakeFlag;
11851 SynchronizeFixtures();
11852 }
11853
11854 SetAwake(true);
11855
11856 m_force.SetZero();
11857 m_torque = 0.0f;
11858
11859 // Delete the attached contacts.
11860 b2ContactEdge* ce = m_contactList;
11861 while (ce) {
11862 b2ContactEdge* ce0 = ce;
11863 ce = ce->next;
11864 m_world->m_contactManager.Destroy(ce0->contact);
11865 }
11866 m_contactList = nullptr;
11867
11868 // Touch the proxies so that new contacts will be created (when appropriate)
11869 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
11870 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
11871 int32 proxyCount = f->m_proxyCount;
11872 for (int32 i = 0; i < proxyCount; ++i) {
11873 broadPhase->TouchProxy(f->m_proxies[i].proxyId);
11874 }
11875 }
11876}
11877
11878b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) {
11879 b2Assert(m_world->IsLocked() == false);
11880 if (m_world->IsLocked() == true) {
11881 return nullptr;
11882 }
11883
11884 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
11885
11886 void* memory = allocator->Allocate(sizeof(b2Fixture));
11887 b2Fixture* fixture = new (memory) b2Fixture;
11888 fixture->Create(allocator, this, def);
11889
11890 if (m_flags & e_enabledFlag) {
11891 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
11892 fixture->CreateProxies(broadPhase, m_xf);
11893 }
11894
11895 fixture->m_next = m_fixtureList;
11896 m_fixtureList = fixture;
11897 ++m_fixtureCount;
11898
11899 fixture->m_body = this;
11900
11901 // Adjust mass properties if needed.
11902 if (fixture->m_density > 0.0f) {
11903 ResetMassData();
11904 }
11905
11906 // Let the world know we have a new fixture. This will cause new contacts
11907 // to be created at the beginning of the next time step.
11908 m_world->m_newContacts = true;
11909
11910 return fixture;
11911}
11912
11913b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float density) {
11914 b2FixtureDef def;
11915 def.shape = shape;
11916 def.density = density;
11917
11918 return CreateFixture(&def);
11919}
11920
11921void b2Body::DestroyFixture(b2Fixture* fixture) {
11922 if (fixture == NULL) {
11923 return;
11924 }
11925
11926 b2Assert(m_world->IsLocked() == false);
11927 if (m_world->IsLocked() == true) {
11928 return;
11929 }
11930
11931 b2Assert(fixture->m_body == this);
11932
11933 // Remove the fixture from this body's singly linked list.
11934 b2Assert(m_fixtureCount > 0);
11935 b2Fixture** node = &m_fixtureList;
11936 bool found = false;
11937 while (*node != nullptr) {
11938 if (*node == fixture) {
11939 *node = fixture->m_next;
11940 found = true;
11941 break;
11942 }
11943
11944 node = &(*node)->m_next;
11945 }
11946
11947 // You tried to remove a shape that is not attached to this body.
11948 b2Assert(found);
11949
11950 // Destroy any contacts associated with the fixture.
11951 b2ContactEdge* edge = m_contactList;
11952 while (edge) {
11953 b2Contact* c = edge->contact;
11954 edge = edge->next;
11955
11956 b2Fixture* fixtureA = c->GetFixtureA();
11957 b2Fixture* fixtureB = c->GetFixtureB();
11958
11959 if (fixture == fixtureA || fixture == fixtureB) {
11960 // This destroys the contact and removes it from
11961 // this body's contact list.
11962 m_world->m_contactManager.Destroy(c);
11963 }
11964 }
11965
11966 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
11967
11968 if (m_flags & e_enabledFlag) {
11969 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
11970 fixture->DestroyProxies(broadPhase);
11971 }
11972
11973 fixture->m_body = nullptr;
11974 fixture->m_next = nullptr;
11975 fixture->Destroy(allocator);
11976 fixture->~b2Fixture();
11977 allocator->Free(fixture, sizeof(b2Fixture));
11978
11979 --m_fixtureCount;
11980
11981 // Reset the mass data.
11982 ResetMassData();
11983}
11984
11985void b2Body::ResetMassData() {
11986 // Compute mass data from shapes. Each shape has its own density.
11987 m_mass = 0.0f;
11988 m_invMass = 0.0f;
11989 m_I = 0.0f;
11990 m_invI = 0.0f;
11991 m_sweep.localCenter.SetZero();
11992
11993 // Static and kinematic bodies have zero mass.
11994 if (m_type == b2_staticBody || m_type == b2_kinematicBody) {
11995 m_sweep.c0 = m_xf.p;
11996 m_sweep.c = m_xf.p;
11997 m_sweep.a0 = m_sweep.a;
11998 return;
11999 }
12000
12001 b2Assert(m_type == b2_dynamicBody);
12002
12003 // Accumulate mass over all fixtures.
12004 b2Vec2 localCenter = b2Vec2_zero;
12005 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12006 if (f->m_density == 0.0f) {
12007 continue;
12008 }
12009
12010 b2MassData massData;
12011 f->GetMassData(&massData);
12012 m_mass += massData.mass;
12013 localCenter += massData.mass * massData.center;
12014 m_I += massData.I;
12015 }
12016
12017 // Compute center of mass.
12018 if (m_mass > 0.0f) {
12019 m_invMass = 1.0f / m_mass;
12020 localCenter *= m_invMass;
12021 }
12022
12023 if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
12024 // Center the inertia about the center of mass.
12025 m_I -= m_mass * b2Dot(localCenter, localCenter);
12026 b2Assert(m_I > 0.0f);
12027 m_invI = 1.0f / m_I;
12028
12029 } else {
12030 m_I = 0.0f;
12031 m_invI = 0.0f;
12032 }
12033
12034 // Move center of mass.
12035 b2Vec2 oldCenter = m_sweep.c;
12036 m_sweep.localCenter = localCenter;
12037 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
12038
12039 // Update center of mass velocity.
12040 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
12041}
12042
12043void b2Body::SetMassData(const b2MassData* massData) {
12044 b2Assert(m_world->IsLocked() == false);
12045 if (m_world->IsLocked() == true) {
12046 return;
12047 }
12048
12049 if (m_type != b2_dynamicBody) {
12050 return;
12051 }
12052
12053 m_invMass = 0.0f;
12054 m_I = 0.0f;
12055 m_invI = 0.0f;
12056
12057 m_mass = massData->mass;
12058 if (m_mass <= 0.0f) {
12059 m_mass = 1.0f;
12060 }
12061
12062 m_invMass = 1.0f / m_mass;
12063
12064 if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) {
12065 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
12066 b2Assert(m_I > 0.0f);
12067 m_invI = 1.0f / m_I;
12068 }
12069
12070 // Move center of mass.
12071 b2Vec2 oldCenter = m_sweep.c;
12072 m_sweep.localCenter = massData->center;
12073 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
12074
12075 // Update center of mass velocity.
12076 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
12077}
12078
12079bool b2Body::ShouldCollide(const b2Body* other) const {
12080 // At least one body should be dynamic.
12081 if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) {
12082 return false;
12083 }
12084
12085 // Does a joint prevent collision?
12086 for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) {
12087 if (jn->other == other) {
12088 if (jn->joint->m_collideConnected == false) {
12089 return false;
12090 }
12091 }
12092 }
12093
12094 return true;
12095}
12096
12097void b2Body::SetTransform(const b2Vec2& position, float angle) {
12098 b2Assert(m_world->IsLocked() == false);
12099 if (m_world->IsLocked() == true) {
12100 return;
12101 }
12102
12103 m_xf.q.Set(angle);
12104 m_xf.p = position;
12105
12106 m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
12107 m_sweep.a = angle;
12108
12109 m_sweep.c0 = m_sweep.c;
12110 m_sweep.a0 = angle;
12111
12112 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
12113 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12114 f->Synchronize(broadPhase, m_xf, m_xf);
12115 }
12116
12117 // Check for new contacts the next step
12118 m_world->m_newContacts = true;
12119}
12120
12121void b2Body::SynchronizeFixtures() {
12122 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
12123
12124 if (m_flags & b2Body::e_awakeFlag) {
12125 b2Transform xf1;
12126 xf1.q.Set(m_sweep.a0);
12127 xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
12128
12129 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12130 f->Synchronize(broadPhase, xf1, m_xf);
12131 }
12132 } else {
12133 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12134 f->Synchronize(broadPhase, m_xf, m_xf);
12135 }
12136 }
12137}
12138
12139void b2Body::SetEnabled(bool flag) {
12140 b2Assert(m_world->IsLocked() == false);
12141
12142 if (flag == IsEnabled()) {
12143 return;
12144 }
12145
12146 if (flag) {
12147 m_flags |= e_enabledFlag;
12148
12149 // Create all proxies.
12150 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
12151 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12152 f->CreateProxies(broadPhase, m_xf);
12153 }
12154
12155 // Contacts are created at the beginning of the next
12156 m_world->m_newContacts = true;
12157 } else {
12158 m_flags &= ~e_enabledFlag;
12159
12160 // Destroy all proxies.
12161 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
12162 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12163 f->DestroyProxies(broadPhase);
12164 }
12165
12166 // Destroy the attached contacts.
12167 b2ContactEdge* ce = m_contactList;
12168 while (ce) {
12169 b2ContactEdge* ce0 = ce;
12170 ce = ce->next;
12171 m_world->m_contactManager.Destroy(ce0->contact);
12172 }
12173 m_contactList = nullptr;
12174 }
12175}
12176
12177void b2Body::SetFixedRotation(bool flag) {
12178 bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
12179 if (status == flag) {
12180 return;
12181 }
12182
12183 if (flag) {
12184 m_flags |= e_fixedRotationFlag;
12185 } else {
12186 m_flags &= ~e_fixedRotationFlag;
12187 }
12188
12189 m_angularVelocity = 0.0f;
12190
12191 ResetMassData();
12192}
12193
12194void b2Body::Dump() {
12195 int32 bodyIndex = m_islandIndex;
12196
12197 // %.9g is sufficient to save and load the same value using text
12198 // FLT_DECIMAL_DIG == 9
12199
12200 b2Dump("{\n");
12201 b2Dump(" b2BodyDef bd;\n");
12202 b2Dump(" bd.type = b2BodyType(%d);\n", m_type);
12203 b2Dump(" bd.position.Set(%.9g, %.9g);\n", m_xf.p.x, m_xf.p.y);
12204 b2Dump(" bd.angle = %.9g;\n", m_sweep.a);
12205 b2Dump(" bd.linearVelocity.Set(%.9g, %.9g);\n", m_linearVelocity.x, m_linearVelocity.y);
12206 b2Dump(" bd.angularVelocity = %.9g;\n", m_angularVelocity);
12207 b2Dump(" bd.linearDamping = %.9g;\n", m_linearDamping);
12208 b2Dump(" bd.angularDamping = %.9g;\n", m_angularDamping);
12209 b2Dump(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
12210 b2Dump(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
12211 b2Dump(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
12212 b2Dump(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
12213 b2Dump(" bd.enabled = bool(%d);\n", m_flags & e_enabledFlag);
12214 b2Dump(" bd.gravityScale = %.9g;\n", m_gravityScale);
12215 b2Dump(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
12216 b2Dump("\n");
12217 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) {
12218 b2Dump(" {\n");
12219 f->Dump(bodyIndex);
12220 b2Dump(" }\n");
12221 }
12222 b2Dump("}\n");
12223}
12224// MIT License
12225
12226// Copyright (c) 2019 Erin Catto
12227
12228// Permission is hereby granted, free of charge, to any person obtaining a copy
12229// of this software and associated documentation files (the "Software"), to deal
12230// in the Software without restriction, including without limitation the rights
12231// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12232// copies of the Software, and to permit persons to whom the Software is
12233// furnished to do so, subject to the following conditions:
12234
12235// The above copyright notice and this permission notice shall be included in all
12236// copies or substantial portions of the Software.
12237
12238// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12239// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12240// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12241// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12242// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12243// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12244// SOFTWARE.
12245
12246#include "b2_chain_circle_contact.h"
12247#include "box2d/b2_block_allocator.h"
12248#include "box2d/b2_fixture.h"
12249#include "box2d/b2_chain_shape.h"
12250#include "box2d/b2_edge_shape.h"
12251
12252#include <new>
12253
12254b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) {
12255 void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
12256 return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
12257}
12258
12259void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
12260 ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
12261 allocator->Free(contact, sizeof(b2ChainAndCircleContact));
12262}
12263
12264b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
12265 : b2Contact(fixtureA, indexA, fixtureB, indexB) {
12266 b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
12267 b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
12268}
12269
12270void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
12271 b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
12272 b2EdgeShape edge;
12273 chain->GetChildEdge(&edge, m_indexA);
12274 b2CollideEdgeAndCircle(manifold, &edge, xfA,
12275 (b2CircleShape*)m_fixtureB->GetShape(), xfB);
12276}
12277// MIT License
12278
12279// Copyright (c) 2019 Erin Catto
12280
12281// Permission is hereby granted, free of charge, to any person obtaining a copy
12282// of this software and associated documentation files (the "Software"), to deal
12283// in the Software without restriction, including without limitation the rights
12284// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12285// copies of the Software, and to permit persons to whom the Software is
12286// furnished to do so, subject to the following conditions:
12287
12288// The above copyright notice and this permission notice shall be included in all
12289// copies or substantial portions of the Software.
12290
12291// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12292// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12293// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12294// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12295// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12296// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12297// SOFTWARE.
12298
12299#include "b2_chain_polygon_contact.h"
12300#include "box2d/b2_block_allocator.h"
12301#include "box2d/b2_fixture.h"
12302#include "box2d/b2_chain_shape.h"
12303#include "box2d/b2_edge_shape.h"
12304
12305#include <new>
12306
12307b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) {
12308 void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
12309 return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
12310}
12311
12312void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
12313 ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
12314 allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
12315}
12316
12317b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
12318 : b2Contact(fixtureA, indexA, fixtureB, indexB) {
12319 b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
12320 b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
12321}
12322
12323void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
12324 b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
12325 b2EdgeShape edge;
12326 chain->GetChildEdge(&edge, m_indexA);
12327 b2CollideEdgeAndPolygon(manifold, &edge, xfA,
12328 (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
12329}
12330// MIT License
12331
12332// Copyright (c) 2019 Erin Catto
12333
12334// Permission is hereby granted, free of charge, to any person obtaining a copy
12335// of this software and associated documentation files (the "Software"), to deal
12336// in the Software without restriction, including without limitation the rights
12337// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12338// copies of the Software, and to permit persons to whom the Software is
12339// furnished to do so, subject to the following conditions:
12340
12341// The above copyright notice and this permission notice shall be included in all
12342// copies or substantial portions of the Software.
12343
12344// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12345// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12346// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12347// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12348// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12349// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12350// SOFTWARE.
12351
12352#include "b2_circle_contact.h"
12353#include "box2d/b2_block_allocator.h"
12354#include "box2d/b2_body.h"
12355#include "box2d/b2_fixture.h"
12356#include "box2d/b2_time_of_impact.h"
12357#include "box2d/b2_world_callbacks.h"
12358
12359#include <new>
12360
12361b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) {
12362 void* mem = allocator->Allocate(sizeof(b2CircleContact));
12363 return new (mem) b2CircleContact(fixtureA, fixtureB);
12364}
12365
12366void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
12367 ((b2CircleContact*)contact)->~b2CircleContact();
12368 allocator->Free(contact, sizeof(b2CircleContact));
12369}
12370
12371b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
12372 : b2Contact(fixtureA, 0, fixtureB, 0) {
12373 b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
12374 b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
12375}
12376
12377void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
12378 b2CollideCircles(manifold,
12379 (b2CircleShape*)m_fixtureA->GetShape(), xfA,
12380 (b2CircleShape*)m_fixtureB->GetShape(), xfB);
12381}
12382// MIT License
12383
12384// Copyright (c) 2019 Erin Catto
12385
12386// Permission is hereby granted, free of charge, to any person obtaining a copy
12387// of this software and associated documentation files (the "Software"), to deal
12388// in the Software without restriction, including without limitation the rights
12389// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12390// copies of the Software, and to permit persons to whom the Software is
12391// furnished to do so, subject to the following conditions:
12392
12393// The above copyright notice and this permission notice shall be included in all
12394// copies or substantial portions of the Software.
12395
12396// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12397// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12398// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12399// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12400// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12401// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12402// SOFTWARE.
12403
12404#include "b2_chain_circle_contact.h"
12405#include "b2_chain_polygon_contact.h"
12406#include "b2_circle_contact.h"
12407#include "b2_contact_solver.h"
12408#include "b2_edge_circle_contact.h"
12409#include "b2_edge_polygon_contact.h"
12410#include "b2_polygon_circle_contact.h"
12411#include "b2_polygon_contact.h"
12412
12413#include "box2d/b2_contact.h"
12414#include "box2d/b2_block_allocator.h"
12415#include "box2d/b2_body.h"
12416#include "box2d/b2_collision.h"
12417#include "box2d/b2_fixture.h"
12418#include "box2d/b2_shape.h"
12419#include "box2d/b2_time_of_impact.h"
12420#include "box2d/b2_world.h"
12421
12422b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
12423bool b2Contact::s_initialized = false;
12424
12425void b2Contact::InitializeRegisters() {
12426 AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
12427 AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
12428 AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
12429 AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
12430 AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
12431 AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);
12432 AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);
12433}
12434
12435void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
12436 b2Shape::Type type1, b2Shape::Type type2) {
12437 b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
12438 b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
12439
12440 s_registers[type1][type2].createFcn = createFcn;
12441 s_registers[type1][type2].destroyFcn = destoryFcn;
12442 s_registers[type1][type2].primary = true;
12443
12444 if (type1 != type2) {
12445 s_registers[type2][type1].createFcn = createFcn;
12446 s_registers[type2][type1].destroyFcn = destoryFcn;
12447 s_registers[type2][type1].primary = false;
12448 }
12449}
12450
12451b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) {
12452 if (s_initialized == false) {
12453 InitializeRegisters();
12454 s_initialized = true;
12455 }
12456
12457 b2Shape::Type type1 = fixtureA->GetType();
12458 b2Shape::Type type2 = fixtureB->GetType();
12459
12460 b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
12461 b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
12462
12463 b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
12464 if (createFcn) {
12465 if (s_registers[type1][type2].primary) {
12466 return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
12467 } else {
12468 return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
12469 }
12470 } else {
12471 return nullptr;
12472 }
12473}
12474
12475void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
12476 b2Assert(s_initialized == true);
12477
12478 b2Fixture* fixtureA = contact->m_fixtureA;
12479 b2Fixture* fixtureB = contact->m_fixtureB;
12480
12481 if (contact->m_manifold.pointCount > 0 &&
12482 fixtureA->IsSensor() == false &&
12483 fixtureB->IsSensor() == false) {
12484 fixtureA->GetBody()->SetAwake(true);
12485 fixtureB->GetBody()->SetAwake(true);
12486 }
12487
12488 b2Shape::Type typeA = fixtureA->GetType();
12489 b2Shape::Type typeB = fixtureB->GetType();
12490
12491 b2Assert(0 <= typeA && typeA < b2Shape::e_typeCount);
12492 b2Assert(0 <= typeB && typeB < b2Shape::e_typeCount);
12493
12494 b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
12495 destroyFcn(contact, allocator);
12496}
12497
12498b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) {
12499 m_flags = e_enabledFlag;
12500
12501 m_fixtureA = fA;
12502 m_fixtureB = fB;
12503
12504 m_indexA = indexA;
12505 m_indexB = indexB;
12506
12507 m_manifold.pointCount = 0;
12508
12509 m_prev = nullptr;
12510 m_next = nullptr;
12511
12512 m_nodeA.contact = nullptr;
12513 m_nodeA.prev = nullptr;
12514 m_nodeA.next = nullptr;
12515 m_nodeA.other = nullptr;
12516
12517 m_nodeB.contact = nullptr;
12518 m_nodeB.prev = nullptr;
12519 m_nodeB.next = nullptr;
12520 m_nodeB.other = nullptr;
12521
12522 m_toiCount = 0;
12523
12524 m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
12525 m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
12526 m_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold);
12527
12528 m_tangentSpeed = 0.0f;
12529}
12530
12531// Update the contact manifold and touching status.
12532// Note: do not assume the fixture AABBs are overlapping or are valid.
12533void b2Contact::Update(b2ContactListener* listener) {
12534 b2Manifold oldManifold = m_manifold;
12535
12536 // Re-enable this contact.
12537 m_flags |= e_enabledFlag;
12538
12539 bool touching = false;
12540 bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
12541
12542 bool sensorA = m_fixtureA->IsSensor();
12543 bool sensorB = m_fixtureB->IsSensor();
12544 bool sensor = sensorA || sensorB;
12545
12546 b2Body* bodyA = m_fixtureA->GetBody();
12547 b2Body* bodyB = m_fixtureB->GetBody();
12548 const b2Transform& xfA = bodyA->GetTransform();
12549 const b2Transform& xfB = bodyB->GetTransform();
12550
12551 // Is this contact a sensor?
12552 if (sensor) {
12553 const b2Shape* shapeA = m_fixtureA->GetShape();
12554 const b2Shape* shapeB = m_fixtureB->GetShape();
12555 touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
12556
12557 // Sensors don't generate manifolds.
12558 m_manifold.pointCount = 0;
12559 } else {
12560 Evaluate(&m_manifold, xfA, xfB);
12561 touching = m_manifold.pointCount > 0;
12562
12563 // Match old contact ids to new contact ids and copy the
12564 // stored impulses to warm start the solver.
12565 for (int32 i = 0; i < m_manifold.pointCount; ++i) {
12566 b2ManifoldPoint* mp2 = m_manifold.points + i;
12567 mp2->normalImpulse = 0.0f;
12568 mp2->tangentImpulse = 0.0f;
12569 b2ContactID id2 = mp2->id;
12570
12571 for (int32 j = 0; j < oldManifold.pointCount; ++j) {
12572 b2ManifoldPoint* mp1 = oldManifold.points + j;
12573
12574 if (mp1->id.key == id2.key) {
12575 mp2->normalImpulse = mp1->normalImpulse;
12576 mp2->tangentImpulse = mp1->tangentImpulse;
12577 break;
12578 }
12579 }
12580 }
12581
12582 if (touching != wasTouching) {
12583 bodyA->SetAwake(true);
12584 bodyB->SetAwake(true);
12585 }
12586 }
12587
12588 if (touching) {
12589 m_flags |= e_touchingFlag;
12590 } else {
12591 m_flags &= ~e_touchingFlag;
12592 }
12593
12594 if (wasTouching == false && touching == true && listener) {
12595 listener->BeginContact(this);
12596 }
12597
12598 if (wasTouching == true && touching == false && listener) {
12599 listener->EndContact(this);
12600 }
12601
12602 if (sensor == false && touching && listener) {
12603 listener->PreSolve(this, &oldManifold);
12604 }
12605}
12606// MIT License
12607
12608// Copyright (c) 2019 Erin Catto
12609
12610// Permission is hereby granted, free of charge, to any person obtaining a copy
12611// of this software and associated documentation files (the "Software"), to deal
12612// in the Software without restriction, including without limitation the rights
12613// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12614// copies of the Software, and to permit persons to whom the Software is
12615// furnished to do so, subject to the following conditions:
12616
12617// The above copyright notice and this permission notice shall be included in all
12618// copies or substantial portions of the Software.
12619
12620// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12621// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12622// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12623// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12624// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12625// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12626// SOFTWARE.
12627
12628#include "box2d/b2_body.h"
12629#include "box2d/b2_contact.h"
12630#include "box2d/b2_contact_manager.h"
12631#include "box2d/b2_fixture.h"
12632#include "box2d/b2_world_callbacks.h"
12633
12634b2ContactFilter b2_defaultFilter;
12635b2ContactListener b2_defaultListener;
12636
12637b2ContactManager::b2ContactManager() {
12638 m_contactList = nullptr;
12639 m_contactCount = 0;
12640 m_contactFilter = &b2_defaultFilter;
12641 m_contactListener = &b2_defaultListener;
12642 m_allocator = nullptr;
12643}
12644
12645void b2ContactManager::Destroy(b2Contact* c) {
12646 b2Fixture* fixtureA = c->GetFixtureA();
12647 b2Fixture* fixtureB = c->GetFixtureB();
12648 b2Body* bodyA = fixtureA->GetBody();
12649 b2Body* bodyB = fixtureB->GetBody();
12650
12651 if (m_contactListener && c->IsTouching()) {
12652 m_contactListener->EndContact(c);
12653 }
12654
12655 // Remove from the world.
12656 if (c->m_prev) {
12657 c->m_prev->m_next = c->m_next;
12658 }
12659
12660 if (c->m_next) {
12661 c->m_next->m_prev = c->m_prev;
12662 }
12663
12664 if (c == m_contactList) {
12665 m_contactList = c->m_next;
12666 }
12667
12668 // Remove from body 1
12669 if (c->m_nodeA.prev) {
12670 c->m_nodeA.prev->next = c->m_nodeA.next;
12671 }
12672
12673 if (c->m_nodeA.next) {
12674 c->m_nodeA.next->prev = c->m_nodeA.prev;
12675 }
12676
12677 if (&c->m_nodeA == bodyA->m_contactList) {
12678 bodyA->m_contactList = c->m_nodeA.next;
12679 }
12680
12681 // Remove from body 2
12682 if (c->m_nodeB.prev) {
12683 c->m_nodeB.prev->next = c->m_nodeB.next;
12684 }
12685
12686 if (c->m_nodeB.next) {
12687 c->m_nodeB.next->prev = c->m_nodeB.prev;
12688 }
12689
12690 if (&c->m_nodeB == bodyB->m_contactList) {
12691 bodyB->m_contactList = c->m_nodeB.next;
12692 }
12693
12694 // Call the factory.
12695 b2Contact::Destroy(c, m_allocator);
12696 --m_contactCount;
12697}
12698
12699// This is the top level collision call for the time step. Here
12700// all the narrow phase collision is processed for the world
12701// contact list.
12702void b2ContactManager::Collide() {
12703 // Update awake contacts.
12704 b2Contact* c = m_contactList;
12705 while (c) {
12706 b2Fixture* fixtureA = c->GetFixtureA();
12707 b2Fixture* fixtureB = c->GetFixtureB();
12708 int32 indexA = c->GetChildIndexA();
12709 int32 indexB = c->GetChildIndexB();
12710 b2Body* bodyA = fixtureA->GetBody();
12711 b2Body* bodyB = fixtureB->GetBody();
12712
12713 // Is this contact flagged for filtering?
12714 if (c->m_flags & b2Contact::e_filterFlag) {
12715 // Should these bodies collide?
12716 if (bodyB->ShouldCollide(bodyA) == false) {
12717 b2Contact* cNuke = c;
12718 c = cNuke->GetNext();
12719 Destroy(cNuke);
12720 continue;
12721 }
12722
12723 // Check user filtering.
12724 if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) {
12725 b2Contact* cNuke = c;
12726 c = cNuke->GetNext();
12727 Destroy(cNuke);
12728 continue;
12729 }
12730
12731 // Clear the filtering flag.
12732 c->m_flags &= ~b2Contact::e_filterFlag;
12733 }
12734
12735 bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;
12736 bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;
12737
12738 // At least one body must be awake and it must be dynamic or kinematic.
12739 if (activeA == false && activeB == false) {
12740 c = c->GetNext();
12741 continue;
12742 }
12743
12744 int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
12745 int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
12746 bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
12747
12748 // Here we destroy contacts that cease to overlap in the broad-phase.
12749 if (overlap == false) {
12750 b2Contact* cNuke = c;
12751 c = cNuke->GetNext();
12752 Destroy(cNuke);
12753 continue;
12754 }
12755
12756 // The contact persists.
12757 c->Update(m_contactListener);
12758 c = c->GetNext();
12759 }
12760}
12761
12762void b2ContactManager::FindNewContacts() {
12763 m_broadPhase.UpdatePairs(this);
12764}
12765
12766void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) {
12767 b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA;
12768 b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB;
12769
12770 b2Fixture* fixtureA = proxyA->fixture;
12771 b2Fixture* fixtureB = proxyB->fixture;
12772
12773 int32 indexA = proxyA->childIndex;
12774 int32 indexB = proxyB->childIndex;
12775
12776 b2Body* bodyA = fixtureA->GetBody();
12777 b2Body* bodyB = fixtureB->GetBody();
12778
12779 // Are the fixtures on the same body?
12780 if (bodyA == bodyB) {
12781 return;
12782 }
12783
12784 // TODO_ERIN use a hash table to remove a potential bottleneck when both
12785 // bodies have a lot of contacts.
12786 // Does a contact already exist?
12787 b2ContactEdge* edge = bodyB->GetContactList();
12788 while (edge) {
12789 if (edge->other == bodyA) {
12790 b2Fixture* fA = edge->contact->GetFixtureA();
12791 b2Fixture* fB = edge->contact->GetFixtureB();
12792 int32 iA = edge->contact->GetChildIndexA();
12793 int32 iB = edge->contact->GetChildIndexB();
12794
12795 if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) {
12796 // A contact already exists.
12797 return;
12798 }
12799
12800 if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) {
12801 // A contact already exists.
12802 return;
12803 }
12804 }
12805
12806 edge = edge->next;
12807 }
12808
12809 // Does a joint override collision? Is at least one body dynamic?
12810 if (bodyB->ShouldCollide(bodyA) == false) {
12811 return;
12812 }
12813
12814 // Check user filtering.
12815 if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) {
12816 return;
12817 }
12818
12819 // Call the factory.
12820 b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
12821 if (c == nullptr) {
12822 return;
12823 }
12824
12825 // Contact creation may swap fixtures.
12826 fixtureA = c->GetFixtureA();
12827 fixtureB = c->GetFixtureB();
12828 indexA = c->GetChildIndexA();
12829 indexB = c->GetChildIndexB();
12830 bodyA = fixtureA->GetBody();
12831 bodyB = fixtureB->GetBody();
12832
12833 // Insert into the world.
12834 c->m_prev = nullptr;
12835 c->m_next = m_contactList;
12836 if (m_contactList != nullptr) {
12837 m_contactList->m_prev = c;
12838 }
12839 m_contactList = c;
12840
12841 // Connect to island graph.
12842
12843 // Connect to body A
12844 c->m_nodeA.contact = c;
12845 c->m_nodeA.other = bodyB;
12846
12847 c->m_nodeA.prev = nullptr;
12848 c->m_nodeA.next = bodyA->m_contactList;
12849 if (bodyA->m_contactList != nullptr) {
12850 bodyA->m_contactList->prev = &c->m_nodeA;
12851 }
12852 bodyA->m_contactList = &c->m_nodeA;
12853
12854 // Connect to body B
12855 c->m_nodeB.contact = c;
12856 c->m_nodeB.other = bodyA;
12857
12858 c->m_nodeB.prev = nullptr;
12859 c->m_nodeB.next = bodyB->m_contactList;
12860 if (bodyB->m_contactList != nullptr) {
12861 bodyB->m_contactList->prev = &c->m_nodeB;
12862 }
12863 bodyB->m_contactList = &c->m_nodeB;
12864
12865 ++m_contactCount;
12866}
12867// MIT License
12868
12869// Copyright (c) 2019 Erin Catto
12870
12871// Permission is hereby granted, free of charge, to any person obtaining a copy
12872// of this software and associated documentation files (the "Software"), to deal
12873// in the Software without restriction, including without limitation the rights
12874// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12875// copies of the Software, and to permit persons to whom the Software is
12876// furnished to do so, subject to the following conditions:
12877
12878// The above copyright notice and this permission notice shall be included in all
12879// copies or substantial portions of the Software.
12880
12881// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12882// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12883// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
12884// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12885// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
12886// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
12887// SOFTWARE.
12888
12889#include "b2_contact_solver.h"
12890
12891#include "box2d/b2_body.h"
12892#include "box2d/b2_contact.h"
12893#include "box2d/b2_fixture.h"
12894#include "box2d/b2_stack_allocator.h"
12895#include "box2d/b2_world.h"
12896
12897// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix.
12898#define B2_DEBUG_SOLVER 0
12899
12900B2_API bool g_blockSolve = true;
12901
12902struct b2ContactPositionConstraint {
12903 b2Vec2 localPoints[b2_maxManifoldPoints];
12904 b2Vec2 localNormal;
12905 b2Vec2 localPoint;
12906 int32 indexA;
12907 int32 indexB;
12908 float invMassA, invMassB;
12909 b2Vec2 localCenterA, localCenterB;
12910 float invIA, invIB;
12911 b2Manifold::Type type;
12912 float radiusA, radiusB;
12913 int32 pointCount;
12914};
12915
12916b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) {
12917 m_step = def->step;
12918 m_allocator = def->allocator;
12919 m_count = def->count;
12920 m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
12921 m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
12922 m_positions = def->positions;
12923 m_velocities = def->velocities;
12924 m_contacts = def->contacts;
12925
12926 // Initialize position independent portions of the constraints.
12927 for (int32 i = 0; i < m_count; ++i) {
12928 b2Contact* contact = m_contacts[i];
12929
12930 b2Fixture* fixtureA = contact->m_fixtureA;
12931 b2Fixture* fixtureB = contact->m_fixtureB;
12932 b2Shape* shapeA = fixtureA->GetShape();
12933 b2Shape* shapeB = fixtureB->GetShape();
12934 float radiusA = shapeA->m_radius;
12935 float radiusB = shapeB->m_radius;
12936 b2Body* bodyA = fixtureA->GetBody();
12937 b2Body* bodyB = fixtureB->GetBody();
12938 b2Manifold* manifold = contact->GetManifold();
12939
12940 int32 pointCount = manifold->pointCount;
12941 b2Assert(pointCount > 0);
12942
12943 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
12944 vc->friction = contact->m_friction;
12945 vc->restitution = contact->m_restitution;
12946 vc->threshold = contact->m_restitutionThreshold;
12947 vc->tangentSpeed = contact->m_tangentSpeed;
12948 vc->indexA = bodyA->m_islandIndex;
12949 vc->indexB = bodyB->m_islandIndex;
12950 vc->invMassA = bodyA->m_invMass;
12951 vc->invMassB = bodyB->m_invMass;
12952 vc->invIA = bodyA->m_invI;
12953 vc->invIB = bodyB->m_invI;
12954 vc->contactIndex = i;
12955 vc->pointCount = pointCount;
12956 vc->K.SetZero();
12957 vc->normalMass.SetZero();
12958
12959 b2ContactPositionConstraint* pc = m_positionConstraints + i;
12960 pc->indexA = bodyA->m_islandIndex;
12961 pc->indexB = bodyB->m_islandIndex;
12962 pc->invMassA = bodyA->m_invMass;
12963 pc->invMassB = bodyB->m_invMass;
12964 pc->localCenterA = bodyA->m_sweep.localCenter;
12965 pc->localCenterB = bodyB->m_sweep.localCenter;
12966 pc->invIA = bodyA->m_invI;
12967 pc->invIB = bodyB->m_invI;
12968 pc->localNormal = manifold->localNormal;
12969 pc->localPoint = manifold->localPoint;
12970 pc->pointCount = pointCount;
12971 pc->radiusA = radiusA;
12972 pc->radiusB = radiusB;
12973 pc->type = manifold->type;
12974
12975 for (int32 j = 0; j < pointCount; ++j) {
12976 b2ManifoldPoint* cp = manifold->points + j;
12977 b2VelocityConstraintPoint* vcp = vc->points + j;
12978
12979 if (m_step.warmStarting) {
12980 vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
12981 vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
12982 } else {
12983 vcp->normalImpulse = 0.0f;
12984 vcp->tangentImpulse = 0.0f;
12985 }
12986
12987 vcp->rA.SetZero();
12988 vcp->rB.SetZero();
12989 vcp->normalMass = 0.0f;
12990 vcp->tangentMass = 0.0f;
12991 vcp->velocityBias = 0.0f;
12992
12993 pc->localPoints[j] = cp->localPoint;
12994 }
12995 }
12996}
12997
12998b2ContactSolver::~b2ContactSolver() {
12999 m_allocator->Free(m_velocityConstraints);
13000 m_allocator->Free(m_positionConstraints);
13001}
13002
13003// Initialize position dependent portions of the velocity constraints.
13004void b2ContactSolver::InitializeVelocityConstraints() {
13005 for (int32 i = 0; i < m_count; ++i) {
13006 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
13007 b2ContactPositionConstraint* pc = m_positionConstraints + i;
13008
13009 float radiusA = pc->radiusA;
13010 float radiusB = pc->radiusB;
13011 b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
13012
13013 int32 indexA = vc->indexA;
13014 int32 indexB = vc->indexB;
13015
13016 float mA = vc->invMassA;
13017 float mB = vc->invMassB;
13018 float iA = vc->invIA;
13019 float iB = vc->invIB;
13020 b2Vec2 localCenterA = pc->localCenterA;
13021 b2Vec2 localCenterB = pc->localCenterB;
13022
13023 b2Vec2 cA = m_positions[indexA].c;
13024 float aA = m_positions[indexA].a;
13025 b2Vec2 vA = m_velocities[indexA].v;
13026 float wA = m_velocities[indexA].w;
13027
13028 b2Vec2 cB = m_positions[indexB].c;
13029 float aB = m_positions[indexB].a;
13030 b2Vec2 vB = m_velocities[indexB].v;
13031 float wB = m_velocities[indexB].w;
13032
13033 b2Assert(manifold->pointCount > 0);
13034
13035 b2Transform xfA, xfB;
13036 xfA.q.Set(aA);
13037 xfB.q.Set(aB);
13038 xfA.p = cA - b2Mul(xfA.q, localCenterA);
13039 xfB.p = cB - b2Mul(xfB.q, localCenterB);
13040
13041 b2WorldManifold worldManifold;
13042 worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
13043
13044 vc->normal = worldManifold.normal;
13045
13046 int32 pointCount = vc->pointCount;
13047 for (int32 j = 0; j < pointCount; ++j) {
13048 b2VelocityConstraintPoint* vcp = vc->points + j;
13049
13050 vcp->rA = worldManifold.points[j] - cA;
13051 vcp->rB = worldManifold.points[j] - cB;
13052
13053 float rnA = b2Cross(vcp->rA, vc->normal);
13054 float rnB = b2Cross(vcp->rB, vc->normal);
13055
13056 float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
13057
13058 vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
13059
13060 b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
13061
13062 float rtA = b2Cross(vcp->rA, tangent);
13063 float rtB = b2Cross(vcp->rB, tangent);
13064
13065 float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
13066
13067 vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
13068
13069 // Setup a velocity bias for restitution.
13070 vcp->velocityBias = 0.0f;
13071 float vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
13072 if (vRel < -vc->threshold) {
13073 vcp->velocityBias = -vc->restitution * vRel;
13074 }
13075 }
13076
13077 // If we have two points, then prepare the block solver.
13078 if (vc->pointCount == 2 && g_blockSolve) {
13079 b2VelocityConstraintPoint* vcp1 = vc->points + 0;
13080 b2VelocityConstraintPoint* vcp2 = vc->points + 1;
13081
13082 float rn1A = b2Cross(vcp1->rA, vc->normal);
13083 float rn1B = b2Cross(vcp1->rB, vc->normal);
13084 float rn2A = b2Cross(vcp2->rA, vc->normal);
13085 float rn2B = b2Cross(vcp2->rB, vc->normal);
13086
13087 float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
13088 float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
13089 float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
13090
13091 // Ensure a reasonable condition number.
13092 const float k_maxConditionNumber = 1000.0f;
13093 if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
13094 // K is safe to invert.
13095 vc->K.ex.Set(k11, k12);
13096 vc->K.ey.Set(k12, k22);
13097 vc->normalMass = vc->K.GetInverse();
13098 } else {
13099 // The constraints are redundant, just use one.
13100 // TODO_ERIN use deepest?
13101 vc->pointCount = 1;
13102 }
13103 }
13104 }
13105}
13106
13107void b2ContactSolver::WarmStart() {
13108 // Warm start.
13109 for (int32 i = 0; i < m_count; ++i) {
13110 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
13111
13112 int32 indexA = vc->indexA;
13113 int32 indexB = vc->indexB;
13114 float mA = vc->invMassA;
13115 float iA = vc->invIA;
13116 float mB = vc->invMassB;
13117 float iB = vc->invIB;
13118 int32 pointCount = vc->pointCount;
13119
13120 b2Vec2 vA = m_velocities[indexA].v;
13121 float wA = m_velocities[indexA].w;
13122 b2Vec2 vB = m_velocities[indexB].v;
13123 float wB = m_velocities[indexB].w;
13124
13125 b2Vec2 normal = vc->normal;
13126 b2Vec2 tangent = b2Cross(normal, 1.0f);
13127
13128 for (int32 j = 0; j < pointCount; ++j) {
13129 b2VelocityConstraintPoint* vcp = vc->points + j;
13130 b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
13131 wA -= iA * b2Cross(vcp->rA, P);
13132 vA -= mA * P;
13133 wB += iB * b2Cross(vcp->rB, P);
13134 vB += mB * P;
13135 }
13136
13137 m_velocities[indexA].v = vA;
13138 m_velocities[indexA].w = wA;
13139 m_velocities[indexB].v = vB;
13140 m_velocities[indexB].w = wB;
13141 }
13142}
13143
13144void b2ContactSolver::SolveVelocityConstraints() {
13145 for (int32 i = 0; i < m_count; ++i) {
13146 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
13147
13148 int32 indexA = vc->indexA;
13149 int32 indexB = vc->indexB;
13150 float mA = vc->invMassA;
13151 float iA = vc->invIA;
13152 float mB = vc->invMassB;
13153 float iB = vc->invIB;
13154 int32 pointCount = vc->pointCount;
13155
13156 b2Vec2 vA = m_velocities[indexA].v;
13157 float wA = m_velocities[indexA].w;
13158 b2Vec2 vB = m_velocities[indexB].v;
13159 float wB = m_velocities[indexB].w;
13160
13161 b2Vec2 normal = vc->normal;
13162 b2Vec2 tangent = b2Cross(normal, 1.0f);
13163 float friction = vc->friction;
13164
13165 b2Assert(pointCount == 1 || pointCount == 2);
13166
13167 // Solve tangent constraints first because non-penetration is more important
13168 // than friction.
13169 for (int32 j = 0; j < pointCount; ++j) {
13170 b2VelocityConstraintPoint* vcp = vc->points + j;
13171
13172 // Relative velocity at contact
13173 b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
13174
13175 // Compute tangent force
13176 float vt = b2Dot(dv, tangent) - vc->tangentSpeed;
13177 float lambda = vcp->tangentMass * (-vt);
13178
13179 // b2Clamp the accumulated force
13180 float maxFriction = friction * vcp->normalImpulse;
13181 float newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
13182 lambda = newImpulse - vcp->tangentImpulse;
13183 vcp->tangentImpulse = newImpulse;
13184
13185 // Apply contact impulse
13186 b2Vec2 P = lambda * tangent;
13187
13188 vA -= mA * P;
13189 wA -= iA * b2Cross(vcp->rA, P);
13190
13191 vB += mB * P;
13192 wB += iB * b2Cross(vcp->rB, P);
13193 }
13194
13195 // Solve normal constraints
13196 if (pointCount == 1 || g_blockSolve == false) {
13197 for (int32 j = 0; j < pointCount; ++j) {
13198 b2VelocityConstraintPoint* vcp = vc->points + j;
13199
13200 // Relative velocity at contact
13201 b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
13202
13203 // Compute normal impulse
13204 float vn = b2Dot(dv, normal);
13205 float lambda = -vcp->normalMass * (vn - vcp->velocityBias);
13206
13207 // b2Clamp the accumulated impulse
13208 float newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
13209 lambda = newImpulse - vcp->normalImpulse;
13210 vcp->normalImpulse = newImpulse;
13211
13212 // Apply contact impulse
13213 b2Vec2 P = lambda * normal;
13214 vA -= mA * P;
13215 wA -= iA * b2Cross(vcp->rA, P);
13216
13217 vB += mB * P;
13218 wB += iB * b2Cross(vcp->rB, P);
13219 }
13220 } else {
13221 // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
13222 // Build the mini LCP for this contact patch
13223 //
13224 // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
13225 //
13226 // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
13227 // b = vn0 - velocityBias
13228 //
13229 // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
13230 // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
13231 // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
13232 // solution that satisfies the problem is chosen.
13233 //
13234 // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
13235 // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
13236 //
13237 // Substitute:
13238 //
13239 // x = a + d
13240 //
13241 // a := old total impulse
13242 // x := new total impulse
13243 // d := incremental impulse
13244 //
13245 // For the current iteration we extend the formula for the incremental impulse
13246 // to compute the new total impulse:
13247 //
13248 // vn = A * d + b
13249 // = A * (x - a) + b
13250 // = A * x + b - A * a
13251 // = A * x + b'
13252 // b' = b - A * a;
13253
13254 b2VelocityConstraintPoint* cp1 = vc->points + 0;
13255 b2VelocityConstraintPoint* cp2 = vc->points + 1;
13256
13257 b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
13258 b2Assert(a.x >= 0.0f && a.y >= 0.0f);
13259
13260 // Relative velocity at contact
13261 b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
13262 b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
13263
13264 // Compute normal velocity
13265 float vn1 = b2Dot(dv1, normal);
13266 float vn2 = b2Dot(dv2, normal);
13267
13268 b2Vec2 b;
13269 b.x = vn1 - cp1->velocityBias;
13270 b.y = vn2 - cp2->velocityBias;
13271
13272 // Compute b'
13273 b -= b2Mul(vc->K, a);
13274
13275 const float k_errorTol = 1e-3f;
13276 B2_NOT_USED(k_errorTol);
13277
13278 for (;;) {
13279 //
13280 // Case 1: vn = 0
13281 //
13282 // 0 = A * x + b'
13283 //
13284 // Solve for x:
13285 //
13286 // x = - inv(A) * b'
13287 //
13288 b2Vec2 x = -b2Mul(vc->normalMass, b);
13289
13290 if (x.x >= 0.0f && x.y >= 0.0f) {
13291 // Get the incremental impulse
13292 b2Vec2 d = x - a;
13293
13294 // Apply incremental impulse
13295 b2Vec2 P1 = d.x * normal;
13296 b2Vec2 P2 = d.y * normal;
13297 vA -= mA * (P1 + P2);
13298 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
13299
13300 vB += mB * (P1 + P2);
13301 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
13302
13303 // Accumulate
13304 cp1->normalImpulse = x.x;
13305 cp2->normalImpulse = x.y;
13306
13307#if B2_DEBUG_SOLVER == 1
13308 // Postconditions
13309 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
13310 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
13311
13312 // Compute normal velocity
13313 vn1 = b2Dot(dv1, normal);
13314 vn2 = b2Dot(dv2, normal);
13315
13316 b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
13317 b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
13318#endif
13319 break;
13320 }
13321
13322 //
13323 // Case 2: vn1 = 0 and x2 = 0
13324 //
13325 // 0 = a11 * x1 + a12 * 0 + b1'
13326 // vn2 = a21 * x1 + a22 * 0 + b2'
13327 //
13328 x.x = -cp1->normalMass * b.x;
13329 x.y = 0.0f;
13330 vn1 = 0.0f;
13331 vn2 = vc->K.ex.y * x.x + b.y;
13332 if (x.x >= 0.0f && vn2 >= 0.0f) {
13333 // Get the incremental impulse
13334 b2Vec2 d = x - a;
13335
13336 // Apply incremental impulse
13337 b2Vec2 P1 = d.x * normal;
13338 b2Vec2 P2 = d.y * normal;
13339 vA -= mA * (P1 + P2);
13340 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
13341
13342 vB += mB * (P1 + P2);
13343 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
13344
13345 // Accumulate
13346 cp1->normalImpulse = x.x;
13347 cp2->normalImpulse = x.y;
13348
13349#if B2_DEBUG_SOLVER == 1
13350 // Postconditions
13351 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
13352
13353 // Compute normal velocity
13354 vn1 = b2Dot(dv1, normal);
13355
13356 b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
13357#endif
13358 break;
13359 }
13360
13361
13362 //
13363 // Case 3: vn2 = 0 and x1 = 0
13364 //
13365 // vn1 = a11 * 0 + a12 * x2 + b1'
13366 // 0 = a21 * 0 + a22 * x2 + b2'
13367 //
13368 x.x = 0.0f;
13369 x.y = -cp2->normalMass * b.y;
13370 vn1 = vc->K.ey.x * x.y + b.x;
13371 vn2 = 0.0f;
13372
13373 if (x.y >= 0.0f && vn1 >= 0.0f) {
13374 // Resubstitute for the incremental impulse
13375 b2Vec2 d = x - a;
13376
13377 // Apply incremental impulse
13378 b2Vec2 P1 = d.x * normal;
13379 b2Vec2 P2 = d.y * normal;
13380 vA -= mA * (P1 + P2);
13381 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
13382
13383 vB += mB * (P1 + P2);
13384 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
13385
13386 // Accumulate
13387 cp1->normalImpulse = x.x;
13388 cp2->normalImpulse = x.y;
13389
13390#if B2_DEBUG_SOLVER == 1
13391 // Postconditions
13392 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
13393
13394 // Compute normal velocity
13395 vn2 = b2Dot(dv2, normal);
13396
13397 b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
13398#endif
13399 break;
13400 }
13401
13402 //
13403 // Case 4: x1 = 0 and x2 = 0
13404 //
13405 // vn1 = b1
13406 // vn2 = b2;
13407 x.x = 0.0f;
13408 x.y = 0.0f;
13409 vn1 = b.x;
13410 vn2 = b.y;
13411
13412 if (vn1 >= 0.0f && vn2 >= 0.0f) {
13413 // Resubstitute for the incremental impulse
13414 b2Vec2 d = x - a;
13415
13416 // Apply incremental impulse
13417 b2Vec2 P1 = d.x * normal;
13418 b2Vec2 P2 = d.y * normal;
13419 vA -= mA * (P1 + P2);
13420 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
13421
13422 vB += mB * (P1 + P2);
13423 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
13424
13425 // Accumulate
13426 cp1->normalImpulse = x.x;
13427 cp2->normalImpulse = x.y;
13428
13429 break;
13430 }
13431
13432 // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
13433 break;
13434 }
13435 }
13436
13437 m_velocities[indexA].v = vA;
13438 m_velocities[indexA].w = wA;
13439 m_velocities[indexB].v = vB;
13440 m_velocities[indexB].w = wB;
13441 }
13442}
13443
13444void b2ContactSolver::StoreImpulses() {
13445 for (int32 i = 0; i < m_count; ++i) {
13446 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
13447 b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
13448
13449 for (int32 j = 0; j < vc->pointCount; ++j) {
13450 manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
13451 manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
13452 }
13453 }
13454}
13455
13456struct b2PositionSolverManifold {
13457 void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) {
13458 b2Assert(pc->pointCount > 0);
13459
13460 switch (pc->type) {
13461 case b2Manifold::e_circles:
13462 {
13463 b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
13464 b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
13465 normal = pointB - pointA;
13466 normal.Normalize();
13467 point = 0.5f * (pointA + pointB);
13468 separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
13469 }
13470 break;
13471
13472 case b2Manifold::e_faceA:
13473 {
13474 normal = b2Mul(xfA.q, pc->localNormal);
13475 b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
13476
13477 b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
13478 separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
13479 point = clipPoint;
13480 }
13481 break;
13482
13483 case b2Manifold::e_faceB:
13484 {
13485 normal = b2Mul(xfB.q, pc->localNormal);
13486 b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
13487
13488 b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
13489 separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
13490 point = clipPoint;
13491
13492 // Ensure normal points from A to B
13493 normal = -normal;
13494 }
13495 break;
13496 }
13497 }
13498
13499 b2Vec2 normal;
13500 b2Vec2 point;
13501 float separation;
13502};
13503
13504// Sequential solver.
13505bool b2ContactSolver::SolvePositionConstraints() {
13506 float minSeparation = 0.0f;
13507
13508 for (int32 i = 0; i < m_count; ++i) {
13509 b2ContactPositionConstraint* pc = m_positionConstraints + i;
13510
13511 int32 indexA = pc->indexA;
13512 int32 indexB = pc->indexB;
13513 b2Vec2 localCenterA = pc->localCenterA;
13514 float mA = pc->invMassA;
13515 float iA = pc->invIA;
13516 b2Vec2 localCenterB = pc->localCenterB;
13517 float mB = pc->invMassB;
13518 float iB = pc->invIB;
13519 int32 pointCount = pc->pointCount;
13520
13521 b2Vec2 cA = m_positions[indexA].c;
13522 float aA = m_positions[indexA].a;
13523
13524 b2Vec2 cB = m_positions[indexB].c;
13525 float aB = m_positions[indexB].a;
13526
13527 // Solve normal constraints
13528 for (int32 j = 0; j < pointCount; ++j) {
13529 b2Transform xfA, xfB;
13530 xfA.q.Set(aA);
13531 xfB.q.Set(aB);
13532 xfA.p = cA - b2Mul(xfA.q, localCenterA);
13533 xfB.p = cB - b2Mul(xfB.q, localCenterB);
13534
13535 b2PositionSolverManifold psm;
13536 psm.Initialize(pc, xfA, xfB, j);
13537 b2Vec2 normal = psm.normal;
13538
13539 b2Vec2 point = psm.point;
13540 float separation = psm.separation;
13541
13542 b2Vec2 rA = point - cA;
13543 b2Vec2 rB = point - cB;
13544
13545 // Track max constraint error.
13546 minSeparation = b2Min(minSeparation, separation);
13547
13548 // Prevent large corrections and allow slop.
13549 float C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
13550
13551 // Compute the effective mass.
13552 float rnA = b2Cross(rA, normal);
13553 float rnB = b2Cross(rB, normal);
13554 float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
13555
13556 // Compute normal impulse
13557 float impulse = K > 0.0f ? -C / K : 0.0f;
13558
13559 b2Vec2 P = impulse * normal;
13560
13561 cA -= mA * P;
13562 aA -= iA * b2Cross(rA, P);
13563
13564 cB += mB * P;
13565 aB += iB * b2Cross(rB, P);
13566 }
13567
13568 m_positions[indexA].c = cA;
13569 m_positions[indexA].a = aA;
13570
13571 m_positions[indexB].c = cB;
13572 m_positions[indexB].a = aB;
13573 }
13574
13575 // We can't expect minSpeparation >= -b2_linearSlop because we don't
13576 // push the separation above -b2_linearSlop.
13577 return minSeparation >= -3.0f * b2_linearSlop;
13578}
13579
13580// Sequential position solver for position constraints.
13581bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) {
13582 float minSeparation = 0.0f;
13583
13584 for (int32 i = 0; i < m_count; ++i) {
13585 b2ContactPositionConstraint* pc = m_positionConstraints + i;
13586
13587 int32 indexA = pc->indexA;
13588 int32 indexB = pc->indexB;
13589 b2Vec2 localCenterA = pc->localCenterA;
13590 b2Vec2 localCenterB = pc->localCenterB;
13591 int32 pointCount = pc->pointCount;
13592
13593 float mA = 0.0f;
13594 float iA = 0.0f;
13595 if (indexA == toiIndexA || indexA == toiIndexB) {
13596 mA = pc->invMassA;
13597 iA = pc->invIA;
13598 }
13599
13600 float mB = 0.0f;
13601 float iB = 0.;
13602 if (indexB == toiIndexA || indexB == toiIndexB) {
13603 mB = pc->invMassB;
13604 iB = pc->invIB;
13605 }
13606
13607 b2Vec2 cA = m_positions[indexA].c;
13608 float aA = m_positions[indexA].a;
13609
13610 b2Vec2 cB = m_positions[indexB].c;
13611 float aB = m_positions[indexB].a;
13612
13613 // Solve normal constraints
13614 for (int32 j = 0; j < pointCount; ++j) {
13615 b2Transform xfA, xfB;
13616 xfA.q.Set(aA);
13617 xfB.q.Set(aB);
13618 xfA.p = cA - b2Mul(xfA.q, localCenterA);
13619 xfB.p = cB - b2Mul(xfB.q, localCenterB);
13620
13621 b2PositionSolverManifold psm;
13622 psm.Initialize(pc, xfA, xfB, j);
13623 b2Vec2 normal = psm.normal;
13624
13625 b2Vec2 point = psm.point;
13626 float separation = psm.separation;
13627
13628 b2Vec2 rA = point - cA;
13629 b2Vec2 rB = point - cB;
13630
13631 // Track max constraint error.
13632 minSeparation = b2Min(minSeparation, separation);
13633
13634 // Prevent large corrections and allow slop.
13635 float C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
13636
13637 // Compute the effective mass.
13638 float rnA = b2Cross(rA, normal);
13639 float rnB = b2Cross(rB, normal);
13640 float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
13641
13642 // Compute normal impulse
13643 float impulse = K > 0.0f ? -C / K : 0.0f;
13644
13645 b2Vec2 P = impulse * normal;
13646
13647 cA -= mA * P;
13648 aA -= iA * b2Cross(rA, P);
13649
13650 cB += mB * P;
13651 aB += iB * b2Cross(rB, P);
13652 }
13653
13654 m_positions[indexA].c = cA;
13655 m_positions[indexA].a = aA;
13656
13657 m_positions[indexB].c = cB;
13658 m_positions[indexB].a = aB;
13659 }
13660
13661 // We can't expect minSpeparation >= -b2_linearSlop because we don't
13662 // push the separation above -b2_linearSlop.
13663 return minSeparation >= -1.5f * b2_linearSlop;
13664}
13665// MIT License
13666
13667// Copyright (c) 2019 Erin Catto
13668
13669// Permission is hereby granted, free of charge, to any person obtaining a copy
13670// of this software and associated documentation files (the "Software"), to deal
13671// in the Software without restriction, including without limitation the rights
13672// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13673// copies of the Software, and to permit persons to whom the Software is
13674// furnished to do so, subject to the following conditions:
13675
13676// The above copyright notice and this permission notice shall be included in all
13677// copies or substantial portions of the Software.
13678
13679// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
13680// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
13681// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
13682// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
13683// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
13684// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
13685// SOFTWARE.
13686
13687#include "box2d/b2_body.h"
13688#include "box2d/b2_draw.h"
13689#include "box2d/b2_distance_joint.h"
13690#include "box2d/b2_time_step.h"
13691
13692// 1-D constrained system
13693// m (v2 - v1) = lambda
13694// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
13695// x2 = x1 + h * v2
13696
13697// 1-D mass-damper-spring system
13698// m (v2 - v1) + h * d * v2 + h * k *
13699
13700// C = norm(p2 - p1) - L
13701// u = (p2 - p1) / norm(p2 - p1)
13702// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
13703// J = [-u -cross(r1, u) u cross(r2, u)]
13704// K = J * invM * JT
13705// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
13706
13707
13708void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
13709 const b2Vec2& anchor1, const b2Vec2& anchor2) {
13710 bodyA = b1;
13711 bodyB = b2;
13712 localAnchorA = bodyA->GetLocalPoint(anchor1);
13713 localAnchorB = bodyB->GetLocalPoint(anchor2);
13714 b2Vec2 d = anchor2 - anchor1;
13715 length = b2Max(d.Length(), b2_linearSlop);
13716 minLength = length;
13717 maxLength = length;
13718}
13719
13720b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
13721 : b2Joint(def) {
13722 m_localAnchorA = def->localAnchorA;
13723 m_localAnchorB = def->localAnchorB;
13724 m_length = b2Max(def->length, b2_linearSlop);
13725 m_minLength = b2Max(def->minLength, b2_linearSlop);
13726 m_maxLength = b2Max(def->maxLength, m_minLength);
13727 m_stiffness = def->stiffness;
13728 m_damping = def->damping;
13729
13730 m_gamma = 0.0f;
13731 m_bias = 0.0f;
13732 m_impulse = 0.0f;
13733 m_lowerImpulse = 0.0f;
13734 m_upperImpulse = 0.0f;
13735 m_currentLength = 0.0f;
13736}
13737
13738void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) {
13739 m_indexA = m_bodyA->m_islandIndex;
13740 m_indexB = m_bodyB->m_islandIndex;
13741 m_localCenterA = m_bodyA->m_sweep.localCenter;
13742 m_localCenterB = m_bodyB->m_sweep.localCenter;
13743 m_invMassA = m_bodyA->m_invMass;
13744 m_invMassB = m_bodyB->m_invMass;
13745 m_invIA = m_bodyA->m_invI;
13746 m_invIB = m_bodyB->m_invI;
13747
13748 b2Vec2 cA = data.positions[m_indexA].c;
13749 float aA = data.positions[m_indexA].a;
13750 b2Vec2 vA = data.velocities[m_indexA].v;
13751 float wA = data.velocities[m_indexA].w;
13752
13753 b2Vec2 cB = data.positions[m_indexB].c;
13754 float aB = data.positions[m_indexB].a;
13755 b2Vec2 vB = data.velocities[m_indexB].v;
13756 float wB = data.velocities[m_indexB].w;
13757
13758 b2Rot qA(aA), qB(aB);
13759
13760 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
13761 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
13762 m_u = cB + m_rB - cA - m_rA;
13763
13764 // Handle singularity.
13765 m_currentLength = m_u.Length();
13766 if (m_currentLength > b2_linearSlop) {
13767 m_u *= 1.0f / m_currentLength;
13768 } else {
13769 m_u.Set(0.0f, 0.0f);
13770 m_mass = 0.0f;
13771 m_impulse = 0.0f;
13772 m_lowerImpulse = 0.0f;
13773 m_upperImpulse = 0.0f;
13774 }
13775
13776 float crAu = b2Cross(m_rA, m_u);
13777 float crBu = b2Cross(m_rB, m_u);
13778 float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
13779 m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
13780
13781 if (m_stiffness > 0.0f && m_minLength < m_maxLength) {
13782 // soft
13783 float C = m_currentLength - m_length;
13784
13785 float d = m_damping;
13786 float k = m_stiffness;
13787
13788 // magic formulas
13789 float h = data.step.dt;
13790
13791 // gamma = 1 / (h * (d + h * k))
13792 // the extra factor of h in the denominator is since the lambda is an impulse, not a force
13793 m_gamma = h * (d + h * k);
13794 m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
13795 m_bias = C * h * k * m_gamma;
13796
13797 invMass += m_gamma;
13798 m_softMass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
13799 } else {
13800 // rigid
13801 m_gamma = 0.0f;
13802 m_bias = 0.0f;
13803 m_softMass = m_mass;
13804 }
13805
13806 if (data.step.warmStarting) {
13807 // Scale the impulse to support a variable time step.
13808 m_impulse *= data.step.dtRatio;
13809 m_lowerImpulse *= data.step.dtRatio;
13810 m_upperImpulse *= data.step.dtRatio;
13811
13812 b2Vec2 P = (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u;
13813 vA -= m_invMassA * P;
13814 wA -= m_invIA * b2Cross(m_rA, P);
13815 vB += m_invMassB * P;
13816 wB += m_invIB * b2Cross(m_rB, P);
13817 } else {
13818 m_impulse = 0.0f;
13819 }
13820
13821 data.velocities[m_indexA].v = vA;
13822 data.velocities[m_indexA].w = wA;
13823 data.velocities[m_indexB].v = vB;
13824 data.velocities[m_indexB].w = wB;
13825}
13826
13827void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) {
13828 b2Vec2 vA = data.velocities[m_indexA].v;
13829 float wA = data.velocities[m_indexA].w;
13830 b2Vec2 vB = data.velocities[m_indexB].v;
13831 float wB = data.velocities[m_indexB].w;
13832
13833 if (m_minLength < m_maxLength) {
13834 if (m_stiffness > 0.0f) {
13835 // Cdot = dot(u, v + cross(w, r))
13836 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
13837 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
13838 float Cdot = b2Dot(m_u, vpB - vpA);
13839
13840 float impulse = -m_softMass * (Cdot + m_bias + m_gamma * m_impulse);
13841 m_impulse += impulse;
13842
13843 b2Vec2 P = impulse * m_u;
13844 vA -= m_invMassA * P;
13845 wA -= m_invIA * b2Cross(m_rA, P);
13846 vB += m_invMassB * P;
13847 wB += m_invIB * b2Cross(m_rB, P);
13848 }
13849
13850 // lower
13851 {
13852 float C = m_currentLength - m_minLength;
13853 float bias = b2Max(0.0f, C) * data.step.inv_dt;
13854
13855 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
13856 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
13857 float Cdot = b2Dot(m_u, vpB - vpA);
13858
13859 float impulse = -m_mass * (Cdot + bias);
13860 float oldImpulse = m_lowerImpulse;
13861 m_lowerImpulse = b2Max(0.0f, m_lowerImpulse + impulse);
13862 impulse = m_lowerImpulse - oldImpulse;
13863 b2Vec2 P = impulse * m_u;
13864
13865 vA -= m_invMassA * P;
13866 wA -= m_invIA * b2Cross(m_rA, P);
13867 vB += m_invMassB * P;
13868 wB += m_invIB * b2Cross(m_rB, P);
13869 }
13870
13871 // upper
13872 {
13873 float C = m_maxLength - m_currentLength;
13874 float bias = b2Max(0.0f, C) * data.step.inv_dt;
13875
13876 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
13877 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
13878 float Cdot = b2Dot(m_u, vpA - vpB);
13879
13880 float impulse = -m_mass * (Cdot + bias);
13881 float oldImpulse = m_upperImpulse;
13882 m_upperImpulse = b2Max(0.0f, m_upperImpulse + impulse);
13883 impulse = m_upperImpulse - oldImpulse;
13884 b2Vec2 P = -impulse * m_u;
13885
13886 vA -= m_invMassA * P;
13887 wA -= m_invIA * b2Cross(m_rA, P);
13888 vB += m_invMassB * P;
13889 wB += m_invIB * b2Cross(m_rB, P);
13890 }
13891 } else {
13892 // Equal limits
13893
13894 // Cdot = dot(u, v + cross(w, r))
13895 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
13896 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
13897 float Cdot = b2Dot(m_u, vpB - vpA);
13898
13899 float impulse = -m_mass * Cdot;
13900 m_impulse += impulse;
13901
13902 b2Vec2 P = impulse * m_u;
13903 vA -= m_invMassA * P;
13904 wA -= m_invIA * b2Cross(m_rA, P);
13905 vB += m_invMassB * P;
13906 wB += m_invIB * b2Cross(m_rB, P);
13907 }
13908
13909 data.velocities[m_indexA].v = vA;
13910 data.velocities[m_indexA].w = wA;
13911 data.velocities[m_indexB].v = vB;
13912 data.velocities[m_indexB].w = wB;
13913}
13914
13915bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) {
13916 b2Vec2 cA = data.positions[m_indexA].c;
13917 float aA = data.positions[m_indexA].a;
13918 b2Vec2 cB = data.positions[m_indexB].c;
13919 float aB = data.positions[m_indexB].a;
13920
13921 b2Rot qA(aA), qB(aB);
13922
13923 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
13924 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
13925 b2Vec2 u = cB + rB - cA - rA;
13926
13927 float length = u.Normalize();
13928 float C;
13929 if (m_minLength == m_maxLength) {
13930 C = length - m_minLength;
13931 } else if (length < m_minLength) {
13932 C = length - m_minLength;
13933 } else if (m_maxLength < length) {
13934 C = length - m_maxLength;
13935 } else {
13936 return true;
13937 }
13938
13939 float impulse = -m_mass * C;
13940 b2Vec2 P = impulse * u;
13941
13942 cA -= m_invMassA * P;
13943 aA -= m_invIA * b2Cross(rA, P);
13944 cB += m_invMassB * P;
13945 aB += m_invIB * b2Cross(rB, P);
13946
13947 data.positions[m_indexA].c = cA;
13948 data.positions[m_indexA].a = aA;
13949 data.positions[m_indexB].c = cB;
13950 data.positions[m_indexB].a = aB;
13951
13952 return b2Abs(C) < b2_linearSlop;
13953}
13954
13955b2Vec2 b2DistanceJoint::GetAnchorA() const {
13956 return m_bodyA->GetWorldPoint(m_localAnchorA);
13957}
13958
13959b2Vec2 b2DistanceJoint::GetAnchorB() const {
13960 return m_bodyB->GetWorldPoint(m_localAnchorB);
13961}
13962
13963b2Vec2 b2DistanceJoint::GetReactionForce(float inv_dt) const {
13964 b2Vec2 F = inv_dt * (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u;
13965 return F;
13966}
13967
13968float b2DistanceJoint::GetReactionTorque(float inv_dt) const {
13969 B2_NOT_USED(inv_dt);
13970 return 0.0f;
13971}
13972
13973float b2DistanceJoint::SetLength(float length) {
13974 m_impulse = 0.0f;
13975 m_length = b2Max(b2_linearSlop, length);
13976 return m_length;
13977}
13978
13979float b2DistanceJoint::SetMinLength(float minLength) {
13980 m_lowerImpulse = 0.0f;
13981 m_minLength = b2Clamp(minLength, b2_linearSlop, m_maxLength);
13982 return m_minLength;
13983}
13984
13985float b2DistanceJoint::SetMaxLength(float maxLength) {
13986 m_upperImpulse = 0.0f;
13987 m_maxLength = b2Max(maxLength, m_minLength);
13988 return m_maxLength;
13989}
13990
13991float b2DistanceJoint::GetCurrentLength() const {
13992 b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
13993 b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
13994 b2Vec2 d = pB - pA;
13995 float length = d.Length();
13996 return length;
13997}
13998
13999void b2DistanceJoint::Dump() {
14000 int32 indexA = m_bodyA->m_islandIndex;
14001 int32 indexB = m_bodyB->m_islandIndex;
14002
14003 b2Dump(" b2DistanceJointDef jd;\n");
14004 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
14005 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
14006 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
14007 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
14008 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
14009 b2Dump(" jd.length = %.9g;\n", m_length);
14010 b2Dump(" jd.minLength = %.9g;\n", m_minLength);
14011 b2Dump(" jd.maxLength = %.9g;\n", m_maxLength);
14012 b2Dump(" jd.stiffness = %.9g;\n", m_stiffness);
14013 b2Dump(" jd.damping = %.9g;\n", m_damping);
14014 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
14015}
14016
14017void b2DistanceJoint::Draw(b2Draw* draw) const {
14018 const b2Transform& xfA = m_bodyA->GetTransform();
14019 const b2Transform& xfB = m_bodyB->GetTransform();
14020 b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
14021 b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
14022
14023 b2Vec2 axis = pB - pA;
14024 float length = axis.Normalize();
14025
14026 b2Color c1(0.7f, 0.7f, 0.7f);
14027 b2Color c2(0.3f, 0.9f, 0.3f);
14028 b2Color c3(0.9f, 0.3f, 0.3f);
14029 b2Color c4(0.4f, 0.4f, 0.4f);
14030
14031 draw->DrawSegment(pA, pB, c4);
14032
14033 b2Vec2 pRest = pA + m_length * axis;
14034 draw->DrawPoint(pRest, 8.0f, c1);
14035
14036 if (m_minLength != m_maxLength) {
14037 if (m_minLength > b2_linearSlop) {
14038 b2Vec2 pMin = pA + m_minLength * axis;
14039 draw->DrawPoint(pMin, 4.0f, c2);
14040 }
14041
14042 if (m_maxLength < FLT_MAX) {
14043 b2Vec2 pMax = pA + m_maxLength * axis;
14044 draw->DrawPoint(pMax, 4.0f, c3);
14045 }
14046 }
14047}
14048// MIT License
14049
14050// Copyright (c) 2019 Erin Catto
14051
14052// Permission is hereby granted, free of charge, to any person obtaining a copy
14053// of this software and associated documentation files (the "Software"), to deal
14054// in the Software without restriction, including without limitation the rights
14055// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
14056// copies of the Software, and to permit persons to whom the Software is
14057// furnished to do so, subject to the following conditions:
14058
14059// The above copyright notice and this permission notice shall be included in all
14060// copies or substantial portions of the Software.
14061
14062// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14063// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14064// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
14065// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14066// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
14067// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14068// SOFTWARE.
14069
14070#include "b2_edge_circle_contact.h"
14071
14072#include "box2d/b2_block_allocator.h"
14073#include "box2d/b2_fixture.h"
14074
14075#include <new>
14076
14077b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) {
14078 void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
14079 return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
14080}
14081
14082void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
14083 ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
14084 allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
14085}
14086
14087b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
14088 : b2Contact(fixtureA, 0, fixtureB, 0) {
14089 b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
14090 b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
14091}
14092
14093void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
14094 b2CollideEdgeAndCircle(manifold,
14095 (b2EdgeShape*)m_fixtureA->GetShape(), xfA,
14096 (b2CircleShape*)m_fixtureB->GetShape(), xfB);
14097}
14098// MIT License
14099
14100// Copyright (c) 2019 Erin Catto
14101
14102// Permission is hereby granted, free of charge, to any person obtaining a copy
14103// of this software and associated documentation files (the "Software"), to deal
14104// in the Software without restriction, including without limitation the rights
14105// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
14106// copies of the Software, and to permit persons to whom the Software is
14107// furnished to do so, subject to the following conditions:
14108
14109// The above copyright notice and this permission notice shall be included in all
14110// copies or substantial portions of the Software.
14111
14112// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14113// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14114// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
14115// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14116// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
14117// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14118// SOFTWARE.
14119
14120#include "b2_edge_polygon_contact.h"
14121
14122#include "box2d/b2_block_allocator.h"
14123#include "box2d/b2_fixture.h"
14124
14125#include <new>
14126
14127b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) {
14128 void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
14129 return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
14130}
14131
14132void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
14133 ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
14134 allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
14135}
14136
14137b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
14138 : b2Contact(fixtureA, 0, fixtureB, 0) {
14139 b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
14140 b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
14141}
14142
14143void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
14144 b2CollideEdgeAndPolygon(manifold,
14145 (b2EdgeShape*)m_fixtureA->GetShape(), xfA,
14146 (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
14147}
14148// MIT License
14149
14150// Copyright (c) 2019 Erin Catto
14151
14152// Permission is hereby granted, free of charge, to any person obtaining a copy
14153// of this software and associated documentation files (the "Software"), to deal
14154// in the Software without restriction, including without limitation the rights
14155// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
14156// copies of the Software, and to permit persons to whom the Software is
14157// furnished to do so, subject to the following conditions:
14158
14159// The above copyright notice and this permission notice shall be included in all
14160// copies or substantial portions of the Software.
14161
14162// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14163// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14164// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
14165// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14166// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
14167// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14168// SOFTWARE.
14169
14170#include "box2d/b2_fixture.h"
14171#include "box2d/b2_block_allocator.h"
14172#include "box2d/b2_broad_phase.h"
14173#include "box2d/b2_chain_shape.h"
14174#include "box2d/b2_circle_shape.h"
14175#include "box2d/b2_collision.h"
14176#include "box2d/b2_contact.h"
14177#include "box2d/b2_edge_shape.h"
14178#include "box2d/b2_polygon_shape.h"
14179#include "box2d/b2_world.h"
14180
14181b2Fixture::b2Fixture() {
14182 m_body = nullptr;
14183 m_next = nullptr;
14184 m_proxies = nullptr;
14185 m_proxyCount = 0;
14186 m_shape = nullptr;
14187 m_density = 0.0f;
14188}
14189
14190void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) {
14191 m_userData = def->userData;
14192 m_friction = def->friction;
14193 m_restitution = def->restitution;
14194 m_restitutionThreshold = def->restitutionThreshold;
14195
14196 m_body = body;
14197 m_next = nullptr;
14198
14199 m_filter = def->filter;
14200
14201 m_isSensor = def->isSensor;
14202
14203 m_shape = def->shape->Clone(allocator);
14204
14205 // Reserve proxy space
14206 int32 childCount = m_shape->GetChildCount();
14207 m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy));
14208 for (int32 i = 0; i < childCount; ++i) {
14209 m_proxies[i].fixture = nullptr;
14210 m_proxies[i].proxyId = b2BroadPhase::e_nullProxy;
14211 }
14212 m_proxyCount = 0;
14213
14214 m_density = def->density;
14215}
14216
14217void b2Fixture::Destroy(b2BlockAllocator* allocator) {
14218 // The proxies must be destroyed before calling this.
14219 b2Assert(m_proxyCount == 0);
14220
14221 // Free the proxy array.
14222 int32 childCount = m_shape->GetChildCount();
14223 allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy));
14224 m_proxies = nullptr;
14225
14226 // Free the child shape.
14227 switch (m_shape->m_type) {
14228 case b2Shape::e_circle:
14229 {
14230 b2CircleShape* s = (b2CircleShape*)m_shape;
14231 s->~b2CircleShape();
14232 allocator->Free(s, sizeof(b2CircleShape));
14233 }
14234 break;
14235
14236 case b2Shape::e_edge:
14237 {
14238 b2EdgeShape* s = (b2EdgeShape*)m_shape;
14239 s->~b2EdgeShape();
14240 allocator->Free(s, sizeof(b2EdgeShape));
14241 }
14242 break;
14243
14244 case b2Shape::e_polygon:
14245 {
14246 b2PolygonShape* s = (b2PolygonShape*)m_shape;
14247 s->~b2PolygonShape();
14248 allocator->Free(s, sizeof(b2PolygonShape));
14249 }
14250 break;
14251
14252 case b2Shape::e_chain:
14253 {
14254 b2ChainShape* s = (b2ChainShape*)m_shape;
14255 s->~b2ChainShape();
14256 allocator->Free(s, sizeof(b2ChainShape));
14257 }
14258 break;
14259
14260 default:
14261 b2Assert(false);
14262 break;
14263 }
14264
14265 m_shape = nullptr;
14266}
14267
14268void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) {
14269 b2Assert(m_proxyCount == 0);
14270
14271 // Create proxies in the broad-phase.
14272 m_proxyCount = m_shape->GetChildCount();
14273
14274 for (int32 i = 0; i < m_proxyCount; ++i) {
14275 b2FixtureProxy* proxy = m_proxies + i;
14276 m_shape->ComputeAABB(&proxy->aabb, xf, i);
14277 proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy);
14278 proxy->fixture = this;
14279 proxy->childIndex = i;
14280 }
14281}
14282
14283void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) {
14284 // Destroy proxies in the broad-phase.
14285 for (int32 i = 0; i < m_proxyCount; ++i) {
14286 b2FixtureProxy* proxy = m_proxies + i;
14287 broadPhase->DestroyProxy(proxy->proxyId);
14288 proxy->proxyId = b2BroadPhase::e_nullProxy;
14289 }
14290
14291 m_proxyCount = 0;
14292}
14293
14294void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) {
14295 if (m_proxyCount == 0) {
14296 return;
14297 }
14298
14299 for (int32 i = 0; i < m_proxyCount; ++i) {
14300 b2FixtureProxy* proxy = m_proxies + i;
14301
14302 // Compute an AABB that covers the swept shape (may miss some rotation effect).
14303 b2AABB aabb1, aabb2;
14304 m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex);
14305 m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex);
14306
14307 proxy->aabb.Combine(aabb1, aabb2);
14308
14309 b2Vec2 displacement = aabb2.GetCenter() - aabb1.GetCenter();
14310
14311 broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
14312 }
14313}
14314
14315void b2Fixture::SetFilterData(const b2Filter& filter) {
14316 m_filter = filter;
14317
14318 Refilter();
14319}
14320
14321void b2Fixture::Refilter() {
14322 if (m_body == nullptr) {
14323 return;
14324 }
14325
14326 // Flag associated contacts for filtering.
14327 b2ContactEdge* edge = m_body->GetContactList();
14328 while (edge) {
14329 b2Contact* contact = edge->contact;
14330 b2Fixture* fixtureA = contact->GetFixtureA();
14331 b2Fixture* fixtureB = contact->GetFixtureB();
14332 if (fixtureA == this || fixtureB == this) {
14333 contact->FlagForFiltering();
14334 }
14335
14336 edge = edge->next;
14337 }
14338
14339 b2World* world = m_body->GetWorld();
14340
14341 if (world == nullptr) {
14342 return;
14343 }
14344
14345 // Touch each proxy so that new pairs may be created
14346 b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase;
14347 for (int32 i = 0; i < m_proxyCount; ++i) {
14348 broadPhase->TouchProxy(m_proxies[i].proxyId);
14349 }
14350}
14351
14352void b2Fixture::SetSensor(bool sensor) {
14353 if (sensor != m_isSensor) {
14354 m_body->SetAwake(true);
14355 m_isSensor = sensor;
14356 }
14357}
14358
14359void b2Fixture::Dump(int32 bodyIndex) {
14360 b2Dump(" b2FixtureDef fd;\n");
14361 b2Dump(" fd.friction = %.9g;\n", m_friction);
14362 b2Dump(" fd.restitution = %.9g;\n", m_restitution);
14363 b2Dump(" fd.restitutionThreshold = %.9g;\n", m_restitutionThreshold);
14364 b2Dump(" fd.density = %.9g;\n", m_density);
14365 b2Dump(" fd.isSensor = bool(%d);\n", m_isSensor);
14366 b2Dump(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);
14367 b2Dump(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);
14368 b2Dump(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);
14369
14370 switch (m_shape->m_type) {
14371 case b2Shape::e_circle:
14372 {
14373 b2CircleShape* s = (b2CircleShape*)m_shape;
14374 b2Dump(" b2CircleShape shape;\n");
14375 b2Dump(" shape.m_radius = %.9g;\n", s->m_radius);
14376 b2Dump(" shape.m_p.Set(%.9g, %.9g);\n", s->m_p.x, s->m_p.y);
14377 }
14378 break;
14379
14380 case b2Shape::e_edge:
14381 {
14382 b2EdgeShape* s = (b2EdgeShape*)m_shape;
14383 b2Dump(" b2EdgeShape shape;\n");
14384 b2Dump(" shape.m_radius = %.9g;\n", s->m_radius);
14385 b2Dump(" shape.m_vertex0.Set(%.9g, %.9g);\n", s->m_vertex0.x, s->m_vertex0.y);
14386 b2Dump(" shape.m_vertex1.Set(%.9g, %.9g);\n", s->m_vertex1.x, s->m_vertex1.y);
14387 b2Dump(" shape.m_vertex2.Set(%.9g, %.9g);\n", s->m_vertex2.x, s->m_vertex2.y);
14388 b2Dump(" shape.m_vertex3.Set(%.9g, %.9g);\n", s->m_vertex3.x, s->m_vertex3.y);
14389 b2Dump(" shape.m_oneSided = bool(%d);\n", s->m_oneSided);
14390 }
14391 break;
14392
14393 case b2Shape::e_polygon:
14394 {
14395 b2PolygonShape* s = (b2PolygonShape*)m_shape;
14396 b2Dump(" b2PolygonShape shape;\n");
14397 b2Dump(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
14398 for (int32 i = 0; i < s->m_count; ++i) {
14399 b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
14400 }
14401 b2Dump(" shape.Set(vs, %d);\n", s->m_count);
14402 }
14403 break;
14404
14405 case b2Shape::e_chain:
14406 {
14407 b2ChainShape* s = (b2ChainShape*)m_shape;
14408 b2Dump(" b2ChainShape shape;\n");
14409 b2Dump(" b2Vec2 vs[%d];\n", s->m_count);
14410 for (int32 i = 0; i < s->m_count; ++i) {
14411 b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
14412 }
14413 b2Dump(" shape.CreateChain(vs, %d);\n", s->m_count);
14414 b2Dump(" shape.m_prevVertex.Set(%.9g, %.9g);\n", s->m_prevVertex.x, s->m_prevVertex.y);
14415 b2Dump(" shape.m_nextVertex.Set(%.9g, %.9g);\n", s->m_nextVertex.x, s->m_nextVertex.y);
14416 }
14417 break;
14418
14419 default:
14420 return;
14421 }
14422
14423 b2Dump("\n");
14424 b2Dump(" fd.shape = &shape;\n");
14425 b2Dump("\n");
14426 b2Dump(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex);
14427}
14428// MIT License
14429
14430// Copyright (c) 2019 Erin Catto
14431
14432// Permission is hereby granted, free of charge, to any person obtaining a copy
14433// of this software and associated documentation files (the "Software"), to deal
14434// in the Software without restriction, including without limitation the rights
14435// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
14436// copies of the Software, and to permit persons to whom the Software is
14437// furnished to do so, subject to the following conditions:
14438
14439// The above copyright notice and this permission notice shall be included in all
14440// copies or substantial portions of the Software.
14441
14442// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14443// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14444// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
14445// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14446// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
14447// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14448// SOFTWARE.
14449
14450#include "box2d/b2_friction_joint.h"
14451#include "box2d/b2_body.h"
14452#include "box2d/b2_time_step.h"
14453
14454// Point-to-point constraint
14455// Cdot = v2 - v1
14456// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
14457// J = [-I -r1_skew I r2_skew ]
14458// Identity used:
14459// w k % (rx i + ry j) = w * (-ry i + rx j)
14460
14461// Angle constraint
14462// Cdot = w2 - w1
14463// J = [0 0 -1 0 0 1]
14464// K = invI1 + invI2
14465
14466void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) {
14467 bodyA = bA;
14468 bodyB = bB;
14469 localAnchorA = bodyA->GetLocalPoint(anchor);
14470 localAnchorB = bodyB->GetLocalPoint(anchor);
14471}
14472
14473b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
14474 : b2Joint(def) {
14475 m_localAnchorA = def->localAnchorA;
14476 m_localAnchorB = def->localAnchorB;
14477
14478 m_linearImpulse.SetZero();
14479 m_angularImpulse = 0.0f;
14480
14481 m_maxForce = def->maxForce;
14482 m_maxTorque = def->maxTorque;
14483}
14484
14485void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) {
14486 m_indexA = m_bodyA->m_islandIndex;
14487 m_indexB = m_bodyB->m_islandIndex;
14488 m_localCenterA = m_bodyA->m_sweep.localCenter;
14489 m_localCenterB = m_bodyB->m_sweep.localCenter;
14490 m_invMassA = m_bodyA->m_invMass;
14491 m_invMassB = m_bodyB->m_invMass;
14492 m_invIA = m_bodyA->m_invI;
14493 m_invIB = m_bodyB->m_invI;
14494
14495 float aA = data.positions[m_indexA].a;
14496 b2Vec2 vA = data.velocities[m_indexA].v;
14497 float wA = data.velocities[m_indexA].w;
14498
14499 float aB = data.positions[m_indexB].a;
14500 b2Vec2 vB = data.velocities[m_indexB].v;
14501 float wB = data.velocities[m_indexB].w;
14502
14503 b2Rot qA(aA), qB(aB);
14504
14505 // Compute the effective mass matrix.
14506 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
14507 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
14508
14509 // J = [-I -r1_skew I r2_skew]
14510 // [ 0 -1 0 1]
14511 // r_skew = [-ry; rx]
14512
14513 // Matlab
14514 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
14515 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
14516 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
14517
14518 float mA = m_invMassA, mB = m_invMassB;
14519 float iA = m_invIA, iB = m_invIB;
14520
14521 b2Mat22 K;
14522 K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
14523 K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
14524 K.ey.x = K.ex.y;
14525 K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
14526
14527 m_linearMass = K.GetInverse();
14528
14529 m_angularMass = iA + iB;
14530 if (m_angularMass > 0.0f) {
14531 m_angularMass = 1.0f / m_angularMass;
14532 }
14533
14534 if (data.step.warmStarting) {
14535 // Scale impulses to support a variable time step.
14536 m_linearImpulse *= data.step.dtRatio;
14537 m_angularImpulse *= data.step.dtRatio;
14538
14539 b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
14540 vA -= mA * P;
14541 wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
14542 vB += mB * P;
14543 wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
14544 } else {
14545 m_linearImpulse.SetZero();
14546 m_angularImpulse = 0.0f;
14547 }
14548
14549 data.velocities[m_indexA].v = vA;
14550 data.velocities[m_indexA].w = wA;
14551 data.velocities[m_indexB].v = vB;
14552 data.velocities[m_indexB].w = wB;
14553}
14554
14555void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) {
14556 b2Vec2 vA = data.velocities[m_indexA].v;
14557 float wA = data.velocities[m_indexA].w;
14558 b2Vec2 vB = data.velocities[m_indexB].v;
14559 float wB = data.velocities[m_indexB].w;
14560
14561 float mA = m_invMassA, mB = m_invMassB;
14562 float iA = m_invIA, iB = m_invIB;
14563
14564 float h = data.step.dt;
14565
14566 // Solve angular friction
14567 {
14568 float Cdot = wB - wA;
14569 float impulse = -m_angularMass * Cdot;
14570
14571 float oldImpulse = m_angularImpulse;
14572 float maxImpulse = h * m_maxTorque;
14573 m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
14574 impulse = m_angularImpulse - oldImpulse;
14575
14576 wA -= iA * impulse;
14577 wB += iB * impulse;
14578 }
14579
14580 // Solve linear friction
14581 {
14582 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
14583
14584 b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
14585 b2Vec2 oldImpulse = m_linearImpulse;
14586 m_linearImpulse += impulse;
14587
14588 float maxImpulse = h * m_maxForce;
14589
14590 if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) {
14591 m_linearImpulse.Normalize();
14592 m_linearImpulse *= maxImpulse;
14593 }
14594
14595 impulse = m_linearImpulse - oldImpulse;
14596
14597 vA -= mA * impulse;
14598 wA -= iA * b2Cross(m_rA, impulse);
14599
14600 vB += mB * impulse;
14601 wB += iB * b2Cross(m_rB, impulse);
14602 }
14603
14604 data.velocities[m_indexA].v = vA;
14605 data.velocities[m_indexA].w = wA;
14606 data.velocities[m_indexB].v = vB;
14607 data.velocities[m_indexB].w = wB;
14608}
14609
14610bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) {
14611 B2_NOT_USED(data);
14612
14613 return true;
14614}
14615
14616b2Vec2 b2FrictionJoint::GetAnchorA() const {
14617 return m_bodyA->GetWorldPoint(m_localAnchorA);
14618}
14619
14620b2Vec2 b2FrictionJoint::GetAnchorB() const {
14621 return m_bodyB->GetWorldPoint(m_localAnchorB);
14622}
14623
14624b2Vec2 b2FrictionJoint::GetReactionForce(float inv_dt) const {
14625 return inv_dt * m_linearImpulse;
14626}
14627
14628float b2FrictionJoint::GetReactionTorque(float inv_dt) const {
14629 return inv_dt * m_angularImpulse;
14630}
14631
14632void b2FrictionJoint::SetMaxForce(float force) {
14633 b2Assert(b2IsValid(force) && force >= 0.0f);
14634 m_maxForce = force;
14635}
14636
14637float b2FrictionJoint::GetMaxForce() const {
14638 return m_maxForce;
14639}
14640
14641void b2FrictionJoint::SetMaxTorque(float torque) {
14642 b2Assert(b2IsValid(torque) && torque >= 0.0f);
14643 m_maxTorque = torque;
14644}
14645
14646float b2FrictionJoint::GetMaxTorque() const {
14647 return m_maxTorque;
14648}
14649
14650void b2FrictionJoint::Dump() {
14651 int32 indexA = m_bodyA->m_islandIndex;
14652 int32 indexB = m_bodyB->m_islandIndex;
14653
14654 b2Dump(" b2FrictionJointDef jd;\n");
14655 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
14656 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
14657 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
14658 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
14659 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
14660 b2Dump(" jd.maxForce = %.9g;\n", m_maxForce);
14661 b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque);
14662 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
14663}
14664// MIT License
14665
14666// Copyright (c) 2019 Erin Catto
14667
14668// Permission is hereby granted, free of charge, to any person obtaining a copy
14669// of this software and associated documentation files (the "Software"), to deal
14670// in the Software without restriction, including without limitation the rights
14671// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
14672// copies of the Software, and to permit persons to whom the Software is
14673// furnished to do so, subject to the following conditions:
14674
14675// The above copyright notice and this permission notice shall be included in all
14676// copies or substantial portions of the Software.
14677
14678// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14679// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14680// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
14681// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14682// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
14683// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14684// SOFTWARE.
14685
14686#include "box2d/b2_gear_joint.h"
14687#include "box2d/b2_revolute_joint.h"
14688#include "box2d/b2_prismatic_joint.h"
14689#include "box2d/b2_body.h"
14690#include "box2d/b2_time_step.h"
14691
14692// Gear Joint:
14693// C0 = (coordinate1 + ratio * coordinate2)_initial
14694// C = (coordinate1 + ratio * coordinate2) - C0 = 0
14695// J = [J1 ratio * J2]
14696// K = J * invM * JT
14697// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
14698//
14699// Revolute:
14700// coordinate = rotation
14701// Cdot = angularVelocity
14702// J = [0 0 1]
14703// K = J * invM * JT = invI
14704//
14705// Prismatic:
14706// coordinate = dot(p - pg, ug)
14707// Cdot = dot(v + cross(w, r), ug)
14708// J = [ug cross(r, ug)]
14709// K = J * invM * JT = invMass + invI * cross(r, ug)^2
14710
14711b2GearJoint::b2GearJoint(const b2GearJointDef* def)
14712 : b2Joint(def) {
14713 m_joint1 = def->joint1;
14714 m_joint2 = def->joint2;
14715
14716 m_typeA = m_joint1->GetType();
14717 m_typeB = m_joint2->GetType();
14718
14719 b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
14720 b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
14721
14722 float coordinateA, coordinateB;
14723
14724 // TODO_ERIN there might be some problem with the joint edges in b2Joint.
14725
14726 m_bodyC = m_joint1->GetBodyA();
14727 m_bodyA = m_joint1->GetBodyB();
14728
14729 // Body B on joint1 must be dynamic
14730 b2Assert(m_bodyA->m_type == b2_dynamicBody);
14731
14732 // Get geometry of joint1
14733 b2Transform xfA = m_bodyA->m_xf;
14734 float aA = m_bodyA->m_sweep.a;
14735 b2Transform xfC = m_bodyC->m_xf;
14736 float aC = m_bodyC->m_sweep.a;
14737
14738 if (m_typeA == e_revoluteJoint) {
14739 b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
14740 m_localAnchorC = revolute->m_localAnchorA;
14741 m_localAnchorA = revolute->m_localAnchorB;
14742 m_referenceAngleA = revolute->m_referenceAngle;
14743 m_localAxisC.SetZero();
14744
14745 coordinateA = aA - aC - m_referenceAngleA;
14746 } else {
14747 b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
14748 m_localAnchorC = prismatic->m_localAnchorA;
14749 m_localAnchorA = prismatic->m_localAnchorB;
14750 m_referenceAngleA = prismatic->m_referenceAngle;
14751 m_localAxisC = prismatic->m_localXAxisA;
14752
14753 b2Vec2 pC = m_localAnchorC;
14754 b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
14755 coordinateA = b2Dot(pA - pC, m_localAxisC);
14756 }
14757
14758 m_bodyD = m_joint2->GetBodyA();
14759 m_bodyB = m_joint2->GetBodyB();
14760
14761 // Body B on joint2 must be dynamic
14762 b2Assert(m_bodyB->m_type == b2_dynamicBody);
14763
14764 // Get geometry of joint2
14765 b2Transform xfB = m_bodyB->m_xf;
14766 float aB = m_bodyB->m_sweep.a;
14767 b2Transform xfD = m_bodyD->m_xf;
14768 float aD = m_bodyD->m_sweep.a;
14769
14770 if (m_typeB == e_revoluteJoint) {
14771 b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
14772 m_localAnchorD = revolute->m_localAnchorA;
14773 m_localAnchorB = revolute->m_localAnchorB;
14774 m_referenceAngleB = revolute->m_referenceAngle;
14775 m_localAxisD.SetZero();
14776
14777 coordinateB = aB - aD - m_referenceAngleB;
14778 } else {
14779 b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
14780 m_localAnchorD = prismatic->m_localAnchorA;
14781 m_localAnchorB = prismatic->m_localAnchorB;
14782 m_referenceAngleB = prismatic->m_referenceAngle;
14783 m_localAxisD = prismatic->m_localXAxisA;
14784
14785 b2Vec2 pD = m_localAnchorD;
14786 b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
14787 coordinateB = b2Dot(pB - pD, m_localAxisD);
14788 }
14789
14790 m_ratio = def->ratio;
14791
14792 m_constant = coordinateA + m_ratio * coordinateB;
14793
14794 m_impulse = 0.0f;
14795}
14796
14797void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) {
14798 m_indexA = m_bodyA->m_islandIndex;
14799 m_indexB = m_bodyB->m_islandIndex;
14800 m_indexC = m_bodyC->m_islandIndex;
14801 m_indexD = m_bodyD->m_islandIndex;
14802 m_lcA = m_bodyA->m_sweep.localCenter;
14803 m_lcB = m_bodyB->m_sweep.localCenter;
14804 m_lcC = m_bodyC->m_sweep.localCenter;
14805 m_lcD = m_bodyD->m_sweep.localCenter;
14806 m_mA = m_bodyA->m_invMass;
14807 m_mB = m_bodyB->m_invMass;
14808 m_mC = m_bodyC->m_invMass;
14809 m_mD = m_bodyD->m_invMass;
14810 m_iA = m_bodyA->m_invI;
14811 m_iB = m_bodyB->m_invI;
14812 m_iC = m_bodyC->m_invI;
14813 m_iD = m_bodyD->m_invI;
14814
14815 float aA = data.positions[m_indexA].a;
14816 b2Vec2 vA = data.velocities[m_indexA].v;
14817 float wA = data.velocities[m_indexA].w;
14818
14819 float aB = data.positions[m_indexB].a;
14820 b2Vec2 vB = data.velocities[m_indexB].v;
14821 float wB = data.velocities[m_indexB].w;
14822
14823 float aC = data.positions[m_indexC].a;
14824 b2Vec2 vC = data.velocities[m_indexC].v;
14825 float wC = data.velocities[m_indexC].w;
14826
14827 float aD = data.positions[m_indexD].a;
14828 b2Vec2 vD = data.velocities[m_indexD].v;
14829 float wD = data.velocities[m_indexD].w;
14830
14831 b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
14832
14833 m_mass = 0.0f;
14834
14835 if (m_typeA == e_revoluteJoint) {
14836 m_JvAC.SetZero();
14837 m_JwA = 1.0f;
14838 m_JwC = 1.0f;
14839 m_mass += m_iA + m_iC;
14840 } else {
14841 b2Vec2 u = b2Mul(qC, m_localAxisC);
14842 b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
14843 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
14844 m_JvAC = u;
14845 m_JwC = b2Cross(rC, u);
14846 m_JwA = b2Cross(rA, u);
14847 m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
14848 }
14849
14850 if (m_typeB == e_revoluteJoint) {
14851 m_JvBD.SetZero();
14852 m_JwB = m_ratio;
14853 m_JwD = m_ratio;
14854 m_mass += m_ratio * m_ratio * (m_iB + m_iD);
14855 } else {
14856 b2Vec2 u = b2Mul(qD, m_localAxisD);
14857 b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
14858 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
14859 m_JvBD = m_ratio * u;
14860 m_JwD = m_ratio * b2Cross(rD, u);
14861 m_JwB = m_ratio * b2Cross(rB, u);
14862 m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
14863 }
14864
14865 // Compute effective mass.
14866 m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
14867
14868 if (data.step.warmStarting) {
14869 vA += (m_mA * m_impulse) * m_JvAC;
14870 wA += m_iA * m_impulse * m_JwA;
14871 vB += (m_mB * m_impulse) * m_JvBD;
14872 wB += m_iB * m_impulse * m_JwB;
14873 vC -= (m_mC * m_impulse) * m_JvAC;
14874 wC -= m_iC * m_impulse * m_JwC;
14875 vD -= (m_mD * m_impulse) * m_JvBD;
14876 wD -= m_iD * m_impulse * m_JwD;
14877 } else {
14878 m_impulse = 0.0f;
14879 }
14880
14881 data.velocities[m_indexA].v = vA;
14882 data.velocities[m_indexA].w = wA;
14883 data.velocities[m_indexB].v = vB;
14884 data.velocities[m_indexB].w = wB;
14885 data.velocities[m_indexC].v = vC;
14886 data.velocities[m_indexC].w = wC;
14887 data.velocities[m_indexD].v = vD;
14888 data.velocities[m_indexD].w = wD;
14889}
14890
14891void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) {
14892 b2Vec2 vA = data.velocities[m_indexA].v;
14893 float wA = data.velocities[m_indexA].w;
14894 b2Vec2 vB = data.velocities[m_indexB].v;
14895 float wB = data.velocities[m_indexB].w;
14896 b2Vec2 vC = data.velocities[m_indexC].v;
14897 float wC = data.velocities[m_indexC].w;
14898 b2Vec2 vD = data.velocities[m_indexD].v;
14899 float wD = data.velocities[m_indexD].w;
14900
14901 float Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
14902 Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
14903
14904 float impulse = -m_mass * Cdot;
14905 m_impulse += impulse;
14906
14907 vA += (m_mA * impulse) * m_JvAC;
14908 wA += m_iA * impulse * m_JwA;
14909 vB += (m_mB * impulse) * m_JvBD;
14910 wB += m_iB * impulse * m_JwB;
14911 vC -= (m_mC * impulse) * m_JvAC;
14912 wC -= m_iC * impulse * m_JwC;
14913 vD -= (m_mD * impulse) * m_JvBD;
14914 wD -= m_iD * impulse * m_JwD;
14915
14916 data.velocities[m_indexA].v = vA;
14917 data.velocities[m_indexA].w = wA;
14918 data.velocities[m_indexB].v = vB;
14919 data.velocities[m_indexB].w = wB;
14920 data.velocities[m_indexC].v = vC;
14921 data.velocities[m_indexC].w = wC;
14922 data.velocities[m_indexD].v = vD;
14923 data.velocities[m_indexD].w = wD;
14924}
14925
14926bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) {
14927 b2Vec2 cA = data.positions[m_indexA].c;
14928 float aA = data.positions[m_indexA].a;
14929 b2Vec2 cB = data.positions[m_indexB].c;
14930 float aB = data.positions[m_indexB].a;
14931 b2Vec2 cC = data.positions[m_indexC].c;
14932 float aC = data.positions[m_indexC].a;
14933 b2Vec2 cD = data.positions[m_indexD].c;
14934 float aD = data.positions[m_indexD].a;
14935
14936 b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
14937
14938 float linearError = 0.0f;
14939
14940 float coordinateA, coordinateB;
14941
14942 b2Vec2 JvAC, JvBD;
14943 float JwA, JwB, JwC, JwD;
14944 float mass = 0.0f;
14945
14946 if (m_typeA == e_revoluteJoint) {
14947 JvAC.SetZero();
14948 JwA = 1.0f;
14949 JwC = 1.0f;
14950 mass += m_iA + m_iC;
14951
14952 coordinateA = aA - aC - m_referenceAngleA;
14953 } else {
14954 b2Vec2 u = b2Mul(qC, m_localAxisC);
14955 b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
14956 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
14957 JvAC = u;
14958 JwC = b2Cross(rC, u);
14959 JwA = b2Cross(rA, u);
14960 mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
14961
14962 b2Vec2 pC = m_localAnchorC - m_lcC;
14963 b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
14964 coordinateA = b2Dot(pA - pC, m_localAxisC);
14965 }
14966
14967 if (m_typeB == e_revoluteJoint) {
14968 JvBD.SetZero();
14969 JwB = m_ratio;
14970 JwD = m_ratio;
14971 mass += m_ratio * m_ratio * (m_iB + m_iD);
14972
14973 coordinateB = aB - aD - m_referenceAngleB;
14974 } else {
14975 b2Vec2 u = b2Mul(qD, m_localAxisD);
14976 b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
14977 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
14978 JvBD = m_ratio * u;
14979 JwD = m_ratio * b2Cross(rD, u);
14980 JwB = m_ratio * b2Cross(rB, u);
14981 mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
14982
14983 b2Vec2 pD = m_localAnchorD - m_lcD;
14984 b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
14985 coordinateB = b2Dot(pB - pD, m_localAxisD);
14986 }
14987
14988 float C = (coordinateA + m_ratio * coordinateB) - m_constant;
14989
14990 float impulse = 0.0f;
14991 if (mass > 0.0f) {
14992 impulse = -C / mass;
14993 }
14994
14995 cA += m_mA * impulse * JvAC;
14996 aA += m_iA * impulse * JwA;
14997 cB += m_mB * impulse * JvBD;
14998 aB += m_iB * impulse * JwB;
14999 cC -= m_mC * impulse * JvAC;
15000 aC -= m_iC * impulse * JwC;
15001 cD -= m_mD * impulse * JvBD;
15002 aD -= m_iD * impulse * JwD;
15003
15004 data.positions[m_indexA].c = cA;
15005 data.positions[m_indexA].a = aA;
15006 data.positions[m_indexB].c = cB;
15007 data.positions[m_indexB].a = aB;
15008 data.positions[m_indexC].c = cC;
15009 data.positions[m_indexC].a = aC;
15010 data.positions[m_indexD].c = cD;
15011 data.positions[m_indexD].a = aD;
15012
15013 // TODO_ERIN not implemented
15014 return linearError < b2_linearSlop;
15015}
15016
15017b2Vec2 b2GearJoint::GetAnchorA() const {
15018 return m_bodyA->GetWorldPoint(m_localAnchorA);
15019}
15020
15021b2Vec2 b2GearJoint::GetAnchorB() const {
15022 return m_bodyB->GetWorldPoint(m_localAnchorB);
15023}
15024
15025b2Vec2 b2GearJoint::GetReactionForce(float inv_dt) const {
15026 b2Vec2 P = m_impulse * m_JvAC;
15027 return inv_dt * P;
15028}
15029
15030float b2GearJoint::GetReactionTorque(float inv_dt) const {
15031 float L = m_impulse * m_JwA;
15032 return inv_dt * L;
15033}
15034
15035void b2GearJoint::SetRatio(float ratio) {
15036 b2Assert(b2IsValid(ratio));
15037 m_ratio = ratio;
15038}
15039
15040float b2GearJoint::GetRatio() const {
15041 return m_ratio;
15042}
15043
15044void b2GearJoint::Dump() {
15045 int32 indexA = m_bodyA->m_islandIndex;
15046 int32 indexB = m_bodyB->m_islandIndex;
15047
15048 int32 index1 = m_joint1->m_index;
15049 int32 index2 = m_joint2->m_index;
15050
15051 b2Dump(" b2GearJointDef jd;\n");
15052 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
15053 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
15054 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
15055 b2Dump(" jd.joint1 = joints[%d];\n", index1);
15056 b2Dump(" jd.joint2 = joints[%d];\n", index2);
15057 b2Dump(" jd.ratio = %.9g;\n", m_ratio);
15058 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
15059}
15060// MIT License
15061
15062// Copyright (c) 2019 Erin Catto
15063
15064// Permission is hereby granted, free of charge, to any person obtaining a copy
15065// of this software and associated documentation files (the "Software"), to deal
15066// in the Software without restriction, including without limitation the rights
15067// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15068// copies of the Software, and to permit persons to whom the Software is
15069// furnished to do so, subject to the following conditions:
15070
15071// The above copyright notice and this permission notice shall be included in all
15072// copies or substantial portions of the Software.
15073
15074// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15075// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15076// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
15077// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
15078// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15079// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
15080// SOFTWARE.
15081
15082#include "box2d/b2_body.h"
15083#include "box2d/b2_contact.h"
15084#include "box2d/b2_distance.h"
15085#include "box2d/b2_fixture.h"
15086#include "box2d/b2_joint.h"
15087#include "box2d/b2_stack_allocator.h"
15088#include "box2d/b2_timer.h"
15089#include "box2d/b2_world.h"
15090
15091#include "b2_island.h"
15092#include "dynamics/b2_contact_solver.h"
15093
15094/*
15095Position Correction Notes
15096=========================
15097I tried the several algorithms for position correction of the 2D revolute joint.
15098I looked at these systems:
15099- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
15100- suspension bridge with 30 1m long planks of length 1m.
15101- multi-link chain with 30 1m long links.
15102
15103Here are the algorithms:
15104
15105Baumgarte - A fraction of the position error is added to the velocity error. There is no
15106separate position solver.
15107
15108Pseudo Velocities - After the velocity solver and position integration,
15109the position error, Jacobian, and effective mass are recomputed. Then
15110the velocity constraints are solved with pseudo velocities and a fraction
15111of the position error is added to the pseudo velocity error. The pseudo
15112velocities are initialized to zero and there is no warm-starting. After
15113the position solver, the pseudo velocities are added to the positions.
15114This is also called the First Order World method or the Position LCP method.
15115
15116Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
15117position error is re-computed for each constraint and the positions are updated
15118after the constraint is solved. The radius vectors (aka Jacobians) are
15119re-computed too (otherwise the algorithm has horrible instability). The pseudo
15120velocity states are not needed because they are effectively zero at the beginning
15121of each iteration. Since we have the current position error, we allow the
15122iterations to terminate early if the error becomes smaller than b2_linearSlop.
15123
15124Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
15125each time a constraint is solved.
15126
15127Here are the results:
15128Baumgarte - this is the cheapest algorithm but it has some stability problems,
15129especially with the bridge. The chain links separate easily close to the root
15130and they jitter as they struggle to pull together. This is one of the most common
15131methods in the field. The big drawback is that the position correction artificially
15132affects the momentum, thus leading to instabilities and false bounce. I used a
15133bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
15134factor makes joints and contacts more spongy.
15135
15136Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
15137stable. However, joints still separate with large angular velocities. Drag the
15138simple pendulum in a circle quickly and the joint will separate. The chain separates
15139easily and does not recover. I used a bias factor of 0.2. A larger value lead to
15140the bridge collapsing when a heavy cube drops on it.
15141
15142Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
15143Velocities, but in other ways it is worse. The bridge and chain are much more
15144stable, but the simple pendulum goes unstable at high angular velocities.
15145
15146Full NGS - stable in all tests. The joints display good stiffness. The bridge
15147still sags, but this is better than infinite forces.
15148
15149Recommendations
15150Pseudo Velocities are not really worthwhile because the bridge and chain cannot
15151recover from joint separation. In other cases the benefit over Baumgarte is small.
15152
15153Modified NGS is not a robust method for the revolute joint due to the violent
15154instability seen in the simple pendulum. Perhaps it is viable with other constraint
15155types, especially scalar constraints where the effective mass is a scalar.
15156
15157This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
15158and is very fast. I don't think we can escape Baumgarte, especially in highly
15159demanding cases where high constraint fidelity is not needed.
15160
15161Full NGS is robust and easy on the eyes. I recommend this as an option for
15162higher fidelity simulation and certainly for suspension bridges and long chains.
15163Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
15164joint separation can be problematic. The number of NGS iterations can be reduced
15165for better performance without harming robustness much.
15166
15167Each joint in a can be handled differently in the position solver. So I recommend
15168a system where the user can select the algorithm on a per joint basis. I would
15169probably default to the slower Full NGS and let the user select the faster
15170Baumgarte method in performance critical scenarios.
15171*/
15172
15173/*
15174Cache Performance
15175
15176The Box2D solvers are dominated by cache misses. Data structures are designed
15177to increase the number of cache hits. Much of misses are due to random access
15178to body data. The constraint structures are iterated over linearly, which leads
15179to few cache misses.
15180
15181The bodies are not accessed during iteration. Instead read only data, such as
15182the mass values are stored with the constraints. The mutable data are the constraint
15183impulses and the bodies velocities/positions. The impulses are held inside the
15184constraint structures. The body velocities/positions are held in compact, temporary
15185arrays to increase the number of cache hits. Linear and angular velocity are
15186stored in a single array since multiple arrays lead to multiple misses.
15187*/
15188
15189/*
151902D Rotation
15191
15192R = [cos(theta) -sin(theta)]
15193 [sin(theta) cos(theta) ]
15194
15195thetaDot = omega
15196
15197Let q1 = cos(theta), q2 = sin(theta).
15198R = [q1 -q2]
15199 [q2 q1]
15200
15201q1Dot = -thetaDot * q2
15202q2Dot = thetaDot * q1
15203
15204q1_new = q1_old - dt * w * q2
15205q2_new = q2_old + dt * w * q1
15206then normalize.
15207
15208This might be faster than computing sin+cos.
15209However, we can compute sin+cos of the same angle fast.
15210*/
15211
15212b2Island::b2Island(
15213 int32 bodyCapacity,
15214 int32 contactCapacity,
15215 int32 jointCapacity,
15216 b2StackAllocator* allocator,
15217 b2ContactListener* listener) {
15218 m_bodyCapacity = bodyCapacity;
15219 m_contactCapacity = contactCapacity;
15220 m_jointCapacity = jointCapacity;
15221 m_bodyCount = 0;
15222 m_contactCount = 0;
15223 m_jointCount = 0;
15224
15225 m_allocator = allocator;
15226 m_listener = listener;
15227
15228 m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
15229 m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
15230 m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
15231
15232 m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
15233 m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
15234}
15235
15236b2Island::~b2Island() {
15237 // Warning: the order should reverse the constructor order.
15238 m_allocator->Free(m_positions);
15239 m_allocator->Free(m_velocities);
15240 m_allocator->Free(m_joints);
15241 m_allocator->Free(m_contacts);
15242 m_allocator->Free(m_bodies);
15243}
15244
15245void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) {
15246 b2Timer timer;
15247
15248 float h = step.dt;
15249
15250 // Integrate velocities and apply damping. Initialize the body state.
15251 for (int32 i = 0; i < m_bodyCount; ++i) {
15252 b2Body* b = m_bodies[i];
15253
15254 b2Vec2 c = b->m_sweep.c;
15255 float a = b->m_sweep.a;
15256 b2Vec2 v = b->m_linearVelocity;
15257 float w = b->m_angularVelocity;
15258
15259 // Store positions for continuous collision.
15260 b->m_sweep.c0 = b->m_sweep.c;
15261 b->m_sweep.a0 = b->m_sweep.a;
15262
15263 if (b->m_type == b2_dynamicBody) {
15264 // Integrate velocities.
15265 v += h * b->m_invMass * (b->m_gravityScale * b->m_mass * gravity + b->m_force);
15266 w += h * b->m_invI * b->m_torque;
15267
15268 // Apply damping.
15269 // ODE: dv/dt + c * v = 0
15270 // Solution: v(t) = v0 * exp(-c * t)
15271 // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
15272 // v2 = exp(-c * dt) * v1
15273 // Pade approximation:
15274 // v2 = v1 * 1 / (1 + c * dt)
15275 v *= 1.0f / (1.0f + h * b->m_linearDamping);
15276 w *= 1.0f / (1.0f + h * b->m_angularDamping);
15277 }
15278
15279 m_positions[i].c = c;
15280 m_positions[i].a = a;
15281 m_velocities[i].v = v;
15282 m_velocities[i].w = w;
15283 }
15284
15285 timer.Reset();
15286
15287 // Solver data
15288 b2SolverData solverData;
15289 solverData.step = step;
15290 solverData.positions = m_positions;
15291 solverData.velocities = m_velocities;
15292
15293 // Initialize velocity constraints.
15294 b2ContactSolverDef contactSolverDef;
15295 contactSolverDef.step = step;
15296 contactSolverDef.contacts = m_contacts;
15297 contactSolverDef.count = m_contactCount;
15298 contactSolverDef.positions = m_positions;
15299 contactSolverDef.velocities = m_velocities;
15300 contactSolverDef.allocator = m_allocator;
15301
15302 b2ContactSolver contactSolver(&contactSolverDef);
15303 contactSolver.InitializeVelocityConstraints();
15304
15305 if (step.warmStarting) {
15306 contactSolver.WarmStart();
15307 }
15308
15309 for (int32 i = 0; i < m_jointCount; ++i) {
15310 m_joints[i]->InitVelocityConstraints(solverData);
15311 }
15312
15313 profile->solveInit = timer.GetMilliseconds();
15314
15315 // Solve velocity constraints
15316 timer.Reset();
15317 for (int32 i = 0; i < step.velocityIterations; ++i) {
15318 for (int32 j = 0; j < m_jointCount; ++j) {
15319 m_joints[j]->SolveVelocityConstraints(solverData);
15320 }
15321
15322 contactSolver.SolveVelocityConstraints();
15323 }
15324
15325 // Store impulses for warm starting
15326 contactSolver.StoreImpulses();
15327 profile->solveVelocity = timer.GetMilliseconds();
15328
15329 // Integrate positions
15330 for (int32 i = 0; i < m_bodyCount; ++i) {
15331 b2Vec2 c = m_positions[i].c;
15332 float a = m_positions[i].a;
15333 b2Vec2 v = m_velocities[i].v;
15334 float w = m_velocities[i].w;
15335
15336 // Check for large velocities
15337 b2Vec2 translation = h * v;
15338 if (b2Dot(translation, translation) > b2_maxTranslationSquared) {
15339 float ratio = b2_maxTranslation / translation.Length();
15340 v *= ratio;
15341 }
15342
15343 float rotation = h * w;
15344 if (rotation * rotation > b2_maxRotationSquared) {
15345 float ratio = b2_maxRotation / b2Abs(rotation);
15346 w *= ratio;
15347 }
15348
15349 // Integrate
15350 c += h * v;
15351 a += h * w;
15352
15353 m_positions[i].c = c;
15354 m_positions[i].a = a;
15355 m_velocities[i].v = v;
15356 m_velocities[i].w = w;
15357 }
15358
15359 // Solve position constraints
15360 timer.Reset();
15361 bool positionSolved = false;
15362 for (int32 i = 0; i < step.positionIterations; ++i) {
15363 bool contactsOkay = contactSolver.SolvePositionConstraints();
15364
15365 bool jointsOkay = true;
15366 for (int32 j = 0; j < m_jointCount; ++j) {
15367 bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData);
15368 jointsOkay = jointsOkay && jointOkay;
15369 }
15370
15371 if (contactsOkay && jointsOkay) {
15372 // Exit early if the position errors are small.
15373 positionSolved = true;
15374 break;
15375 }
15376 }
15377
15378 // Copy state buffers back to the bodies
15379 for (int32 i = 0; i < m_bodyCount; ++i) {
15380 b2Body* body = m_bodies[i];
15381 body->m_sweep.c = m_positions[i].c;
15382 body->m_sweep.a = m_positions[i].a;
15383 body->m_linearVelocity = m_velocities[i].v;
15384 body->m_angularVelocity = m_velocities[i].w;
15385 body->SynchronizeTransform();
15386 }
15387
15388 profile->solvePosition = timer.GetMilliseconds();
15389
15390 Report(contactSolver.m_velocityConstraints);
15391
15392 if (allowSleep) {
15393 float minSleepTime = b2_maxFloat;
15394
15395 const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
15396 const float angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
15397
15398 for (int32 i = 0; i < m_bodyCount; ++i) {
15399 b2Body* b = m_bodies[i];
15400 if (b->GetType() == b2_staticBody) {
15401 continue;
15402 }
15403
15404 if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
15405 b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
15406 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) {
15407 b->m_sleepTime = 0.0f;
15408 minSleepTime = 0.0f;
15409 } else {
15410 b->m_sleepTime += h;
15411 minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
15412 }
15413 }
15414
15415 if (minSleepTime >= b2_timeToSleep && positionSolved) {
15416 for (int32 i = 0; i < m_bodyCount; ++i) {
15417 b2Body* b = m_bodies[i];
15418 b->SetAwake(false);
15419 }
15420 }
15421 }
15422}
15423
15424void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) {
15425 b2Assert(toiIndexA < m_bodyCount);
15426 b2Assert(toiIndexB < m_bodyCount);
15427
15428 // Initialize the body state.
15429 for (int32 i = 0; i < m_bodyCount; ++i) {
15430 b2Body* b = m_bodies[i];
15431 m_positions[i].c = b->m_sweep.c;
15432 m_positions[i].a = b->m_sweep.a;
15433 m_velocities[i].v = b->m_linearVelocity;
15434 m_velocities[i].w = b->m_angularVelocity;
15435 }
15436
15437 b2ContactSolverDef contactSolverDef;
15438 contactSolverDef.contacts = m_contacts;
15439 contactSolverDef.count = m_contactCount;
15440 contactSolverDef.allocator = m_allocator;
15441 contactSolverDef.step = subStep;
15442 contactSolverDef.positions = m_positions;
15443 contactSolverDef.velocities = m_velocities;
15444 b2ContactSolver contactSolver(&contactSolverDef);
15445
15446 // Solve position constraints.
15447 for (int32 i = 0; i < subStep.positionIterations; ++i) {
15448 bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
15449 if (contactsOkay) {
15450 break;
15451 }
15452 }
15453
15454#if 0
15455 // Is the new position really safe?
15456 for (int32 i = 0; i < m_contactCount; ++i) {
15457 b2Contact* c = m_contacts[i];
15458 b2Fixture* fA = c->GetFixtureA();
15459 b2Fixture* fB = c->GetFixtureB();
15460
15461 b2Body* bA = fA->GetBody();
15462 b2Body* bB = fB->GetBody();
15463
15464 int32 indexA = c->GetChildIndexA();
15465 int32 indexB = c->GetChildIndexB();
15466
15467 b2DistanceInput input;
15468 input.proxyA.Set(fA->GetShape(), indexA);
15469 input.proxyB.Set(fB->GetShape(), indexB);
15470 input.transformA = bA->GetTransform();
15471 input.transformB = bB->GetTransform();
15472 input.useRadii = false;
15473
15474 b2DistanceOutput output;
15475 b2SimplexCache cache;
15476 cache.count = 0;
15477 b2Distance(&output, &cache, &input);
15478
15479 if (output.distance == 0 || cache.count == 3) {
15480 cache.count += 0;
15481 }
15482 }
15483#endif
15484
15485 // Leap of faith to new safe state.
15486 m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
15487 m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
15488 m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
15489 m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
15490
15491 // No warm starting is needed for TOI events because warm
15492 // starting impulses were applied in the discrete solver.
15493 contactSolver.InitializeVelocityConstraints();
15494
15495 // Solve velocity constraints.
15496 for (int32 i = 0; i < subStep.velocityIterations; ++i) {
15497 contactSolver.SolveVelocityConstraints();
15498 }
15499
15500 // Don't store the TOI contact forces for warm starting
15501 // because they can be quite large.
15502
15503 float h = subStep.dt;
15504
15505 // Integrate positions
15506 for (int32 i = 0; i < m_bodyCount; ++i) {
15507 b2Vec2 c = m_positions[i].c;
15508 float a = m_positions[i].a;
15509 b2Vec2 v = m_velocities[i].v;
15510 float w = m_velocities[i].w;
15511
15512 // Check for large velocities
15513 b2Vec2 translation = h * v;
15514 if (b2Dot(translation, translation) > b2_maxTranslationSquared) {
15515 float ratio = b2_maxTranslation / translation.Length();
15516 v *= ratio;
15517 }
15518
15519 float rotation = h * w;
15520 if (rotation * rotation > b2_maxRotationSquared) {
15521 float ratio = b2_maxRotation / b2Abs(rotation);
15522 w *= ratio;
15523 }
15524
15525 // Integrate
15526 c += h * v;
15527 a += h * w;
15528
15529 m_positions[i].c = c;
15530 m_positions[i].a = a;
15531 m_velocities[i].v = v;
15532 m_velocities[i].w = w;
15533
15534 // Sync bodies
15535 b2Body* body = m_bodies[i];
15536 body->m_sweep.c = c;
15537 body->m_sweep.a = a;
15538 body->m_linearVelocity = v;
15539 body->m_angularVelocity = w;
15540 body->SynchronizeTransform();
15541 }
15542
15543 Report(contactSolver.m_velocityConstraints);
15544}
15545
15546void b2Island::Report(const b2ContactVelocityConstraint* constraints) {
15547 if (m_listener == nullptr) {
15548 return;
15549 }
15550
15551 for (int32 i = 0; i < m_contactCount; ++i) {
15552 b2Contact* c = m_contacts[i];
15553
15554 const b2ContactVelocityConstraint* vc = constraints + i;
15555
15556 b2ContactImpulse impulse;
15557 impulse.count = vc->pointCount;
15558 for (int32 j = 0; j < vc->pointCount; ++j) {
15559 impulse.normalImpulses[j] = vc->points[j].normalImpulse;
15560 impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
15561 }
15562
15563 m_listener->PostSolve(c, &impulse);
15564 }
15565}
15566// MIT License
15567
15568// Copyright (c) 2019 Erin Catto
15569
15570// Permission is hereby granted, free of charge, to any person obtaining a copy
15571// of this software and associated documentation files (the "Software"), to deal
15572// in the Software without restriction, including without limitation the rights
15573// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15574// copies of the Software, and to permit persons to whom the Software is
15575// furnished to do so, subject to the following conditions:
15576
15577// The above copyright notice and this permission notice shall be included in all
15578// copies or substantial portions of the Software.
15579
15580// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15581// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15582// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
15583// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
15584// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15585// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
15586// SOFTWARE.
15587
15588#include "box2d/b2_block_allocator.h"
15589#include "box2d/b2_body.h"
15590#include "box2d/b2_distance_joint.h"
15591#include "box2d/b2_draw.h"
15592#include "box2d/b2_friction_joint.h"
15593#include "box2d/b2_gear_joint.h"
15594#include "box2d/b2_motor_joint.h"
15595#include "box2d/b2_mouse_joint.h"
15596#include "box2d/b2_prismatic_joint.h"
15597#include "box2d/b2_pulley_joint.h"
15598#include "box2d/b2_revolute_joint.h"
15599#include "box2d/b2_weld_joint.h"
15600#include "box2d/b2_wheel_joint.h"
15601#include "box2d/b2_world.h"
15602
15603#include <new>
15604
15605void b2LinearStiffness(float& stiffness, float& damping,
15606 float frequencyHertz, float dampingRatio,
15607 const b2Body* bodyA, const b2Body* bodyB) {
15608 float massA = bodyA->GetMass();
15609 float massB = bodyB->GetMass();
15610 float mass;
15611 if (massA > 0.0f && massB > 0.0f) {
15612 mass = massA * massB / (massA + massB);
15613 } else if (massA > 0.0f) {
15614 mass = massA;
15615 } else {
15616 mass = massB;
15617 }
15618
15619 float omega = 2.0f * b2_pi * frequencyHertz;
15620 stiffness = mass * omega * omega;
15621 damping = 2.0f * mass * dampingRatio * omega;
15622}
15623
15624void b2AngularStiffness(float& stiffness, float& damping,
15625 float frequencyHertz, float dampingRatio,
15626 const b2Body* bodyA, const b2Body* bodyB) {
15627 float IA = bodyA->GetInertia();
15628 float IB = bodyB->GetInertia();
15629 float I;
15630 if (IA > 0.0f && IB > 0.0f) {
15631 I = IA * IB / (IA + IB);
15632 } else if (IA > 0.0f) {
15633 I = IA;
15634 } else {
15635 I = IB;
15636 }
15637
15638 float omega = 2.0f * b2_pi * frequencyHertz;
15639 stiffness = I * omega * omega;
15640 damping = 2.0f * I * dampingRatio * omega;
15641}
15642
15643b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) {
15644 b2Joint* joint = nullptr;
15645
15646 switch (def->type) {
15647 case e_distanceJoint:
15648 {
15649 void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
15650 joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
15651 }
15652 break;
15653
15654 case e_mouseJoint:
15655 {
15656 void* mem = allocator->Allocate(sizeof(b2MouseJoint));
15657 joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
15658 }
15659 break;
15660
15661 case e_prismaticJoint:
15662 {
15663 void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
15664 joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
15665 }
15666 break;
15667
15668 case e_revoluteJoint:
15669 {
15670 void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
15671 joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
15672 }
15673 break;
15674
15675 case e_pulleyJoint:
15676 {
15677 void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
15678 joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
15679 }
15680 break;
15681
15682 case e_gearJoint:
15683 {
15684 void* mem = allocator->Allocate(sizeof(b2GearJoint));
15685 joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
15686 }
15687 break;
15688
15689 case e_wheelJoint:
15690 {
15691 void* mem = allocator->Allocate(sizeof(b2WheelJoint));
15692 joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
15693 }
15694 break;
15695
15696 case e_weldJoint:
15697 {
15698 void* mem = allocator->Allocate(sizeof(b2WeldJoint));
15699 joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
15700 }
15701 break;
15702
15703 case e_frictionJoint:
15704 {
15705 void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
15706 joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
15707 }
15708 break;
15709
15710 case e_motorJoint:
15711 {
15712 void* mem = allocator->Allocate(sizeof(b2MotorJoint));
15713 joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
15714 }
15715 break;
15716
15717 default:
15718 b2Assert(false);
15719 break;
15720 }
15721
15722 return joint;
15723}
15724
15725void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) {
15726 joint->~b2Joint();
15727 switch (joint->m_type) {
15728 case e_distanceJoint:
15729 allocator->Free(joint, sizeof(b2DistanceJoint));
15730 break;
15731
15732 case e_mouseJoint:
15733 allocator->Free(joint, sizeof(b2MouseJoint));
15734 break;
15735
15736 case e_prismaticJoint:
15737 allocator->Free(joint, sizeof(b2PrismaticJoint));
15738 break;
15739
15740 case e_revoluteJoint:
15741 allocator->Free(joint, sizeof(b2RevoluteJoint));
15742 break;
15743
15744 case e_pulleyJoint:
15745 allocator->Free(joint, sizeof(b2PulleyJoint));
15746 break;
15747
15748 case e_gearJoint:
15749 allocator->Free(joint, sizeof(b2GearJoint));
15750 break;
15751
15752 case e_wheelJoint:
15753 allocator->Free(joint, sizeof(b2WheelJoint));
15754 break;
15755
15756 case e_weldJoint:
15757 allocator->Free(joint, sizeof(b2WeldJoint));
15758 break;
15759
15760 case e_frictionJoint:
15761 allocator->Free(joint, sizeof(b2FrictionJoint));
15762 break;
15763
15764 case e_motorJoint:
15765 allocator->Free(joint, sizeof(b2MotorJoint));
15766 break;
15767
15768 default:
15769 b2Assert(false);
15770 break;
15771 }
15772}
15773
15774b2Joint::b2Joint(const b2JointDef* def) {
15775 b2Assert(def->bodyA != def->bodyB);
15776
15777 m_type = def->type;
15778 m_prev = nullptr;
15779 m_next = nullptr;
15780 m_bodyA = def->bodyA;
15781 m_bodyB = def->bodyB;
15782 m_index = 0;
15783 m_collideConnected = def->collideConnected;
15784 m_islandFlag = false;
15785 m_userData = def->userData;
15786
15787 m_edgeA.joint = nullptr;
15788 m_edgeA.other = nullptr;
15789 m_edgeA.prev = nullptr;
15790 m_edgeA.next = nullptr;
15791
15792 m_edgeB.joint = nullptr;
15793 m_edgeB.other = nullptr;
15794 m_edgeB.prev = nullptr;
15795 m_edgeB.next = nullptr;
15796}
15797
15798bool b2Joint::IsEnabled() const {
15799 return m_bodyA->IsEnabled() && m_bodyB->IsEnabled();
15800}
15801
15802void b2Joint::Draw(b2Draw* draw) const {
15803 const b2Transform& xf1 = m_bodyA->GetTransform();
15804 const b2Transform& xf2 = m_bodyB->GetTransform();
15805 b2Vec2 x1 = xf1.p;
15806 b2Vec2 x2 = xf2.p;
15807 b2Vec2 p1 = GetAnchorA();
15808 b2Vec2 p2 = GetAnchorB();
15809
15810 b2Color color(0.5f, 0.8f, 0.8f);
15811
15812 switch (m_type) {
15813 case e_distanceJoint:
15814 draw->DrawSegment(p1, p2, color);
15815 break;
15816
15817 case e_pulleyJoint:
15818 {
15819 b2PulleyJoint* pulley = (b2PulleyJoint*)this;
15820 b2Vec2 s1 = pulley->GetGroundAnchorA();
15821 b2Vec2 s2 = pulley->GetGroundAnchorB();
15822 draw->DrawSegment(s1, p1, color);
15823 draw->DrawSegment(s2, p2, color);
15824 draw->DrawSegment(s1, s2, color);
15825 }
15826 break;
15827
15828 case e_mouseJoint:
15829 {
15830 b2Color c;
15831 c.Set(0.0f, 1.0f, 0.0f);
15832 draw->DrawPoint(p1, 4.0f, c);
15833 draw->DrawPoint(p2, 4.0f, c);
15834
15835 c.Set(0.8f, 0.8f, 0.8f);
15836 draw->DrawSegment(p1, p2, c);
15837
15838 }
15839 break;
15840
15841 default:
15842 draw->DrawSegment(x1, p1, color);
15843 draw->DrawSegment(p1, p2, color);
15844 draw->DrawSegment(x2, p2, color);
15845 }
15846}
15847// MIT License
15848
15849// Copyright (c) 2019 Erin Catto
15850
15851// Permission is hereby granted, free of charge, to any person obtaining a copy
15852// of this software and associated documentation files (the "Software"), to deal
15853// in the Software without restriction, including without limitation the rights
15854// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15855// copies of the Software, and to permit persons to whom the Software is
15856// furnished to do so, subject to the following conditions:
15857
15858// The above copyright notice and this permission notice shall be included in all
15859// copies or substantial portions of the Software.
15860
15861// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15862// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15863// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
15864// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
15865// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15866// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
15867// SOFTWARE.
15868
15869#include "box2d/b2_body.h"
15870#include "box2d/b2_motor_joint.h"
15871#include "box2d/b2_time_step.h"
15872
15873// Point-to-point constraint
15874// Cdot = v2 - v1
15875// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
15876// J = [-I -r1_skew I r2_skew ]
15877// Identity used:
15878// w k % (rx i + ry j) = w * (-ry i + rx j)
15879//
15880// r1 = offset - c1
15881// r2 = -c2
15882
15883// Angle constraint
15884// Cdot = w2 - w1
15885// J = [0 0 -1 0 0 1]
15886// K = invI1 + invI2
15887
15888void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) {
15889 bodyA = bA;
15890 bodyB = bB;
15891 b2Vec2 xB = bodyB->GetPosition();
15892 linearOffset = bodyA->GetLocalPoint(xB);
15893
15894 float angleA = bodyA->GetAngle();
15895 float angleB = bodyB->GetAngle();
15896 angularOffset = angleB - angleA;
15897}
15898
15899b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)
15900 : b2Joint(def) {
15901 m_linearOffset = def->linearOffset;
15902 m_angularOffset = def->angularOffset;
15903
15904 m_linearImpulse.SetZero();
15905 m_angularImpulse = 0.0f;
15906
15907 m_maxForce = def->maxForce;
15908 m_maxTorque = def->maxTorque;
15909 m_correctionFactor = def->correctionFactor;
15910}
15911
15912void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) {
15913 m_indexA = m_bodyA->m_islandIndex;
15914 m_indexB = m_bodyB->m_islandIndex;
15915 m_localCenterA = m_bodyA->m_sweep.localCenter;
15916 m_localCenterB = m_bodyB->m_sweep.localCenter;
15917 m_invMassA = m_bodyA->m_invMass;
15918 m_invMassB = m_bodyB->m_invMass;
15919 m_invIA = m_bodyA->m_invI;
15920 m_invIB = m_bodyB->m_invI;
15921
15922 b2Vec2 cA = data.positions[m_indexA].c;
15923 float aA = data.positions[m_indexA].a;
15924 b2Vec2 vA = data.velocities[m_indexA].v;
15925 float wA = data.velocities[m_indexA].w;
15926
15927 b2Vec2 cB = data.positions[m_indexB].c;
15928 float aB = data.positions[m_indexB].a;
15929 b2Vec2 vB = data.velocities[m_indexB].v;
15930 float wB = data.velocities[m_indexB].w;
15931
15932 b2Rot qA(aA), qB(aB);
15933
15934 // Compute the effective mass matrix.
15935 m_rA = b2Mul(qA, m_linearOffset - m_localCenterA);
15936 m_rB = b2Mul(qB, -m_localCenterB);
15937
15938 // J = [-I -r1_skew I r2_skew]
15939 // r_skew = [-ry; rx]
15940
15941 // Matlab
15942 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
15943 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
15944 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
15945
15946 float mA = m_invMassA, mB = m_invMassB;
15947 float iA = m_invIA, iB = m_invIB;
15948
15949 // Upper 2 by 2 of K for point to point
15950 b2Mat22 K;
15951 K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
15952 K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
15953 K.ey.x = K.ex.y;
15954 K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
15955
15956 m_linearMass = K.GetInverse();
15957
15958 m_angularMass = iA + iB;
15959 if (m_angularMass > 0.0f) {
15960 m_angularMass = 1.0f / m_angularMass;
15961 }
15962
15963 m_linearError = cB + m_rB - cA - m_rA;
15964 m_angularError = aB - aA - m_angularOffset;
15965
15966 if (data.step.warmStarting) {
15967 // Scale impulses to support a variable time step.
15968 m_linearImpulse *= data.step.dtRatio;
15969 m_angularImpulse *= data.step.dtRatio;
15970
15971 b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
15972 vA -= mA * P;
15973 wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
15974 vB += mB * P;
15975 wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
15976 } else {
15977 m_linearImpulse.SetZero();
15978 m_angularImpulse = 0.0f;
15979 }
15980
15981 data.velocities[m_indexA].v = vA;
15982 data.velocities[m_indexA].w = wA;
15983 data.velocities[m_indexB].v = vB;
15984 data.velocities[m_indexB].w = wB;
15985}
15986
15987void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) {
15988 b2Vec2 vA = data.velocities[m_indexA].v;
15989 float wA = data.velocities[m_indexA].w;
15990 b2Vec2 vB = data.velocities[m_indexB].v;
15991 float wB = data.velocities[m_indexB].w;
15992
15993 float mA = m_invMassA, mB = m_invMassB;
15994 float iA = m_invIA, iB = m_invIB;
15995
15996 float h = data.step.dt;
15997 float inv_h = data.step.inv_dt;
15998
15999 // Solve angular friction
16000 {
16001 float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
16002 float impulse = -m_angularMass * Cdot;
16003
16004 float oldImpulse = m_angularImpulse;
16005 float maxImpulse = h * m_maxTorque;
16006 m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
16007 impulse = m_angularImpulse - oldImpulse;
16008
16009 wA -= iA * impulse;
16010 wB += iB * impulse;
16011 }
16012
16013 // Solve linear friction
16014 {
16015 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
16016
16017 b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
16018 b2Vec2 oldImpulse = m_linearImpulse;
16019 m_linearImpulse += impulse;
16020
16021 float maxImpulse = h * m_maxForce;
16022
16023 if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) {
16024 m_linearImpulse.Normalize();
16025 m_linearImpulse *= maxImpulse;
16026 }
16027
16028 impulse = m_linearImpulse - oldImpulse;
16029
16030 vA -= mA * impulse;
16031 wA -= iA * b2Cross(m_rA, impulse);
16032
16033 vB += mB * impulse;
16034 wB += iB * b2Cross(m_rB, impulse);
16035 }
16036
16037 data.velocities[m_indexA].v = vA;
16038 data.velocities[m_indexA].w = wA;
16039 data.velocities[m_indexB].v = vB;
16040 data.velocities[m_indexB].w = wB;
16041}
16042
16043bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) {
16044 B2_NOT_USED(data);
16045
16046 return true;
16047}
16048
16049b2Vec2 b2MotorJoint::GetAnchorA() const {
16050 return m_bodyA->GetPosition();
16051}
16052
16053b2Vec2 b2MotorJoint::GetAnchorB() const {
16054 return m_bodyB->GetPosition();
16055}
16056
16057b2Vec2 b2MotorJoint::GetReactionForce(float inv_dt) const {
16058 return inv_dt * m_linearImpulse;
16059}
16060
16061float b2MotorJoint::GetReactionTorque(float inv_dt) const {
16062 return inv_dt * m_angularImpulse;
16063}
16064
16065void b2MotorJoint::SetMaxForce(float force) {
16066 b2Assert(b2IsValid(force) && force >= 0.0f);
16067 m_maxForce = force;
16068}
16069
16070float b2MotorJoint::GetMaxForce() const {
16071 return m_maxForce;
16072}
16073
16074void b2MotorJoint::SetMaxTorque(float torque) {
16075 b2Assert(b2IsValid(torque) && torque >= 0.0f);
16076 m_maxTorque = torque;
16077}
16078
16079float b2MotorJoint::GetMaxTorque() const {
16080 return m_maxTorque;
16081}
16082
16083void b2MotorJoint::SetCorrectionFactor(float factor) {
16084 b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
16085 m_correctionFactor = factor;
16086}
16087
16088float b2MotorJoint::GetCorrectionFactor() const {
16089 return m_correctionFactor;
16090}
16091
16092void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) {
16093 if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) {
16094 m_bodyA->SetAwake(true);
16095 m_bodyB->SetAwake(true);
16096 m_linearOffset = linearOffset;
16097 }
16098}
16099
16100const b2Vec2& b2MotorJoint::GetLinearOffset() const {
16101 return m_linearOffset;
16102}
16103
16104void b2MotorJoint::SetAngularOffset(float angularOffset) {
16105 if (angularOffset != m_angularOffset) {
16106 m_bodyA->SetAwake(true);
16107 m_bodyB->SetAwake(true);
16108 m_angularOffset = angularOffset;
16109 }
16110}
16111
16112float b2MotorJoint::GetAngularOffset() const {
16113 return m_angularOffset;
16114}
16115
16116void b2MotorJoint::Dump() {
16117 int32 indexA = m_bodyA->m_islandIndex;
16118 int32 indexB = m_bodyB->m_islandIndex;
16119
16120 b2Dump(" b2MotorJointDef jd;\n");
16121 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
16122 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
16123 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
16124 b2Dump(" jd.linearOffset.Set(%.9g, %.9g);\n", m_linearOffset.x, m_linearOffset.y);
16125 b2Dump(" jd.angularOffset = %.9g;\n", m_angularOffset);
16126 b2Dump(" jd.maxForce = %.9g;\n", m_maxForce);
16127 b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque);
16128 b2Dump(" jd.correctionFactor = %.9g;\n", m_correctionFactor);
16129 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
16130}
16131// MIT License
16132
16133// Copyright (c) 2019 Erin Catto
16134
16135// Permission is hereby granted, free of charge, to any person obtaining a copy
16136// of this software and associated documentation files (the "Software"), to deal
16137// in the Software without restriction, including without limitation the rights
16138// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16139// copies of the Software, and to permit persons to whom the Software is
16140// furnished to do so, subject to the following conditions:
16141
16142// The above copyright notice and this permission notice shall be included in all
16143// copies or substantial portions of the Software.
16144
16145// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16146// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16147// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16148// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16149// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16150// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
16151// SOFTWARE.
16152
16153#include "box2d/b2_body.h"
16154#include "box2d/b2_mouse_joint.h"
16155#include "box2d/b2_time_step.h"
16156
16157// p = attached point, m = mouse point
16158// C = p - m
16159// Cdot = v
16160// = v + cross(w, r)
16161// J = [I r_skew]
16162// Identity used:
16163// w k % (rx i + ry j) = w * (-ry i + rx j)
16164
16165b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
16166 : b2Joint(def) {
16167 m_targetA = def->target;
16168 m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
16169 m_maxForce = def->maxForce;
16170 m_stiffness = def->stiffness;
16171 m_damping = def->damping;
16172
16173 m_impulse.SetZero();
16174 m_beta = 0.0f;
16175 m_gamma = 0.0f;
16176}
16177
16178void b2MouseJoint::SetTarget(const b2Vec2& target) {
16179 if (target != m_targetA) {
16180 m_bodyB->SetAwake(true);
16181 m_targetA = target;
16182 }
16183}
16184
16185const b2Vec2& b2MouseJoint::GetTarget() const {
16186 return m_targetA;
16187}
16188
16189void b2MouseJoint::SetMaxForce(float force) {
16190 m_maxForce = force;
16191}
16192
16193float b2MouseJoint::GetMaxForce() const {
16194 return m_maxForce;
16195}
16196
16197void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) {
16198 m_indexB = m_bodyB->m_islandIndex;
16199 m_localCenterB = m_bodyB->m_sweep.localCenter;
16200 m_invMassB = m_bodyB->m_invMass;
16201 m_invIB = m_bodyB->m_invI;
16202
16203 b2Vec2 cB = data.positions[m_indexB].c;
16204 float aB = data.positions[m_indexB].a;
16205 b2Vec2 vB = data.velocities[m_indexB].v;
16206 float wB = data.velocities[m_indexB].w;
16207
16208 b2Rot qB(aB);
16209
16210 float mass = m_bodyB->GetMass();
16211
16212 float d = m_damping;
16213 float k = m_stiffness;
16214
16215 // magic formulas
16216 // gamma has units of inverse mass.
16217 // beta has units of inverse time.
16218 float h = data.step.dt;
16219 m_gamma = h * (d + h * k);
16220 if (m_gamma != 0.0f) {
16221 m_gamma = 1.0f / m_gamma;
16222 }
16223 m_beta = h * k * m_gamma;
16224
16225 // Compute the effective mass matrix.
16226 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
16227
16228 // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
16229 // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
16230 // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
16231 b2Mat22 K;
16232 K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
16233 K.ex.y = -m_invIB * m_rB.x * m_rB.y;
16234 K.ey.x = K.ex.y;
16235 K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
16236
16237 m_mass = K.GetInverse();
16238
16239 m_C = cB + m_rB - m_targetA;
16240 m_C *= m_beta;
16241
16242 // Cheat with some damping
16243 wB *= 0.98f;
16244
16245 if (data.step.warmStarting) {
16246 m_impulse *= data.step.dtRatio;
16247 vB += m_invMassB * m_impulse;
16248 wB += m_invIB * b2Cross(m_rB, m_impulse);
16249 } else {
16250 m_impulse.SetZero();
16251 }
16252
16253 data.velocities[m_indexB].v = vB;
16254 data.velocities[m_indexB].w = wB;
16255}
16256
16257void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) {
16258 b2Vec2 vB = data.velocities[m_indexB].v;
16259 float wB = data.velocities[m_indexB].w;
16260
16261 // Cdot = v + cross(w, r)
16262 b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
16263 b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
16264
16265 b2Vec2 oldImpulse = m_impulse;
16266 m_impulse += impulse;
16267 float maxImpulse = data.step.dt * m_maxForce;
16268 if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) {
16269 m_impulse *= maxImpulse / m_impulse.Length();
16270 }
16271 impulse = m_impulse - oldImpulse;
16272
16273 vB += m_invMassB * impulse;
16274 wB += m_invIB * b2Cross(m_rB, impulse);
16275
16276 data.velocities[m_indexB].v = vB;
16277 data.velocities[m_indexB].w = wB;
16278}
16279
16280bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) {
16281 B2_NOT_USED(data);
16282 return true;
16283}
16284
16285b2Vec2 b2MouseJoint::GetAnchorA() const {
16286 return m_targetA;
16287}
16288
16289b2Vec2 b2MouseJoint::GetAnchorB() const {
16290 return m_bodyB->GetWorldPoint(m_localAnchorB);
16291}
16292
16293b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const {
16294 return inv_dt * m_impulse;
16295}
16296
16297float b2MouseJoint::GetReactionTorque(float inv_dt) const {
16298 return inv_dt * 0.0f;
16299}
16300
16301void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) {
16302 m_targetA -= newOrigin;
16303}
16304// MIT License
16305
16306// Copyright (c) 2019 Erin Catto
16307
16308// Permission is hereby granted, free of charge, to any person obtaining a copy
16309// of this software and associated documentation files (the "Software"), to deal
16310// in the Software without restriction, including without limitation the rights
16311// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16312// copies of the Software, and to permit persons to whom the Software is
16313// furnished to do so, subject to the following conditions:
16314
16315// The above copyright notice and this permission notice shall be included in all
16316// copies or substantial portions of the Software.
16317
16318// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16319// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16320// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16321// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16322// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16323// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
16324// SOFTWARE.
16325
16326#include "b2_polygon_circle_contact.h"
16327
16328#include "box2d/b2_block_allocator.h"
16329#include "box2d/b2_fixture.h"
16330
16331#include <new>
16332
16333b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) {
16334 void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
16335 return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
16336}
16337
16338void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
16339 ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
16340 allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
16341}
16342
16343b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
16344 : b2Contact(fixtureA, 0, fixtureB, 0) {
16345 b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
16346 b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
16347}
16348
16349void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
16350 b2CollidePolygonAndCircle(manifold,
16351 (b2PolygonShape*)m_fixtureA->GetShape(), xfA,
16352 (b2CircleShape*)m_fixtureB->GetShape(), xfB);
16353}
16354// MIT License
16355
16356// Copyright (c) 2019 Erin Catto
16357
16358// Permission is hereby granted, free of charge, to any person obtaining a copy
16359// of this software and associated documentation files (the "Software"), to deal
16360// in the Software without restriction, including without limitation the rights
16361// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16362// copies of the Software, and to permit persons to whom the Software is
16363// furnished to do so, subject to the following conditions:
16364
16365// The above copyright notice and this permission notice shall be included in all
16366// copies or substantial portions of the Software.
16367
16368// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16369// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16370// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16371// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16372// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16373// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
16374// SOFTWARE.
16375
16376#include "b2_polygon_contact.h"
16377
16378#include "box2d/b2_block_allocator.h"
16379#include "box2d/b2_body.h"
16380#include "box2d/b2_fixture.h"
16381#include "box2d/b2_time_of_impact.h"
16382#include "box2d/b2_world_callbacks.h"
16383
16384#include <new>
16385
16386b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) {
16387 void* mem = allocator->Allocate(sizeof(b2PolygonContact));
16388 return new (mem) b2PolygonContact(fixtureA, fixtureB);
16389}
16390
16391void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) {
16392 ((b2PolygonContact*)contact)->~b2PolygonContact();
16393 allocator->Free(contact, sizeof(b2PolygonContact));
16394}
16395
16396b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
16397 : b2Contact(fixtureA, 0, fixtureB, 0) {
16398 b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
16399 b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
16400}
16401
16402void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) {
16403 b2CollidePolygons(manifold,
16404 (b2PolygonShape*)m_fixtureA->GetShape(), xfA,
16405 (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
16406}
16407// MIT License
16408
16409// Copyright (c) 2019 Erin Catto
16410
16411// Permission is hereby granted, free of charge, to any person obtaining a copy
16412// of this software and associated documentation files (the "Software"), to deal
16413// in the Software without restriction, including without limitation the rights
16414// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16415// copies of the Software, and to permit persons to whom the Software is
16416// furnished to do so, subject to the following conditions:
16417
16418// The above copyright notice and this permission notice shall be included in all
16419// copies or substantial portions of the Software.
16420
16421// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16422// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16423// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16424// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16425// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16426// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
16427// SOFTWARE.
16428
16429#include "box2d/b2_body.h"
16430#include "box2d/b2_draw.h"
16431#include "box2d/b2_prismatic_joint.h"
16432#include "box2d/b2_time_step.h"
16433
16434// Linear constraint (point-to-line)
16435// d = p2 - p1 = x2 + r2 - x1 - r1
16436// C = dot(perp, d)
16437// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
16438// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
16439// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
16440//
16441// Angular constraint
16442// C = a2 - a1 + a_initial
16443// Cdot = w2 - w1
16444// J = [0 0 -1 0 0 1]
16445//
16446// K = J * invM * JT
16447//
16448// J = [-a -s1 a s2]
16449// [0 -1 0 1]
16450// a = perp
16451// s1 = cross(d + r1, a) = cross(p2 - x1, a)
16452// s2 = cross(r2, a) = cross(p2 - x2, a)
16453
16454// Motor/Limit linear constraint
16455// C = dot(ax1, d)
16456// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
16457// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
16458
16459// Predictive limit is applied even when the limit is not active.
16460// Prevents a constraint speed that can lead to a constraint error in one time step.
16461// Want C2 = C1 + h * Cdot >= 0
16462// Or:
16463// Cdot + C1/h >= 0
16464// I do not apply a negative constraint error because that is handled in position correction.
16465// So:
16466// Cdot + max(C1, 0)/h >= 0
16467
16468// Block Solver
16469// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.
16470//
16471// The Jacobian has 2 rows:
16472// J = [-uT -s1 uT s2] // linear
16473// [0 -1 0 1] // angular
16474//
16475// u = perp
16476// s1 = cross(d + r1, u), s2 = cross(r2, u)
16477// a1 = cross(d + r1, v), a2 = cross(r2, v)
16478
16479void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) {
16480 bodyA = bA;
16481 bodyB = bB;
16482 localAnchorA = bodyA->GetLocalPoint(anchor);
16483 localAnchorB = bodyB->GetLocalPoint(anchor);
16484 localAxisA = bodyA->GetLocalVector(axis);
16485 referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
16486}
16487
16488b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
16489 : b2Joint(def) {
16490 m_localAnchorA = def->localAnchorA;
16491 m_localAnchorB = def->localAnchorB;
16492 m_localXAxisA = def->localAxisA;
16493 m_localXAxisA.Normalize();
16494 m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
16495 m_referenceAngle = def->referenceAngle;
16496
16497 m_impulse.SetZero();
16498 m_axialMass = 0.0f;
16499 m_motorImpulse = 0.0f;
16500 m_lowerImpulse = 0.0f;
16501 m_upperImpulse = 0.0f;
16502
16503 m_lowerTranslation = def->lowerTranslation;
16504 m_upperTranslation = def->upperTranslation;
16505
16506 b2Assert(m_lowerTranslation <= m_upperTranslation);
16507
16508 m_maxMotorForce = def->maxMotorForce;
16509 m_motorSpeed = def->motorSpeed;
16510 m_enableLimit = def->enableLimit;
16511 m_enableMotor = def->enableMotor;
16512
16513 m_translation = 0.0f;
16514 m_axis.SetZero();
16515 m_perp.SetZero();
16516}
16517
16518void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) {
16519 m_indexA = m_bodyA->m_islandIndex;
16520 m_indexB = m_bodyB->m_islandIndex;
16521 m_localCenterA = m_bodyA->m_sweep.localCenter;
16522 m_localCenterB = m_bodyB->m_sweep.localCenter;
16523 m_invMassA = m_bodyA->m_invMass;
16524 m_invMassB = m_bodyB->m_invMass;
16525 m_invIA = m_bodyA->m_invI;
16526 m_invIB = m_bodyB->m_invI;
16527
16528 b2Vec2 cA = data.positions[m_indexA].c;
16529 float aA = data.positions[m_indexA].a;
16530 b2Vec2 vA = data.velocities[m_indexA].v;
16531 float wA = data.velocities[m_indexA].w;
16532
16533 b2Vec2 cB = data.positions[m_indexB].c;
16534 float aB = data.positions[m_indexB].a;
16535 b2Vec2 vB = data.velocities[m_indexB].v;
16536 float wB = data.velocities[m_indexB].w;
16537
16538 b2Rot qA(aA), qB(aB);
16539
16540 // Compute the effective masses.
16541 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
16542 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
16543 b2Vec2 d = (cB - cA) + rB - rA;
16544
16545 float mA = m_invMassA, mB = m_invMassB;
16546 float iA = m_invIA, iB = m_invIB;
16547
16548 // Compute motor Jacobian and effective mass.
16549 {
16550 m_axis = b2Mul(qA, m_localXAxisA);
16551 m_a1 = b2Cross(d + rA, m_axis);
16552 m_a2 = b2Cross(rB, m_axis);
16553
16554 m_axialMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
16555 if (m_axialMass > 0.0f) {
16556 m_axialMass = 1.0f / m_axialMass;
16557 }
16558 }
16559
16560 // Prismatic constraint.
16561 {
16562 m_perp = b2Mul(qA, m_localYAxisA);
16563
16564 m_s1 = b2Cross(d + rA, m_perp);
16565 m_s2 = b2Cross(rB, m_perp);
16566
16567 float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
16568 float k12 = iA * m_s1 + iB * m_s2;
16569 float k22 = iA + iB;
16570 if (k22 == 0.0f) {
16571 // For bodies with fixed rotation.
16572 k22 = 1.0f;
16573 }
16574
16575 m_K.ex.Set(k11, k12);
16576 m_K.ey.Set(k12, k22);
16577 }
16578
16579 if (m_enableLimit) {
16580 m_translation = b2Dot(m_axis, d);
16581 } else {
16582 m_lowerImpulse = 0.0f;
16583 m_upperImpulse = 0.0f;
16584 }
16585
16586 if (m_enableMotor == false) {
16587 m_motorImpulse = 0.0f;
16588 }
16589
16590 if (data.step.warmStarting) {
16591 // Account for variable time step.
16592 m_impulse *= data.step.dtRatio;
16593 m_motorImpulse *= data.step.dtRatio;
16594 m_lowerImpulse *= data.step.dtRatio;
16595 m_upperImpulse *= data.step.dtRatio;
16596
16597 float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse;
16598 b2Vec2 P = m_impulse.x * m_perp + axialImpulse * m_axis;
16599 float LA = m_impulse.x * m_s1 + m_impulse.y + axialImpulse * m_a1;
16600 float LB = m_impulse.x * m_s2 + m_impulse.y + axialImpulse * m_a2;
16601
16602 vA -= mA * P;
16603 wA -= iA * LA;
16604
16605 vB += mB * P;
16606 wB += iB * LB;
16607 } else {
16608 m_impulse.SetZero();
16609 m_motorImpulse = 0.0f;
16610 m_lowerImpulse = 0.0f;
16611 m_upperImpulse = 0.0f;
16612 }
16613
16614 data.velocities[m_indexA].v = vA;
16615 data.velocities[m_indexA].w = wA;
16616 data.velocities[m_indexB].v = vB;
16617 data.velocities[m_indexB].w = wB;
16618}
16619
16620void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) {
16621 b2Vec2 vA = data.velocities[m_indexA].v;
16622 float wA = data.velocities[m_indexA].w;
16623 b2Vec2 vB = data.velocities[m_indexB].v;
16624 float wB = data.velocities[m_indexB].w;
16625
16626 float mA = m_invMassA, mB = m_invMassB;
16627 float iA = m_invIA, iB = m_invIB;
16628
16629 // Solve linear motor constraint
16630 if (m_enableMotor) {
16631 float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
16632 float impulse = m_axialMass * (m_motorSpeed - Cdot);
16633 float oldImpulse = m_motorImpulse;
16634 float maxImpulse = data.step.dt * m_maxMotorForce;
16635 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
16636 impulse = m_motorImpulse - oldImpulse;
16637
16638 b2Vec2 P = impulse * m_axis;
16639 float LA = impulse * m_a1;
16640 float LB = impulse * m_a2;
16641
16642 vA -= mA * P;
16643 wA -= iA * LA;
16644 vB += mB * P;
16645 wB += iB * LB;
16646 }
16647
16648 if (m_enableLimit) {
16649 // Lower limit
16650 {
16651 float C = m_translation - m_lowerTranslation;
16652 float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
16653 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
16654 float oldImpulse = m_lowerImpulse;
16655 m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
16656 impulse = m_lowerImpulse - oldImpulse;
16657
16658 b2Vec2 P = impulse * m_axis;
16659 float LA = impulse * m_a1;
16660 float LB = impulse * m_a2;
16661
16662 vA -= mA * P;
16663 wA -= iA * LA;
16664 vB += mB * P;
16665 wB += iB * LB;
16666 }
16667
16668 // Upper limit
16669 // Note: signs are flipped to keep C positive when the constraint is satisfied.
16670 // This also keeps the impulse positive when the limit is active.
16671 {
16672 float C = m_upperTranslation - m_translation;
16673 float Cdot = b2Dot(m_axis, vA - vB) + m_a1 * wA - m_a2 * wB;
16674 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
16675 float oldImpulse = m_upperImpulse;
16676 m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
16677 impulse = m_upperImpulse - oldImpulse;
16678
16679 b2Vec2 P = impulse * m_axis;
16680 float LA = impulse * m_a1;
16681 float LB = impulse * m_a2;
16682
16683 vA += mA * P;
16684 wA += iA * LA;
16685 vB -= mB * P;
16686 wB -= iB * LB;
16687 }
16688 }
16689
16690 // Solve the prismatic constraint in block form.
16691 {
16692 b2Vec2 Cdot;
16693 Cdot.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
16694 Cdot.y = wB - wA;
16695
16696 b2Vec2 df = m_K.Solve(-Cdot);
16697 m_impulse += df;
16698
16699 b2Vec2 P = df.x * m_perp;
16700 float LA = df.x * m_s1 + df.y;
16701 float LB = df.x * m_s2 + df.y;
16702
16703 vA -= mA * P;
16704 wA -= iA * LA;
16705
16706 vB += mB * P;
16707 wB += iB * LB;
16708 }
16709
16710 data.velocities[m_indexA].v = vA;
16711 data.velocities[m_indexA].w = wA;
16712 data.velocities[m_indexB].v = vB;
16713 data.velocities[m_indexB].w = wB;
16714}
16715
16716// A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context,
16717// the position solver is not there to resolve forces.It is only there to cope with integration error.
16718//
16719// Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck.
16720//
16721// We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity
16722// solver indicates the limit is inactive.
16723bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) {
16724 b2Vec2 cA = data.positions[m_indexA].c;
16725 float aA = data.positions[m_indexA].a;
16726 b2Vec2 cB = data.positions[m_indexB].c;
16727 float aB = data.positions[m_indexB].a;
16728
16729 b2Rot qA(aA), qB(aB);
16730
16731 float mA = m_invMassA, mB = m_invMassB;
16732 float iA = m_invIA, iB = m_invIB;
16733
16734 // Compute fresh Jacobians
16735 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
16736 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
16737 b2Vec2 d = cB + rB - cA - rA;
16738
16739 b2Vec2 axis = b2Mul(qA, m_localXAxisA);
16740 float a1 = b2Cross(d + rA, axis);
16741 float a2 = b2Cross(rB, axis);
16742 b2Vec2 perp = b2Mul(qA, m_localYAxisA);
16743
16744 float s1 = b2Cross(d + rA, perp);
16745 float s2 = b2Cross(rB, perp);
16746
16747 b2Vec3 impulse;
16748 b2Vec2 C1;
16749 C1.x = b2Dot(perp, d);
16750 C1.y = aB - aA - m_referenceAngle;
16751
16752 float linearError = b2Abs(C1.x);
16753 float angularError = b2Abs(C1.y);
16754
16755 bool active = false;
16756 float C2 = 0.0f;
16757 if (m_enableLimit) {
16758 float translation = b2Dot(axis, d);
16759 if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) {
16760 C2 = translation;
16761 linearError = b2Max(linearError, b2Abs(translation));
16762 active = true;
16763 } else if (translation <= m_lowerTranslation) {
16764 C2 = b2Min(translation - m_lowerTranslation, 0.0f);
16765 linearError = b2Max(linearError, m_lowerTranslation - translation);
16766 active = true;
16767 } else if (translation >= m_upperTranslation) {
16768 C2 = b2Max(translation - m_upperTranslation, 0.0f);
16769 linearError = b2Max(linearError, translation - m_upperTranslation);
16770 active = true;
16771 }
16772 }
16773
16774 if (active) {
16775 float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
16776 float k12 = iA * s1 + iB * s2;
16777 float k13 = iA * s1 * a1 + iB * s2 * a2;
16778 float k22 = iA + iB;
16779 if (k22 == 0.0f) {
16780 // For fixed rotation
16781 k22 = 1.0f;
16782 }
16783 float k23 = iA * a1 + iB * a2;
16784 float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
16785
16786 b2Mat33 K;
16787 K.ex.Set(k11, k12, k13);
16788 K.ey.Set(k12, k22, k23);
16789 K.ez.Set(k13, k23, k33);
16790
16791 b2Vec3 C;
16792 C.x = C1.x;
16793 C.y = C1.y;
16794 C.z = C2;
16795
16796 impulse = K.Solve33(-C);
16797 } else {
16798 float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
16799 float k12 = iA * s1 + iB * s2;
16800 float k22 = iA + iB;
16801 if (k22 == 0.0f) {
16802 k22 = 1.0f;
16803 }
16804
16805 b2Mat22 K;
16806 K.ex.Set(k11, k12);
16807 K.ey.Set(k12, k22);
16808
16809 b2Vec2 impulse1 = K.Solve(-C1);
16810 impulse.x = impulse1.x;
16811 impulse.y = impulse1.y;
16812 impulse.z = 0.0f;
16813 }
16814
16815 b2Vec2 P = impulse.x * perp + impulse.z * axis;
16816 float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
16817 float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
16818
16819 cA -= mA * P;
16820 aA -= iA * LA;
16821 cB += mB * P;
16822 aB += iB * LB;
16823
16824 data.positions[m_indexA].c = cA;
16825 data.positions[m_indexA].a = aA;
16826 data.positions[m_indexB].c = cB;
16827 data.positions[m_indexB].a = aB;
16828
16829 return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
16830}
16831
16832b2Vec2 b2PrismaticJoint::GetAnchorA() const {
16833 return m_bodyA->GetWorldPoint(m_localAnchorA);
16834}
16835
16836b2Vec2 b2PrismaticJoint::GetAnchorB() const {
16837 return m_bodyB->GetWorldPoint(m_localAnchorB);
16838}
16839
16840b2Vec2 b2PrismaticJoint::GetReactionForce(float inv_dt) const {
16841 return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_lowerImpulse - m_upperImpulse) * m_axis);
16842}
16843
16844float b2PrismaticJoint::GetReactionTorque(float inv_dt) const {
16845 return inv_dt * m_impulse.y;
16846}
16847
16848float b2PrismaticJoint::GetJointTranslation() const {
16849 b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
16850 b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
16851 b2Vec2 d = pB - pA;
16852 b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);
16853
16854 float translation = b2Dot(d, axis);
16855 return translation;
16856}
16857
16858float b2PrismaticJoint::GetJointSpeed() const {
16859 b2Body* bA = m_bodyA;
16860 b2Body* bB = m_bodyB;
16861
16862 b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
16863 b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
16864 b2Vec2 p1 = bA->m_sweep.c + rA;
16865 b2Vec2 p2 = bB->m_sweep.c + rB;
16866 b2Vec2 d = p2 - p1;
16867 b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
16868
16869 b2Vec2 vA = bA->m_linearVelocity;
16870 b2Vec2 vB = bB->m_linearVelocity;
16871 float wA = bA->m_angularVelocity;
16872 float wB = bB->m_angularVelocity;
16873
16874 float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
16875 return speed;
16876}
16877
16878bool b2PrismaticJoint::IsLimitEnabled() const {
16879 return m_enableLimit;
16880}
16881
16882void b2PrismaticJoint::EnableLimit(bool flag) {
16883 if (flag != m_enableLimit) {
16884 m_bodyA->SetAwake(true);
16885 m_bodyB->SetAwake(true);
16886 m_enableLimit = flag;
16887 m_lowerImpulse = 0.0f;
16888 m_upperImpulse = 0.0f;
16889 }
16890}
16891
16892float b2PrismaticJoint::GetLowerLimit() const {
16893 return m_lowerTranslation;
16894}
16895
16896float b2PrismaticJoint::GetUpperLimit() const {
16897 return m_upperTranslation;
16898}
16899
16900void b2PrismaticJoint::SetLimits(float lower, float upper) {
16901 b2Assert(lower <= upper);
16902 if (lower != m_lowerTranslation || upper != m_upperTranslation) {
16903 m_bodyA->SetAwake(true);
16904 m_bodyB->SetAwake(true);
16905 m_lowerTranslation = lower;
16906 m_upperTranslation = upper;
16907 m_lowerImpulse = 0.0f;
16908 m_upperImpulse = 0.0f;
16909 }
16910}
16911
16912bool b2PrismaticJoint::IsMotorEnabled() const {
16913 return m_enableMotor;
16914}
16915
16916void b2PrismaticJoint::EnableMotor(bool flag) {
16917 if (flag != m_enableMotor) {
16918 m_bodyA->SetAwake(true);
16919 m_bodyB->SetAwake(true);
16920 m_enableMotor = flag;
16921 }
16922}
16923
16924void b2PrismaticJoint::SetMotorSpeed(float speed) {
16925 if (speed != m_motorSpeed) {
16926 m_bodyA->SetAwake(true);
16927 m_bodyB->SetAwake(true);
16928 m_motorSpeed = speed;
16929 }
16930}
16931
16932void b2PrismaticJoint::SetMaxMotorForce(float force) {
16933 if (force != m_maxMotorForce) {
16934 m_bodyA->SetAwake(true);
16935 m_bodyB->SetAwake(true);
16936 m_maxMotorForce = force;
16937 }
16938}
16939
16940float b2PrismaticJoint::GetMotorForce(float inv_dt) const {
16941 return inv_dt * m_motorImpulse;
16942}
16943
16944void b2PrismaticJoint::Dump() {
16945 // FLT_DECIMAL_DIG == 9
16946
16947 int32 indexA = m_bodyA->m_islandIndex;
16948 int32 indexB = m_bodyB->m_islandIndex;
16949
16950 b2Dump(" b2PrismaticJointDef jd;\n");
16951 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
16952 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
16953 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
16954 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
16955 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
16956 b2Dump(" jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y);
16957 b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle);
16958 b2Dump(" jd.enableLimit = bool(%d);\n", m_enableLimit);
16959 b2Dump(" jd.lowerTranslation = %.9g;\n", m_lowerTranslation);
16960 b2Dump(" jd.upperTranslation = %.9g;\n", m_upperTranslation);
16961 b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor);
16962 b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed);
16963 b2Dump(" jd.maxMotorForce = %.9g;\n", m_maxMotorForce);
16964 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
16965}
16966
16967void b2PrismaticJoint::Draw(b2Draw* draw) const {
16968 const b2Transform& xfA = m_bodyA->GetTransform();
16969 const b2Transform& xfB = m_bodyB->GetTransform();
16970 b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
16971 b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
16972
16973 b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA);
16974
16975 b2Color c1(0.7f, 0.7f, 0.7f);
16976 b2Color c2(0.3f, 0.9f, 0.3f);
16977 b2Color c3(0.9f, 0.3f, 0.3f);
16978 b2Color c4(0.3f, 0.3f, 0.9f);
16979 b2Color c5(0.4f, 0.4f, 0.4f);
16980
16981 draw->DrawSegment(pA, pB, c5);
16982
16983 if (m_enableLimit) {
16984 b2Vec2 lower = pA + m_lowerTranslation * axis;
16985 b2Vec2 upper = pA + m_upperTranslation * axis;
16986 b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA);
16987 draw->DrawSegment(lower, upper, c1);
16988 draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2);
16989 draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3);
16990 } else {
16991 draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1);
16992 }
16993
16994 draw->DrawPoint(pA, 5.0f, c1);
16995 draw->DrawPoint(pB, 5.0f, c4);
16996}
16997// MIT License
16998
16999// Copyright (c) 2019 Erin Catto
17000
17001// Permission is hereby granted, free of charge, to any person obtaining a copy
17002// of this software and associated documentation files (the "Software"), to deal
17003// in the Software without restriction, including without limitation the rights
17004// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17005// copies of the Software, and to permit persons to whom the Software is
17006// furnished to do so, subject to the following conditions:
17007
17008// The above copyright notice and this permission notice shall be included in all
17009// copies or substantial portions of the Software.
17010
17011// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17012// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17013// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17014// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17015// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
17016// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
17017// SOFTWARE.
17018
17019#include "box2d/b2_body.h"
17020#include "box2d/b2_pulley_joint.h"
17021#include "box2d/b2_time_step.h"
17022
17023// Pulley:
17024// length1 = norm(p1 - s1)
17025// length2 = norm(p2 - s2)
17026// C0 = (length1 + ratio * length2)_initial
17027// C = C0 - (length1 + ratio * length2)
17028// u1 = (p1 - s1) / norm(p1 - s1)
17029// u2 = (p2 - s2) / norm(p2 - s2)
17030// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
17031// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
17032// K = J * invM * JT
17033// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
17034
17035void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,
17036 const b2Vec2& groundA, const b2Vec2& groundB,
17037 const b2Vec2& anchorA, const b2Vec2& anchorB,
17038 float r) {
17039 bodyA = bA;
17040 bodyB = bB;
17041 groundAnchorA = groundA;
17042 groundAnchorB = groundB;
17043 localAnchorA = bodyA->GetLocalPoint(anchorA);
17044 localAnchorB = bodyB->GetLocalPoint(anchorB);
17045 b2Vec2 dA = anchorA - groundA;
17046 lengthA = dA.Length();
17047 b2Vec2 dB = anchorB - groundB;
17048 lengthB = dB.Length();
17049 ratio = r;
17050 b2Assert(ratio > b2_epsilon);
17051}
17052
17053b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
17054 : b2Joint(def) {
17055 m_groundAnchorA = def->groundAnchorA;
17056 m_groundAnchorB = def->groundAnchorB;
17057 m_localAnchorA = def->localAnchorA;
17058 m_localAnchorB = def->localAnchorB;
17059
17060 m_lengthA = def->lengthA;
17061 m_lengthB = def->lengthB;
17062
17063 b2Assert(def->ratio != 0.0f);
17064 m_ratio = def->ratio;
17065
17066 m_constant = def->lengthA + m_ratio * def->lengthB;
17067
17068 m_impulse = 0.0f;
17069}
17070
17071void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) {
17072 m_indexA = m_bodyA->m_islandIndex;
17073 m_indexB = m_bodyB->m_islandIndex;
17074 m_localCenterA = m_bodyA->m_sweep.localCenter;
17075 m_localCenterB = m_bodyB->m_sweep.localCenter;
17076 m_invMassA = m_bodyA->m_invMass;
17077 m_invMassB = m_bodyB->m_invMass;
17078 m_invIA = m_bodyA->m_invI;
17079 m_invIB = m_bodyB->m_invI;
17080
17081 b2Vec2 cA = data.positions[m_indexA].c;
17082 float aA = data.positions[m_indexA].a;
17083 b2Vec2 vA = data.velocities[m_indexA].v;
17084 float wA = data.velocities[m_indexA].w;
17085
17086 b2Vec2 cB = data.positions[m_indexB].c;
17087 float aB = data.positions[m_indexB].a;
17088 b2Vec2 vB = data.velocities[m_indexB].v;
17089 float wB = data.velocities[m_indexB].w;
17090
17091 b2Rot qA(aA), qB(aB);
17092
17093 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17094 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17095
17096 // Get the pulley axes.
17097 m_uA = cA + m_rA - m_groundAnchorA;
17098 m_uB = cB + m_rB - m_groundAnchorB;
17099
17100 float lengthA = m_uA.Length();
17101 float lengthB = m_uB.Length();
17102
17103 if (lengthA > 10.0f * b2_linearSlop) {
17104 m_uA *= 1.0f / lengthA;
17105 } else {
17106 m_uA.SetZero();
17107 }
17108
17109 if (lengthB > 10.0f * b2_linearSlop) {
17110 m_uB *= 1.0f / lengthB;
17111 } else {
17112 m_uB.SetZero();
17113 }
17114
17115 // Compute effective mass.
17116 float ruA = b2Cross(m_rA, m_uA);
17117 float ruB = b2Cross(m_rB, m_uB);
17118
17119 float mA = m_invMassA + m_invIA * ruA * ruA;
17120 float mB = m_invMassB + m_invIB * ruB * ruB;
17121
17122 m_mass = mA + m_ratio * m_ratio * mB;
17123
17124 if (m_mass > 0.0f) {
17125 m_mass = 1.0f / m_mass;
17126 }
17127
17128 if (data.step.warmStarting) {
17129 // Scale impulses to support variable time steps.
17130 m_impulse *= data.step.dtRatio;
17131
17132 // Warm starting.
17133 b2Vec2 PA = -(m_impulse)*m_uA;
17134 b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
17135
17136 vA += m_invMassA * PA;
17137 wA += m_invIA * b2Cross(m_rA, PA);
17138 vB += m_invMassB * PB;
17139 wB += m_invIB * b2Cross(m_rB, PB);
17140 } else {
17141 m_impulse = 0.0f;
17142 }
17143
17144 data.velocities[m_indexA].v = vA;
17145 data.velocities[m_indexA].w = wA;
17146 data.velocities[m_indexB].v = vB;
17147 data.velocities[m_indexB].w = wB;
17148}
17149
17150void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) {
17151 b2Vec2 vA = data.velocities[m_indexA].v;
17152 float wA = data.velocities[m_indexA].w;
17153 b2Vec2 vB = data.velocities[m_indexB].v;
17154 float wB = data.velocities[m_indexB].w;
17155
17156 b2Vec2 vpA = vA + b2Cross(wA, m_rA);
17157 b2Vec2 vpB = vB + b2Cross(wB, m_rB);
17158
17159 float Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
17160 float impulse = -m_mass * Cdot;
17161 m_impulse += impulse;
17162
17163 b2Vec2 PA = -impulse * m_uA;
17164 b2Vec2 PB = -m_ratio * impulse * m_uB;
17165 vA += m_invMassA * PA;
17166 wA += m_invIA * b2Cross(m_rA, PA);
17167 vB += m_invMassB * PB;
17168 wB += m_invIB * b2Cross(m_rB, PB);
17169
17170 data.velocities[m_indexA].v = vA;
17171 data.velocities[m_indexA].w = wA;
17172 data.velocities[m_indexB].v = vB;
17173 data.velocities[m_indexB].w = wB;
17174}
17175
17176bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) {
17177 b2Vec2 cA = data.positions[m_indexA].c;
17178 float aA = data.positions[m_indexA].a;
17179 b2Vec2 cB = data.positions[m_indexB].c;
17180 float aB = data.positions[m_indexB].a;
17181
17182 b2Rot qA(aA), qB(aB);
17183
17184 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17185 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17186
17187 // Get the pulley axes.
17188 b2Vec2 uA = cA + rA - m_groundAnchorA;
17189 b2Vec2 uB = cB + rB - m_groundAnchorB;
17190
17191 float lengthA = uA.Length();
17192 float lengthB = uB.Length();
17193
17194 if (lengthA > 10.0f * b2_linearSlop) {
17195 uA *= 1.0f / lengthA;
17196 } else {
17197 uA.SetZero();
17198 }
17199
17200 if (lengthB > 10.0f * b2_linearSlop) {
17201 uB *= 1.0f / lengthB;
17202 } else {
17203 uB.SetZero();
17204 }
17205
17206 // Compute effective mass.
17207 float ruA = b2Cross(rA, uA);
17208 float ruB = b2Cross(rB, uB);
17209
17210 float mA = m_invMassA + m_invIA * ruA * ruA;
17211 float mB = m_invMassB + m_invIB * ruB * ruB;
17212
17213 float mass = mA + m_ratio * m_ratio * mB;
17214
17215 if (mass > 0.0f) {
17216 mass = 1.0f / mass;
17217 }
17218
17219 float C = m_constant - lengthA - m_ratio * lengthB;
17220 float linearError = b2Abs(C);
17221
17222 float impulse = -mass * C;
17223
17224 b2Vec2 PA = -impulse * uA;
17225 b2Vec2 PB = -m_ratio * impulse * uB;
17226
17227 cA += m_invMassA * PA;
17228 aA += m_invIA * b2Cross(rA, PA);
17229 cB += m_invMassB * PB;
17230 aB += m_invIB * b2Cross(rB, PB);
17231
17232 data.positions[m_indexA].c = cA;
17233 data.positions[m_indexA].a = aA;
17234 data.positions[m_indexB].c = cB;
17235 data.positions[m_indexB].a = aB;
17236
17237 return linearError < b2_linearSlop;
17238}
17239
17240b2Vec2 b2PulleyJoint::GetAnchorA() const {
17241 return m_bodyA->GetWorldPoint(m_localAnchorA);
17242}
17243
17244b2Vec2 b2PulleyJoint::GetAnchorB() const {
17245 return m_bodyB->GetWorldPoint(m_localAnchorB);
17246}
17247
17248b2Vec2 b2PulleyJoint::GetReactionForce(float inv_dt) const {
17249 b2Vec2 P = m_impulse * m_uB;
17250 return inv_dt * P;
17251}
17252
17253float b2PulleyJoint::GetReactionTorque(float inv_dt) const {
17254 B2_NOT_USED(inv_dt);
17255 return 0.0f;
17256}
17257
17258b2Vec2 b2PulleyJoint::GetGroundAnchorA() const {
17259 return m_groundAnchorA;
17260}
17261
17262b2Vec2 b2PulleyJoint::GetGroundAnchorB() const {
17263 return m_groundAnchorB;
17264}
17265
17266float b2PulleyJoint::GetLengthA() const {
17267 return m_lengthA;
17268}
17269
17270float b2PulleyJoint::GetLengthB() const {
17271 return m_lengthB;
17272}
17273
17274float b2PulleyJoint::GetRatio() const {
17275 return m_ratio;
17276}
17277
17278float b2PulleyJoint::GetCurrentLengthA() const {
17279 b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
17280 b2Vec2 s = m_groundAnchorA;
17281 b2Vec2 d = p - s;
17282 return d.Length();
17283}
17284
17285float b2PulleyJoint::GetCurrentLengthB() const {
17286 b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
17287 b2Vec2 s = m_groundAnchorB;
17288 b2Vec2 d = p - s;
17289 return d.Length();
17290}
17291
17292void b2PulleyJoint::Dump() {
17293 int32 indexA = m_bodyA->m_islandIndex;
17294 int32 indexB = m_bodyB->m_islandIndex;
17295
17296 b2Dump(" b2PulleyJointDef jd;\n");
17297 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
17298 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
17299 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
17300 b2Dump(" jd.groundAnchorA.Set(%.9g, %.9g);\n", m_groundAnchorA.x, m_groundAnchorA.y);
17301 b2Dump(" jd.groundAnchorB.Set(%.9g, %.9g);\n", m_groundAnchorB.x, m_groundAnchorB.y);
17302 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
17303 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
17304 b2Dump(" jd.lengthA = %.9g;\n", m_lengthA);
17305 b2Dump(" jd.lengthB = %.9g;\n", m_lengthB);
17306 b2Dump(" jd.ratio = %.9g;\n", m_ratio);
17307 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
17308}
17309
17310void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) {
17311 m_groundAnchorA -= newOrigin;
17312 m_groundAnchorB -= newOrigin;
17313}
17314// MIT License
17315
17316// Copyright (c) 2019 Erin Catto
17317
17318// Permission is hereby granted, free of charge, to any person obtaining a copy
17319// of this software and associated documentation files (the "Software"), to deal
17320// in the Software without restriction, including without limitation the rights
17321// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17322// copies of the Software, and to permit persons to whom the Software is
17323// furnished to do so, subject to the following conditions:
17324
17325// The above copyright notice and this permission notice shall be included in all
17326// copies or substantial portions of the Software.
17327
17328// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17329// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17330// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17331// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17332// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
17333// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
17334// SOFTWARE.
17335
17336#include "box2d/b2_body.h"
17337#include "box2d/b2_draw.h"
17338#include "box2d/b2_revolute_joint.h"
17339#include "box2d/b2_time_step.h"
17340
17341// Point-to-point constraint
17342// C = p2 - p1
17343// Cdot = v2 - v1
17344// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
17345// J = [-I -r1_skew I r2_skew ]
17346// Identity used:
17347// w k % (rx i + ry j) = w * (-ry i + rx j)
17348
17349// Motor constraint
17350// Cdot = w2 - w1
17351// J = [0 0 -1 0 0 1]
17352// K = invI1 + invI2
17353
17354void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) {
17355 bodyA = bA;
17356 bodyB = bB;
17357 localAnchorA = bodyA->GetLocalPoint(anchor);
17358 localAnchorB = bodyB->GetLocalPoint(anchor);
17359 referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
17360}
17361
17362b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
17363 : b2Joint(def) {
17364 m_localAnchorA = def->localAnchorA;
17365 m_localAnchorB = def->localAnchorB;
17366 m_referenceAngle = def->referenceAngle;
17367
17368 m_impulse.SetZero();
17369 m_axialMass = 0.0f;
17370 m_motorImpulse = 0.0f;
17371 m_lowerImpulse = 0.0f;
17372 m_upperImpulse = 0.0f;
17373
17374 m_lowerAngle = def->lowerAngle;
17375 m_upperAngle = def->upperAngle;
17376 m_maxMotorTorque = def->maxMotorTorque;
17377 m_motorSpeed = def->motorSpeed;
17378 m_enableLimit = def->enableLimit;
17379 m_enableMotor = def->enableMotor;
17380
17381 m_angle = 0.0f;
17382}
17383
17384void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) {
17385 m_indexA = m_bodyA->m_islandIndex;
17386 m_indexB = m_bodyB->m_islandIndex;
17387 m_localCenterA = m_bodyA->m_sweep.localCenter;
17388 m_localCenterB = m_bodyB->m_sweep.localCenter;
17389 m_invMassA = m_bodyA->m_invMass;
17390 m_invMassB = m_bodyB->m_invMass;
17391 m_invIA = m_bodyA->m_invI;
17392 m_invIB = m_bodyB->m_invI;
17393
17394 float aA = data.positions[m_indexA].a;
17395 b2Vec2 vA = data.velocities[m_indexA].v;
17396 float wA = data.velocities[m_indexA].w;
17397
17398 float aB = data.positions[m_indexB].a;
17399 b2Vec2 vB = data.velocities[m_indexB].v;
17400 float wB = data.velocities[m_indexB].w;
17401
17402 b2Rot qA(aA), qB(aB);
17403
17404 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17405 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17406
17407 // J = [-I -r1_skew I r2_skew]
17408 // r_skew = [-ry; rx]
17409
17410 // Matlab
17411 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x]
17412 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB]
17413
17414 float mA = m_invMassA, mB = m_invMassB;
17415 float iA = m_invIA, iB = m_invIB;
17416
17417 m_K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
17418 m_K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
17419 m_K.ex.y = m_K.ey.x;
17420 m_K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
17421
17422 m_axialMass = iA + iB;
17423 bool fixedRotation;
17424 if (m_axialMass > 0.0f) {
17425 m_axialMass = 1.0f / m_axialMass;
17426 fixedRotation = false;
17427 } else {
17428 fixedRotation = true;
17429 }
17430
17431 m_angle = aB - aA - m_referenceAngle;
17432 if (m_enableLimit == false || fixedRotation) {
17433 m_lowerImpulse = 0.0f;
17434 m_upperImpulse = 0.0f;
17435 }
17436
17437 if (m_enableMotor == false || fixedRotation) {
17438 m_motorImpulse = 0.0f;
17439 }
17440
17441 if (data.step.warmStarting) {
17442 // Scale impulses to support a variable time step.
17443 m_impulse *= data.step.dtRatio;
17444 m_motorImpulse *= data.step.dtRatio;
17445 m_lowerImpulse *= data.step.dtRatio;
17446 m_upperImpulse *= data.step.dtRatio;
17447
17448 float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse;
17449 b2Vec2 P(m_impulse.x, m_impulse.y);
17450
17451 vA -= mA * P;
17452 wA -= iA * (b2Cross(m_rA, P) + axialImpulse);
17453
17454 vB += mB * P;
17455 wB += iB * (b2Cross(m_rB, P) + axialImpulse);
17456 } else {
17457 m_impulse.SetZero();
17458 m_motorImpulse = 0.0f;
17459 m_lowerImpulse = 0.0f;
17460 m_upperImpulse = 0.0f;
17461 }
17462
17463 data.velocities[m_indexA].v = vA;
17464 data.velocities[m_indexA].w = wA;
17465 data.velocities[m_indexB].v = vB;
17466 data.velocities[m_indexB].w = wB;
17467}
17468
17469void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) {
17470 b2Vec2 vA = data.velocities[m_indexA].v;
17471 float wA = data.velocities[m_indexA].w;
17472 b2Vec2 vB = data.velocities[m_indexB].v;
17473 float wB = data.velocities[m_indexB].w;
17474
17475 float mA = m_invMassA, mB = m_invMassB;
17476 float iA = m_invIA, iB = m_invIB;
17477
17478 bool fixedRotation = (iA + iB == 0.0f);
17479
17480 // Solve motor constraint.
17481 if (m_enableMotor && fixedRotation == false) {
17482 float Cdot = wB - wA - m_motorSpeed;
17483 float impulse = -m_axialMass * Cdot;
17484 float oldImpulse = m_motorImpulse;
17485 float maxImpulse = data.step.dt * m_maxMotorTorque;
17486 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
17487 impulse = m_motorImpulse - oldImpulse;
17488
17489 wA -= iA * impulse;
17490 wB += iB * impulse;
17491 }
17492
17493 if (m_enableLimit && fixedRotation == false) {
17494 // Lower limit
17495 {
17496 float C = m_angle - m_lowerAngle;
17497 float Cdot = wB - wA;
17498 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
17499 float oldImpulse = m_lowerImpulse;
17500 m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
17501 impulse = m_lowerImpulse - oldImpulse;
17502
17503 wA -= iA * impulse;
17504 wB += iB * impulse;
17505 }
17506
17507 // Upper limit
17508 // Note: signs are flipped to keep C positive when the constraint is satisfied.
17509 // This also keeps the impulse positive when the limit is active.
17510 {
17511 float C = m_upperAngle - m_angle;
17512 float Cdot = wA - wB;
17513 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
17514 float oldImpulse = m_upperImpulse;
17515 m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
17516 impulse = m_upperImpulse - oldImpulse;
17517
17518 wA += iA * impulse;
17519 wB -= iB * impulse;
17520 }
17521 }
17522
17523 // Solve point-to-point constraint
17524 {
17525 b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
17526 b2Vec2 impulse = m_K.Solve(-Cdot);
17527
17528 m_impulse.x += impulse.x;
17529 m_impulse.y += impulse.y;
17530
17531 vA -= mA * impulse;
17532 wA -= iA * b2Cross(m_rA, impulse);
17533
17534 vB += mB * impulse;
17535 wB += iB * b2Cross(m_rB, impulse);
17536 }
17537
17538 data.velocities[m_indexA].v = vA;
17539 data.velocities[m_indexA].w = wA;
17540 data.velocities[m_indexB].v = vB;
17541 data.velocities[m_indexB].w = wB;
17542}
17543
17544bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) {
17545 b2Vec2 cA = data.positions[m_indexA].c;
17546 float aA = data.positions[m_indexA].a;
17547 b2Vec2 cB = data.positions[m_indexB].c;
17548 float aB = data.positions[m_indexB].a;
17549
17550 b2Rot qA(aA), qB(aB);
17551
17552 float angularError = 0.0f;
17553 float positionError = 0.0f;
17554
17555 bool fixedRotation = (m_invIA + m_invIB == 0.0f);
17556
17557 // Solve angular limit constraint
17558 if (m_enableLimit && fixedRotation == false) {
17559 float angle = aB - aA - m_referenceAngle;
17560 float C = 0.0f;
17561
17562 if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) {
17563 // Prevent large angular corrections
17564 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
17565 } else if (angle <= m_lowerAngle) {
17566 // Prevent large angular corrections and allow some slop.
17567 C = b2Clamp(angle - m_lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
17568 } else if (angle >= m_upperAngle) {
17569 // Prevent large angular corrections and allow some slop.
17570 C = b2Clamp(angle - m_upperAngle - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
17571 }
17572
17573 float limitImpulse = -m_axialMass * C;
17574 aA -= m_invIA * limitImpulse;
17575 aB += m_invIB * limitImpulse;
17576 angularError = b2Abs(C);
17577 }
17578
17579 // Solve point-to-point constraint.
17580 {
17581 qA.Set(aA);
17582 qB.Set(aB);
17583 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17584 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17585
17586 b2Vec2 C = cB + rB - cA - rA;
17587 positionError = C.Length();
17588
17589 float mA = m_invMassA, mB = m_invMassB;
17590 float iA = m_invIA, iB = m_invIB;
17591
17592 b2Mat22 K;
17593 K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
17594 K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
17595 K.ey.x = K.ex.y;
17596 K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
17597
17598 b2Vec2 impulse = -K.Solve(C);
17599
17600 cA -= mA * impulse;
17601 aA -= iA * b2Cross(rA, impulse);
17602
17603 cB += mB * impulse;
17604 aB += iB * b2Cross(rB, impulse);
17605 }
17606
17607 data.positions[m_indexA].c = cA;
17608 data.positions[m_indexA].a = aA;
17609 data.positions[m_indexB].c = cB;
17610 data.positions[m_indexB].a = aB;
17611
17612 return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
17613}
17614
17615b2Vec2 b2RevoluteJoint::GetAnchorA() const {
17616 return m_bodyA->GetWorldPoint(m_localAnchorA);
17617}
17618
17619b2Vec2 b2RevoluteJoint::GetAnchorB() const {
17620 return m_bodyB->GetWorldPoint(m_localAnchorB);
17621}
17622
17623b2Vec2 b2RevoluteJoint::GetReactionForce(float inv_dt) const {
17624 b2Vec2 P(m_impulse.x, m_impulse.y);
17625 return inv_dt * P;
17626}
17627
17628float b2RevoluteJoint::GetReactionTorque(float inv_dt) const {
17629 return inv_dt * (m_motorImpulse + m_lowerImpulse - m_upperImpulse);
17630}
17631
17632float b2RevoluteJoint::GetJointAngle() const {
17633 b2Body* bA = m_bodyA;
17634 b2Body* bB = m_bodyB;
17635 return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
17636}
17637
17638float b2RevoluteJoint::GetJointSpeed() const {
17639 b2Body* bA = m_bodyA;
17640 b2Body* bB = m_bodyB;
17641 return bB->m_angularVelocity - bA->m_angularVelocity;
17642}
17643
17644bool b2RevoluteJoint::IsMotorEnabled() const {
17645 return m_enableMotor;
17646}
17647
17648void b2RevoluteJoint::EnableMotor(bool flag) {
17649 if (flag != m_enableMotor) {
17650 m_bodyA->SetAwake(true);
17651 m_bodyB->SetAwake(true);
17652 m_enableMotor = flag;
17653 }
17654}
17655
17656float b2RevoluteJoint::GetMotorTorque(float inv_dt) const {
17657 return inv_dt * m_motorImpulse;
17658}
17659
17660void b2RevoluteJoint::SetMotorSpeed(float speed) {
17661 if (speed != m_motorSpeed) {
17662 m_bodyA->SetAwake(true);
17663 m_bodyB->SetAwake(true);
17664 m_motorSpeed = speed;
17665 }
17666}
17667
17668void b2RevoluteJoint::SetMaxMotorTorque(float torque) {
17669 if (torque != m_maxMotorTorque) {
17670 m_bodyA->SetAwake(true);
17671 m_bodyB->SetAwake(true);
17672 m_maxMotorTorque = torque;
17673 }
17674}
17675
17676bool b2RevoluteJoint::IsLimitEnabled() const {
17677 return m_enableLimit;
17678}
17679
17680void b2RevoluteJoint::EnableLimit(bool flag) {
17681 if (flag != m_enableLimit) {
17682 m_bodyA->SetAwake(true);
17683 m_bodyB->SetAwake(true);
17684 m_enableLimit = flag;
17685 m_lowerImpulse = 0.0f;
17686 m_upperImpulse = 0.0f;
17687 }
17688}
17689
17690float b2RevoluteJoint::GetLowerLimit() const {
17691 return m_lowerAngle;
17692}
17693
17694float b2RevoluteJoint::GetUpperLimit() const {
17695 return m_upperAngle;
17696}
17697
17698void b2RevoluteJoint::SetLimits(float lower, float upper) {
17699 b2Assert(lower <= upper);
17700
17701 if (lower != m_lowerAngle || upper != m_upperAngle) {
17702 m_bodyA->SetAwake(true);
17703 m_bodyB->SetAwake(true);
17704 m_lowerImpulse = 0.0f;
17705 m_upperImpulse = 0.0f;
17706 m_lowerAngle = lower;
17707 m_upperAngle = upper;
17708 }
17709}
17710
17711void b2RevoluteJoint::Dump() {
17712 int32 indexA = m_bodyA->m_islandIndex;
17713 int32 indexB = m_bodyB->m_islandIndex;
17714
17715 b2Dump(" b2RevoluteJointDef jd;\n");
17716 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
17717 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
17718 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
17719 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
17720 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
17721 b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle);
17722 b2Dump(" jd.enableLimit = bool(%d);\n", m_enableLimit);
17723 b2Dump(" jd.lowerAngle = %.9g;\n", m_lowerAngle);
17724 b2Dump(" jd.upperAngle = %.9g;\n", m_upperAngle);
17725 b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor);
17726 b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed);
17727 b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque);
17728 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
17729}
17730
17731///
17732void b2RevoluteJoint::Draw(b2Draw* draw) const {
17733 const b2Transform& xfA = m_bodyA->GetTransform();
17734 const b2Transform& xfB = m_bodyB->GetTransform();
17735 b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
17736 b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
17737
17738 b2Color c1(0.7f, 0.7f, 0.7f);
17739 b2Color c2(0.3f, 0.9f, 0.3f);
17740 b2Color c3(0.9f, 0.3f, 0.3f);
17741 b2Color c4(0.3f, 0.3f, 0.9f);
17742 b2Color c5(0.4f, 0.4f, 0.4f);
17743
17744 draw->DrawPoint(pA, 5.0f, c4);
17745 draw->DrawPoint(pB, 5.0f, c5);
17746
17747 float aA = m_bodyA->GetAngle();
17748 float aB = m_bodyB->GetAngle();
17749 float angle = aB - aA - m_referenceAngle;
17750
17751 const float L = 0.5f;
17752
17753 b2Vec2 r = L * b2Vec2(cosf(angle), sinf(angle));
17754 draw->DrawSegment(pB, pB + r, c1);
17755 draw->DrawCircle(pB, L, c1);
17756
17757 if (m_enableLimit) {
17758 b2Vec2 rlo = L * b2Vec2(cosf(m_lowerAngle), sinf(m_lowerAngle));
17759 b2Vec2 rhi = L * b2Vec2(cosf(m_upperAngle), sinf(m_upperAngle));
17760
17761 draw->DrawSegment(pB, pB + rlo, c2);
17762 draw->DrawSegment(pB, pB + rhi, c3);
17763 }
17764
17765 b2Color color(0.5f, 0.8f, 0.8f);
17766 draw->DrawSegment(xfA.p, pA, color);
17767 draw->DrawSegment(pA, pB, color);
17768 draw->DrawSegment(xfB.p, pB, color);
17769}
17770// MIT License
17771
17772// Copyright (c) 2019 Erin Catto
17773
17774// Permission is hereby granted, free of charge, to any person obtaining a copy
17775// of this software and associated documentation files (the "Software"), to deal
17776// in the Software without restriction, including without limitation the rights
17777// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17778// copies of the Software, and to permit persons to whom the Software is
17779// furnished to do so, subject to the following conditions:
17780
17781// The above copyright notice and this permission notice shall be included in all
17782// copies or substantial portions of the Software.
17783
17784// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17785// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17786// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17787// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17788// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
17789// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
17790// SOFTWARE.
17791
17792#include "box2d/b2_body.h"
17793#include "box2d/b2_time_step.h"
17794#include "box2d/b2_weld_joint.h"
17795
17796// Point-to-point constraint
17797// C = p2 - p1
17798// Cdot = v2 - v1
17799// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
17800// J = [-I -r1_skew I r2_skew ]
17801// Identity used:
17802// w k % (rx i + ry j) = w * (-ry i + rx j)
17803
17804// Angle constraint
17805// C = angle2 - angle1 - referenceAngle
17806// Cdot = w2 - w1
17807// J = [0 0 -1 0 0 1]
17808// K = invI1 + invI2
17809
17810void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) {
17811 bodyA = bA;
17812 bodyB = bB;
17813 localAnchorA = bodyA->GetLocalPoint(anchor);
17814 localAnchorB = bodyB->GetLocalPoint(anchor);
17815 referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
17816}
17817
17818b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
17819 : b2Joint(def) {
17820 m_localAnchorA = def->localAnchorA;
17821 m_localAnchorB = def->localAnchorB;
17822 m_referenceAngle = def->referenceAngle;
17823 m_stiffness = def->stiffness;
17824 m_damping = def->damping;
17825
17826 m_impulse.SetZero();
17827}
17828
17829void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) {
17830 m_indexA = m_bodyA->m_islandIndex;
17831 m_indexB = m_bodyB->m_islandIndex;
17832 m_localCenterA = m_bodyA->m_sweep.localCenter;
17833 m_localCenterB = m_bodyB->m_sweep.localCenter;
17834 m_invMassA = m_bodyA->m_invMass;
17835 m_invMassB = m_bodyB->m_invMass;
17836 m_invIA = m_bodyA->m_invI;
17837 m_invIB = m_bodyB->m_invI;
17838
17839 float aA = data.positions[m_indexA].a;
17840 b2Vec2 vA = data.velocities[m_indexA].v;
17841 float wA = data.velocities[m_indexA].w;
17842
17843 float aB = data.positions[m_indexB].a;
17844 b2Vec2 vB = data.velocities[m_indexB].v;
17845 float wB = data.velocities[m_indexB].w;
17846
17847 b2Rot qA(aA), qB(aB);
17848
17849 m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17850 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17851
17852 // J = [-I -r1_skew I r2_skew]
17853 // [ 0 -1 0 1]
17854 // r_skew = [-ry; rx]
17855
17856 // Matlab
17857 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
17858 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
17859 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
17860
17861 float mA = m_invMassA, mB = m_invMassB;
17862 float iA = m_invIA, iB = m_invIB;
17863
17864 b2Mat33 K;
17865 K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
17866 K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
17867 K.ez.x = -m_rA.y * iA - m_rB.y * iB;
17868 K.ex.y = K.ey.x;
17869 K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
17870 K.ez.y = m_rA.x * iA + m_rB.x * iB;
17871 K.ex.z = K.ez.x;
17872 K.ey.z = K.ez.y;
17873 K.ez.z = iA + iB;
17874
17875 if (m_stiffness > 0.0f) {
17876 K.GetInverse22(&m_mass);
17877
17878 float invM = iA + iB;
17879
17880 float C = aB - aA - m_referenceAngle;
17881
17882 // Damping coefficient
17883 float d = m_damping;
17884
17885 // Spring stiffness
17886 float k = m_stiffness;
17887
17888 // magic formulas
17889 float h = data.step.dt;
17890 m_gamma = h * (d + h * k);
17891 m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
17892 m_bias = C * h * k * m_gamma;
17893
17894 invM += m_gamma;
17895 m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
17896 } else if (K.ez.z == 0.0f) {
17897 K.GetInverse22(&m_mass);
17898 m_gamma = 0.0f;
17899 m_bias = 0.0f;
17900 } else {
17901 K.GetSymInverse33(&m_mass);
17902 m_gamma = 0.0f;
17903 m_bias = 0.0f;
17904 }
17905
17906 if (data.step.warmStarting) {
17907 // Scale impulses to support a variable time step.
17908 m_impulse *= data.step.dtRatio;
17909
17910 b2Vec2 P(m_impulse.x, m_impulse.y);
17911
17912 vA -= mA * P;
17913 wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
17914
17915 vB += mB * P;
17916 wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
17917 } else {
17918 m_impulse.SetZero();
17919 }
17920
17921 data.velocities[m_indexA].v = vA;
17922 data.velocities[m_indexA].w = wA;
17923 data.velocities[m_indexB].v = vB;
17924 data.velocities[m_indexB].w = wB;
17925}
17926
17927void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) {
17928 b2Vec2 vA = data.velocities[m_indexA].v;
17929 float wA = data.velocities[m_indexA].w;
17930 b2Vec2 vB = data.velocities[m_indexB].v;
17931 float wB = data.velocities[m_indexB].w;
17932
17933 float mA = m_invMassA, mB = m_invMassB;
17934 float iA = m_invIA, iB = m_invIB;
17935
17936 if (m_stiffness > 0.0f) {
17937 float Cdot2 = wB - wA;
17938
17939 float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
17940 m_impulse.z += impulse2;
17941
17942 wA -= iA * impulse2;
17943 wB += iB * impulse2;
17944
17945 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
17946
17947 b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
17948 m_impulse.x += impulse1.x;
17949 m_impulse.y += impulse1.y;
17950
17951 b2Vec2 P = impulse1;
17952
17953 vA -= mA * P;
17954 wA -= iA * b2Cross(m_rA, P);
17955
17956 vB += mB * P;
17957 wB += iB * b2Cross(m_rB, P);
17958 } else {
17959 b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
17960 float Cdot2 = wB - wA;
17961 b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
17962
17963 b2Vec3 impulse = -b2Mul(m_mass, Cdot);
17964 m_impulse += impulse;
17965
17966 b2Vec2 P(impulse.x, impulse.y);
17967
17968 vA -= mA * P;
17969 wA -= iA * (b2Cross(m_rA, P) + impulse.z);
17970
17971 vB += mB * P;
17972 wB += iB * (b2Cross(m_rB, P) + impulse.z);
17973 }
17974
17975 data.velocities[m_indexA].v = vA;
17976 data.velocities[m_indexA].w = wA;
17977 data.velocities[m_indexB].v = vB;
17978 data.velocities[m_indexB].w = wB;
17979}
17980
17981bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) {
17982 b2Vec2 cA = data.positions[m_indexA].c;
17983 float aA = data.positions[m_indexA].a;
17984 b2Vec2 cB = data.positions[m_indexB].c;
17985 float aB = data.positions[m_indexB].a;
17986
17987 b2Rot qA(aA), qB(aB);
17988
17989 float mA = m_invMassA, mB = m_invMassB;
17990 float iA = m_invIA, iB = m_invIB;
17991
17992 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
17993 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
17994
17995 float positionError, angularError;
17996
17997 b2Mat33 K;
17998 K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
17999 K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
18000 K.ez.x = -rA.y * iA - rB.y * iB;
18001 K.ex.y = K.ey.x;
18002 K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
18003 K.ez.y = rA.x * iA + rB.x * iB;
18004 K.ex.z = K.ez.x;
18005 K.ey.z = K.ez.y;
18006 K.ez.z = iA + iB;
18007
18008 if (m_stiffness > 0.0f) {
18009 b2Vec2 C1 = cB + rB - cA - rA;
18010
18011 positionError = C1.Length();
18012 angularError = 0.0f;
18013
18014 b2Vec2 P = -K.Solve22(C1);
18015
18016 cA -= mA * P;
18017 aA -= iA * b2Cross(rA, P);
18018
18019 cB += mB * P;
18020 aB += iB * b2Cross(rB, P);
18021 } else {
18022 b2Vec2 C1 = cB + rB - cA - rA;
18023 float C2 = aB - aA - m_referenceAngle;
18024
18025 positionError = C1.Length();
18026 angularError = b2Abs(C2);
18027
18028 b2Vec3 C(C1.x, C1.y, C2);
18029
18030 b2Vec3 impulse;
18031 if (K.ez.z > 0.0f) {
18032 impulse = -K.Solve33(C);
18033 } else {
18034 b2Vec2 impulse2 = -K.Solve22(C1);
18035 impulse.Set(impulse2.x, impulse2.y, 0.0f);
18036 }
18037
18038 b2Vec2 P(impulse.x, impulse.y);
18039
18040 cA -= mA * P;
18041 aA -= iA * (b2Cross(rA, P) + impulse.z);
18042
18043 cB += mB * P;
18044 aB += iB * (b2Cross(rB, P) + impulse.z);
18045 }
18046
18047 data.positions[m_indexA].c = cA;
18048 data.positions[m_indexA].a = aA;
18049 data.positions[m_indexB].c = cB;
18050 data.positions[m_indexB].a = aB;
18051
18052 return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
18053}
18054
18055b2Vec2 b2WeldJoint::GetAnchorA() const {
18056 return m_bodyA->GetWorldPoint(m_localAnchorA);
18057}
18058
18059b2Vec2 b2WeldJoint::GetAnchorB() const {
18060 return m_bodyB->GetWorldPoint(m_localAnchorB);
18061}
18062
18063b2Vec2 b2WeldJoint::GetReactionForce(float inv_dt) const {
18064 b2Vec2 P(m_impulse.x, m_impulse.y);
18065 return inv_dt * P;
18066}
18067
18068float b2WeldJoint::GetReactionTorque(float inv_dt) const {
18069 return inv_dt * m_impulse.z;
18070}
18071
18072void b2WeldJoint::Dump() {
18073 int32 indexA = m_bodyA->m_islandIndex;
18074 int32 indexB = m_bodyB->m_islandIndex;
18075
18076 b2Dump(" b2WeldJointDef jd;\n");
18077 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
18078 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
18079 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
18080 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
18081 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
18082 b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle);
18083 b2Dump(" jd.stiffness = %.9g;\n", m_stiffness);
18084 b2Dump(" jd.damping = %.9g;\n", m_damping);
18085 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
18086}
18087// MIT License
18088
18089// Copyright (c) 2019 Erin Catto
18090
18091// Permission is hereby granted, free of charge, to any person obtaining a copy
18092// of this software and associated documentation files (the "Software"), to deal
18093// in the Software without restriction, including without limitation the rights
18094// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
18095// copies of the Software, and to permit persons to whom the Software is
18096// furnished to do so, subject to the following conditions:
18097
18098// The above copyright notice and this permission notice shall be included in all
18099// copies or substantial portions of the Software.
18100
18101// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18102// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18103// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18104// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18105// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18106// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
18107// SOFTWARE.
18108
18109#include "box2d/b2_body.h"
18110#include "box2d/b2_draw.h"
18111#include "box2d/b2_wheel_joint.h"
18112#include "box2d/b2_time_step.h"
18113
18114// Linear constraint (point-to-line)
18115// d = pB - pA = xB + rB - xA - rA
18116// C = dot(ay, d)
18117// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
18118// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
18119// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
18120
18121// Spring linear constraint
18122// C = dot(ax, d)
18123// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
18124// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
18125
18126// Motor rotational constraint
18127// Cdot = wB - wA
18128// J = [0 0 -1 0 0 1]
18129
18130void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) {
18131 bodyA = bA;
18132 bodyB = bB;
18133 localAnchorA = bodyA->GetLocalPoint(anchor);
18134 localAnchorB = bodyB->GetLocalPoint(anchor);
18135 localAxisA = bodyA->GetLocalVector(axis);
18136}
18137
18138b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
18139 : b2Joint(def) {
18140 m_localAnchorA = def->localAnchorA;
18141 m_localAnchorB = def->localAnchorB;
18142 m_localXAxisA = def->localAxisA;
18143 m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
18144
18145 m_mass = 0.0f;
18146 m_impulse = 0.0f;
18147 m_motorMass = 0.0f;
18148 m_motorImpulse = 0.0f;
18149 m_springMass = 0.0f;
18150 m_springImpulse = 0.0f;
18151
18152 m_axialMass = 0.0f;
18153 m_lowerImpulse = 0.0f;
18154 m_upperImpulse = 0.0f;
18155 m_lowerTranslation = def->lowerTranslation;
18156 m_upperTranslation = def->upperTranslation;
18157 m_enableLimit = def->enableLimit;
18158
18159 m_maxMotorTorque = def->maxMotorTorque;
18160 m_motorSpeed = def->motorSpeed;
18161 m_enableMotor = def->enableMotor;
18162
18163 m_bias = 0.0f;
18164 m_gamma = 0.0f;
18165
18166 m_ax.SetZero();
18167 m_ay.SetZero();
18168
18169 m_stiffness = def->stiffness;
18170 m_damping = def->damping;
18171}
18172
18173void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) {
18174 m_indexA = m_bodyA->m_islandIndex;
18175 m_indexB = m_bodyB->m_islandIndex;
18176 m_localCenterA = m_bodyA->m_sweep.localCenter;
18177 m_localCenterB = m_bodyB->m_sweep.localCenter;
18178 m_invMassA = m_bodyA->m_invMass;
18179 m_invMassB = m_bodyB->m_invMass;
18180 m_invIA = m_bodyA->m_invI;
18181 m_invIB = m_bodyB->m_invI;
18182
18183 float mA = m_invMassA, mB = m_invMassB;
18184 float iA = m_invIA, iB = m_invIB;
18185
18186 b2Vec2 cA = data.positions[m_indexA].c;
18187 float aA = data.positions[m_indexA].a;
18188 b2Vec2 vA = data.velocities[m_indexA].v;
18189 float wA = data.velocities[m_indexA].w;
18190
18191 b2Vec2 cB = data.positions[m_indexB].c;
18192 float aB = data.positions[m_indexB].a;
18193 b2Vec2 vB = data.velocities[m_indexB].v;
18194 float wB = data.velocities[m_indexB].w;
18195
18196 b2Rot qA(aA), qB(aB);
18197
18198 // Compute the effective masses.
18199 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
18200 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
18201 b2Vec2 d = cB + rB - cA - rA;
18202
18203 // Point to line constraint
18204 {
18205 m_ay = b2Mul(qA, m_localYAxisA);
18206 m_sAy = b2Cross(d + rA, m_ay);
18207 m_sBy = b2Cross(rB, m_ay);
18208
18209 m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
18210
18211 if (m_mass > 0.0f) {
18212 m_mass = 1.0f / m_mass;
18213 }
18214 }
18215
18216 // Spring constraint
18217 m_ax = b2Mul(qA, m_localXAxisA);
18218 m_sAx = b2Cross(d + rA, m_ax);
18219 m_sBx = b2Cross(rB, m_ax);
18220
18221 const float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
18222 if (invMass > 0.0f) {
18223 m_axialMass = 1.0f / invMass;
18224 } else {
18225 m_axialMass = 0.0f;
18226 }
18227
18228 m_springMass = 0.0f;
18229 m_bias = 0.0f;
18230 m_gamma = 0.0f;
18231
18232 if (m_stiffness > 0.0f && invMass > 0.0f) {
18233 m_springMass = 1.0f / invMass;
18234
18235 float C = b2Dot(d, m_ax);
18236
18237 // magic formulas
18238 float h = data.step.dt;
18239 m_gamma = h * (m_damping + h * m_stiffness);
18240 if (m_gamma > 0.0f) {
18241 m_gamma = 1.0f / m_gamma;
18242 }
18243
18244 m_bias = C * h * m_stiffness * m_gamma;
18245
18246 m_springMass = invMass + m_gamma;
18247 if (m_springMass > 0.0f) {
18248 m_springMass = 1.0f / m_springMass;
18249 }
18250 } else {
18251 m_springImpulse = 0.0f;
18252 }
18253
18254 if (m_enableLimit) {
18255 m_translation = b2Dot(m_ax, d);
18256 } else {
18257 m_lowerImpulse = 0.0f;
18258 m_upperImpulse = 0.0f;
18259 }
18260
18261 if (m_enableMotor) {
18262 m_motorMass = iA + iB;
18263 if (m_motorMass > 0.0f) {
18264 m_motorMass = 1.0f / m_motorMass;
18265 }
18266 } else {
18267 m_motorMass = 0.0f;
18268 m_motorImpulse = 0.0f;
18269 }
18270
18271 if (data.step.warmStarting) {
18272 // Account for variable time step.
18273 m_impulse *= data.step.dtRatio;
18274 m_springImpulse *= data.step.dtRatio;
18275 m_motorImpulse *= data.step.dtRatio;
18276
18277 float axialImpulse = m_springImpulse + m_lowerImpulse - m_upperImpulse;
18278 b2Vec2 P = m_impulse * m_ay + axialImpulse * m_ax;
18279 float LA = m_impulse * m_sAy + axialImpulse * m_sAx + m_motorImpulse;
18280 float LB = m_impulse * m_sBy + axialImpulse * m_sBx + m_motorImpulse;
18281
18282 vA -= m_invMassA * P;
18283 wA -= m_invIA * LA;
18284
18285 vB += m_invMassB * P;
18286 wB += m_invIB * LB;
18287 } else {
18288 m_impulse = 0.0f;
18289 m_springImpulse = 0.0f;
18290 m_motorImpulse = 0.0f;
18291 m_lowerImpulse = 0.0f;
18292 m_upperImpulse = 0.0f;
18293 }
18294
18295 data.velocities[m_indexA].v = vA;
18296 data.velocities[m_indexA].w = wA;
18297 data.velocities[m_indexB].v = vB;
18298 data.velocities[m_indexB].w = wB;
18299}
18300
18301void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) {
18302 float mA = m_invMassA, mB = m_invMassB;
18303 float iA = m_invIA, iB = m_invIB;
18304
18305 b2Vec2 vA = data.velocities[m_indexA].v;
18306 float wA = data.velocities[m_indexA].w;
18307 b2Vec2 vB = data.velocities[m_indexB].v;
18308 float wB = data.velocities[m_indexB].w;
18309
18310 // Solve spring constraint
18311 {
18312 float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
18313 float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
18314 m_springImpulse += impulse;
18315
18316 b2Vec2 P = impulse * m_ax;
18317 float LA = impulse * m_sAx;
18318 float LB = impulse * m_sBx;
18319
18320 vA -= mA * P;
18321 wA -= iA * LA;
18322
18323 vB += mB * P;
18324 wB += iB * LB;
18325 }
18326
18327 // Solve rotational motor constraint
18328 {
18329 float Cdot = wB - wA - m_motorSpeed;
18330 float impulse = -m_motorMass * Cdot;
18331
18332 float oldImpulse = m_motorImpulse;
18333 float maxImpulse = data.step.dt * m_maxMotorTorque;
18334 m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
18335 impulse = m_motorImpulse - oldImpulse;
18336
18337 wA -= iA * impulse;
18338 wB += iB * impulse;
18339 }
18340
18341 if (m_enableLimit) {
18342 // Lower limit
18343 {
18344 float C = m_translation - m_lowerTranslation;
18345 float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
18346 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
18347 float oldImpulse = m_lowerImpulse;
18348 m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
18349 impulse = m_lowerImpulse - oldImpulse;
18350
18351 b2Vec2 P = impulse * m_ax;
18352 float LA = impulse * m_sAx;
18353 float LB = impulse * m_sBx;
18354
18355 vA -= mA * P;
18356 wA -= iA * LA;
18357 vB += mB * P;
18358 wB += iB * LB;
18359 }
18360
18361 // Upper limit
18362 // Note: signs are flipped to keep C positive when the constraint is satisfied.
18363 // This also keeps the impulse positive when the limit is active.
18364 {
18365 float C = m_upperTranslation - m_translation;
18366 float Cdot = b2Dot(m_ax, vA - vB) + m_sAx * wA - m_sBx * wB;
18367 float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
18368 float oldImpulse = m_upperImpulse;
18369 m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
18370 impulse = m_upperImpulse - oldImpulse;
18371
18372 b2Vec2 P = impulse * m_ax;
18373 float LA = impulse * m_sAx;
18374 float LB = impulse * m_sBx;
18375
18376 vA += mA * P;
18377 wA += iA * LA;
18378 vB -= mB * P;
18379 wB -= iB * LB;
18380 }
18381 }
18382
18383 // Solve point to line constraint
18384 {
18385 float Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
18386 float impulse = -m_mass * Cdot;
18387 m_impulse += impulse;
18388
18389 b2Vec2 P = impulse * m_ay;
18390 float LA = impulse * m_sAy;
18391 float LB = impulse * m_sBy;
18392
18393 vA -= mA * P;
18394 wA -= iA * LA;
18395
18396 vB += mB * P;
18397 wB += iB * LB;
18398 }
18399
18400 data.velocities[m_indexA].v = vA;
18401 data.velocities[m_indexA].w = wA;
18402 data.velocities[m_indexB].v = vB;
18403 data.velocities[m_indexB].w = wB;
18404}
18405
18406bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) {
18407 b2Vec2 cA = data.positions[m_indexA].c;
18408 float aA = data.positions[m_indexA].a;
18409 b2Vec2 cB = data.positions[m_indexB].c;
18410 float aB = data.positions[m_indexB].a;
18411
18412 float linearError = 0.0f;
18413
18414 if (m_enableLimit) {
18415 b2Rot qA(aA), qB(aB);
18416
18417 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
18418 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
18419 b2Vec2 d = (cB - cA) + rB - rA;
18420
18421 b2Vec2 ax = b2Mul(qA, m_localXAxisA);
18422 float sAx = b2Cross(d + rA, m_ax);
18423 float sBx = b2Cross(rB, m_ax);
18424
18425 float C = 0.0f;
18426 float translation = b2Dot(ax, d);
18427 if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) {
18428 C = translation;
18429 } else if (translation <= m_lowerTranslation) {
18430 C = b2Min(translation - m_lowerTranslation, 0.0f);
18431 } else if (translation >= m_upperTranslation) {
18432 C = b2Max(translation - m_upperTranslation, 0.0f);
18433 }
18434
18435 if (C != 0.0f) {
18436
18437 float invMass = m_invMassA + m_invMassB + m_invIA * sAx * sAx + m_invIB * sBx * sBx;
18438 float impulse = 0.0f;
18439 if (invMass != 0.0f) {
18440 impulse = -C / invMass;
18441 }
18442
18443 b2Vec2 P = impulse * ax;
18444 float LA = impulse * sAx;
18445 float LB = impulse * sBx;
18446
18447 cA -= m_invMassA * P;
18448 aA -= m_invIA * LA;
18449 cB += m_invMassB * P;
18450 aB += m_invIB * LB;
18451
18452 linearError = b2Abs(C);
18453 }
18454 }
18455
18456 // Solve perpendicular constraint
18457 {
18458 b2Rot qA(aA), qB(aB);
18459
18460 b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
18461 b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
18462 b2Vec2 d = (cB - cA) + rB - rA;
18463
18464 b2Vec2 ay = b2Mul(qA, m_localYAxisA);
18465
18466 float sAy = b2Cross(d + rA, ay);
18467 float sBy = b2Cross(rB, ay);
18468
18469 float C = b2Dot(d, ay);
18470
18471 float invMass = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
18472
18473 float impulse = 0.0f;
18474 if (invMass != 0.0f) {
18475 impulse = -C / invMass;
18476 }
18477
18478 b2Vec2 P = impulse * ay;
18479 float LA = impulse * sAy;
18480 float LB = impulse * sBy;
18481
18482 cA -= m_invMassA * P;
18483 aA -= m_invIA * LA;
18484 cB += m_invMassB * P;
18485 aB += m_invIB * LB;
18486
18487 linearError = b2Max(linearError, b2Abs(C));
18488 }
18489
18490 data.positions[m_indexA].c = cA;
18491 data.positions[m_indexA].a = aA;
18492 data.positions[m_indexB].c = cB;
18493 data.positions[m_indexB].a = aB;
18494
18495 return linearError <= b2_linearSlop;
18496}
18497
18498b2Vec2 b2WheelJoint::GetAnchorA() const {
18499 return m_bodyA->GetWorldPoint(m_localAnchorA);
18500}
18501
18502b2Vec2 b2WheelJoint::GetAnchorB() const {
18503 return m_bodyB->GetWorldPoint(m_localAnchorB);
18504}
18505
18506b2Vec2 b2WheelJoint::GetReactionForce(float inv_dt) const {
18507 return inv_dt * (m_impulse * m_ay + (m_springImpulse + m_lowerImpulse - m_upperImpulse) * m_ax);
18508}
18509
18510float b2WheelJoint::GetReactionTorque(float inv_dt) const {
18511 return inv_dt * m_motorImpulse;
18512}
18513
18514float b2WheelJoint::GetJointTranslation() const {
18515 b2Body* bA = m_bodyA;
18516 b2Body* bB = m_bodyB;
18517
18518 b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
18519 b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
18520 b2Vec2 d = pB - pA;
18521 b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
18522
18523 float translation = b2Dot(d, axis);
18524 return translation;
18525}
18526
18527float b2WheelJoint::GetJointLinearSpeed() const {
18528 b2Body* bA = m_bodyA;
18529 b2Body* bB = m_bodyB;
18530
18531 b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
18532 b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
18533 b2Vec2 p1 = bA->m_sweep.c + rA;
18534 b2Vec2 p2 = bB->m_sweep.c + rB;
18535 b2Vec2 d = p2 - p1;
18536 b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
18537
18538 b2Vec2 vA = bA->m_linearVelocity;
18539 b2Vec2 vB = bB->m_linearVelocity;
18540 float wA = bA->m_angularVelocity;
18541 float wB = bB->m_angularVelocity;
18542
18543 float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
18544 return speed;
18545}
18546
18547float b2WheelJoint::GetJointAngle() const {
18548 b2Body* bA = m_bodyA;
18549 b2Body* bB = m_bodyB;
18550 return bB->m_sweep.a - bA->m_sweep.a;
18551}
18552
18553float b2WheelJoint::GetJointAngularSpeed() const {
18554 float wA = m_bodyA->m_angularVelocity;
18555 float wB = m_bodyB->m_angularVelocity;
18556 return wB - wA;
18557}
18558
18559bool b2WheelJoint::IsLimitEnabled() const {
18560 return m_enableLimit;
18561}
18562
18563void b2WheelJoint::EnableLimit(bool flag) {
18564 if (flag != m_enableLimit) {
18565 m_bodyA->SetAwake(true);
18566 m_bodyB->SetAwake(true);
18567 m_enableLimit = flag;
18568 m_lowerImpulse = 0.0f;
18569 m_upperImpulse = 0.0f;
18570 }
18571}
18572
18573float b2WheelJoint::GetLowerLimit() const {
18574 return m_lowerTranslation;
18575}
18576
18577float b2WheelJoint::GetUpperLimit() const {
18578 return m_upperTranslation;
18579}
18580
18581void b2WheelJoint::SetLimits(float lower, float upper) {
18582 b2Assert(lower <= upper);
18583 if (lower != m_lowerTranslation || upper != m_upperTranslation) {
18584 m_bodyA->SetAwake(true);
18585 m_bodyB->SetAwake(true);
18586 m_lowerTranslation = lower;
18587 m_upperTranslation = upper;
18588 m_lowerImpulse = 0.0f;
18589 m_upperImpulse = 0.0f;
18590 }
18591}
18592
18593bool b2WheelJoint::IsMotorEnabled() const {
18594 return m_enableMotor;
18595}
18596
18597void b2WheelJoint::EnableMotor(bool flag) {
18598 if (flag != m_enableMotor) {
18599 m_bodyA->SetAwake(true);
18600 m_bodyB->SetAwake(true);
18601 m_enableMotor = flag;
18602 }
18603}
18604
18605void b2WheelJoint::SetMotorSpeed(float speed) {
18606 if (speed != m_motorSpeed) {
18607 m_bodyA->SetAwake(true);
18608 m_bodyB->SetAwake(true);
18609 m_motorSpeed = speed;
18610 }
18611}
18612
18613void b2WheelJoint::SetMaxMotorTorque(float torque) {
18614 if (torque != m_maxMotorTorque) {
18615 m_bodyA->SetAwake(true);
18616 m_bodyB->SetAwake(true);
18617 m_maxMotorTorque = torque;
18618 }
18619}
18620
18621float b2WheelJoint::GetMotorTorque(float inv_dt) const {
18622 return inv_dt * m_motorImpulse;
18623}
18624
18625void b2WheelJoint::SetStiffness(float stiffness) {
18626 m_stiffness = stiffness;
18627}
18628
18629float b2WheelJoint::GetStiffness() const {
18630 return m_stiffness;
18631}
18632
18633void b2WheelJoint::SetDamping(float damping) {
18634 m_damping = damping;
18635}
18636
18637float b2WheelJoint::GetDamping() const {
18638 return m_damping;
18639}
18640
18641void b2WheelJoint::Dump() {
18642 // FLT_DECIMAL_DIG == 9
18643
18644 int32 indexA = m_bodyA->m_islandIndex;
18645 int32 indexB = m_bodyB->m_islandIndex;
18646
18647 b2Dump(" b2WheelJointDef jd;\n");
18648 b2Dump(" jd.bodyA = bodies[%d];\n", indexA);
18649 b2Dump(" jd.bodyB = bodies[%d];\n", indexB);
18650 b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected);
18651 b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
18652 b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
18653 b2Dump(" jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y);
18654 b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor);
18655 b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed);
18656 b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque);
18657 b2Dump(" jd.stiffness = %.9g;\n", m_stiffness);
18658 b2Dump(" jd.damping = %.9g;\n", m_damping);
18659 b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
18660}
18661
18662///
18663void b2WheelJoint::Draw(b2Draw* draw) const {
18664 const b2Transform& xfA = m_bodyA->GetTransform();
18665 const b2Transform& xfB = m_bodyB->GetTransform();
18666 b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
18667 b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
18668
18669 b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA);
18670
18671 b2Color c1(0.7f, 0.7f, 0.7f);
18672 b2Color c2(0.3f, 0.9f, 0.3f);
18673 b2Color c3(0.9f, 0.3f, 0.3f);
18674 b2Color c4(0.3f, 0.3f, 0.9f);
18675 b2Color c5(0.4f, 0.4f, 0.4f);
18676
18677 draw->DrawSegment(pA, pB, c5);
18678
18679 if (m_enableLimit) {
18680 b2Vec2 lower = pA + m_lowerTranslation * axis;
18681 b2Vec2 upper = pA + m_upperTranslation * axis;
18682 b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA);
18683 draw->DrawSegment(lower, upper, c1);
18684 draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2);
18685 draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3);
18686 } else {
18687 draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1);
18688 }
18689
18690 draw->DrawPoint(pA, 5.0f, c1);
18691 draw->DrawPoint(pB, 5.0f, c4);
18692}
18693// MIT License
18694
18695// Copyright (c) 2019 Erin Catto
18696
18697// Permission is hereby granted, free of charge, to any person obtaining a copy
18698// of this software and associated documentation files (the "Software"), to deal
18699// in the Software without restriction, including without limitation the rights
18700// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
18701// copies of the Software, and to permit persons to whom the Software is
18702// furnished to do so, subject to the following conditions:
18703
18704// The above copyright notice and this permission notice shall be included in all
18705// copies or substantial portions of the Software.
18706
18707// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18708// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18709// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18710// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18711// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18712// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
18713// SOFTWARE.
18714
18715#include "b2_contact_solver.h"
18716#include "b2_island.h"
18717
18718#include "box2d/b2_body.h"
18719#include "box2d/b2_broad_phase.h"
18720#include "box2d/b2_chain_shape.h"
18721#include "box2d/b2_circle_shape.h"
18722#include "box2d/b2_collision.h"
18723#include "box2d/b2_contact.h"
18724#include "box2d/b2_draw.h"
18725#include "box2d/b2_edge_shape.h"
18726#include "box2d/b2_fixture.h"
18727#include "box2d/b2_polygon_shape.h"
18728#include "box2d/b2_pulley_joint.h"
18729#include "box2d/b2_time_of_impact.h"
18730#include "box2d/b2_timer.h"
18731#include "box2d/b2_world.h"
18732
18733#include <new>
18734
18735b2World::b2World(const b2Vec2& gravity) {
18736 m_destructionListener = nullptr;
18737 m_debugDraw = nullptr;
18738
18739 m_bodyList = nullptr;
18740 m_jointList = nullptr;
18741
18742 m_bodyCount = 0;
18743 m_jointCount = 0;
18744
18745 m_warmStarting = true;
18746 m_continuousPhysics = true;
18747 m_subStepping = false;
18748
18749 m_stepComplete = true;
18750
18751 m_allowSleep = true;
18752 m_gravity = gravity;
18753
18754 m_newContacts = false;
18755 m_locked = false;
18756 m_clearForces = true;
18757
18758 m_inv_dt0 = 0.0f;
18759
18760 m_contactManager.m_allocator = &m_blockAllocator;
18761
18762 memset(&m_profile, 0, sizeof(b2Profile));
18763}
18764
18765b2World::~b2World() {
18766 // Some shapes allocate using b2Alloc.
18767 b2Body* b = m_bodyList;
18768 while (b) {
18769 b2Body* bNext = b->m_next;
18770
18771 b2Fixture* f = b->m_fixtureList;
18772 while (f) {
18773 b2Fixture* fNext = f->m_next;
18774 f->m_proxyCount = 0;
18775 f->Destroy(&m_blockAllocator);
18776 f = fNext;
18777 }
18778
18779 b = bNext;
18780 }
18781}
18782
18783void b2World::SetDestructionListener(b2DestructionListener* listener) {
18784 m_destructionListener = listener;
18785}
18786
18787void b2World::SetContactFilter(b2ContactFilter* filter) {
18788 m_contactManager.m_contactFilter = filter;
18789}
18790
18791void b2World::SetContactListener(b2ContactListener* listener) {
18792 m_contactManager.m_contactListener = listener;
18793}
18794
18795void b2World::SetDebugDraw(b2Draw* debugDraw) {
18796 m_debugDraw = debugDraw;
18797}
18798
18799b2Body* b2World::CreateBody(const b2BodyDef* def) {
18800 b2Assert(IsLocked() == false);
18801 if (IsLocked()) {
18802 return nullptr;
18803 }
18804
18805 void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
18806 b2Body* b = new (mem) b2Body(def, this);
18807
18808 // Add to world doubly linked list.
18809 b->m_prev = nullptr;
18810 b->m_next = m_bodyList;
18811 if (m_bodyList) {
18812 m_bodyList->m_prev = b;
18813 }
18814 m_bodyList = b;
18815 ++m_bodyCount;
18816
18817 return b;
18818}
18819
18820void b2World::DestroyBody(b2Body* b) {
18821 b2Assert(m_bodyCount > 0);
18822 b2Assert(IsLocked() == false);
18823 if (IsLocked()) {
18824 return;
18825 }
18826
18827 // Delete the attached joints.
18828 b2JointEdge* je = b->m_jointList;
18829 while (je) {
18830 b2JointEdge* je0 = je;
18831 je = je->next;
18832
18833 if (m_destructionListener) {
18834 m_destructionListener->SayGoodbye(je0->joint);
18835 }
18836
18837 DestroyJoint(je0->joint);
18838
18839 b->m_jointList = je;
18840 }
18841 b->m_jointList = nullptr;
18842
18843 // Delete the attached contacts.
18844 b2ContactEdge* ce = b->m_contactList;
18845 while (ce) {
18846 b2ContactEdge* ce0 = ce;
18847 ce = ce->next;
18848 m_contactManager.Destroy(ce0->contact);
18849 }
18850 b->m_contactList = nullptr;
18851
18852 // Delete the attached fixtures. This destroys broad-phase proxies.
18853 b2Fixture* f = b->m_fixtureList;
18854 while (f) {
18855 b2Fixture* f0 = f;
18856 f = f->m_next;
18857
18858 if (m_destructionListener) {
18859 m_destructionListener->SayGoodbye(f0);
18860 }
18861
18862 f0->DestroyProxies(&m_contactManager.m_broadPhase);
18863 f0->Destroy(&m_blockAllocator);
18864 f0->~b2Fixture();
18865 m_blockAllocator.Free(f0, sizeof(b2Fixture));
18866
18867 b->m_fixtureList = f;
18868 b->m_fixtureCount -= 1;
18869 }
18870 b->m_fixtureList = nullptr;
18871 b->m_fixtureCount = 0;
18872
18873 // Remove world body list.
18874 if (b->m_prev) {
18875 b->m_prev->m_next = b->m_next;
18876 }
18877
18878 if (b->m_next) {
18879 b->m_next->m_prev = b->m_prev;
18880 }
18881
18882 if (b == m_bodyList) {
18883 m_bodyList = b->m_next;
18884 }
18885
18886 --m_bodyCount;
18887 b->~b2Body();
18888 m_blockAllocator.Free(b, sizeof(b2Body));
18889}
18890
18891b2Joint* b2World::CreateJoint(const b2JointDef* def) {
18892 b2Assert(IsLocked() == false);
18893 if (IsLocked()) {
18894 return nullptr;
18895 }
18896
18897 b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
18898
18899 // Connect to the world list.
18900 j->m_prev = nullptr;
18901 j->m_next = m_jointList;
18902 if (m_jointList) {
18903 m_jointList->m_prev = j;
18904 }
18905 m_jointList = j;
18906 ++m_jointCount;
18907
18908 // Connect to the bodies' doubly linked lists.
18909 j->m_edgeA.joint = j;
18910 j->m_edgeA.other = j->m_bodyB;
18911 j->m_edgeA.prev = nullptr;
18912 j->m_edgeA.next = j->m_bodyA->m_jointList;
18913 if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
18914 j->m_bodyA->m_jointList = &j->m_edgeA;
18915
18916 j->m_edgeB.joint = j;
18917 j->m_edgeB.other = j->m_bodyA;
18918 j->m_edgeB.prev = nullptr;
18919 j->m_edgeB.next = j->m_bodyB->m_jointList;
18920 if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
18921 j->m_bodyB->m_jointList = &j->m_edgeB;
18922
18923 b2Body* bodyA = def->bodyA;
18924 b2Body* bodyB = def->bodyB;
18925
18926 // If the joint prevents collisions, then flag any contacts for filtering.
18927 if (def->collideConnected == false) {
18928 b2ContactEdge* edge = bodyB->GetContactList();
18929 while (edge) {
18930 if (edge->other == bodyA) {
18931 // Flag the contact for filtering at the next time step (where either
18932 // body is awake).
18933 edge->contact->FlagForFiltering();
18934 }
18935
18936 edge = edge->next;
18937 }
18938 }
18939
18940 // Note: creating a joint doesn't wake the bodies.
18941
18942 return j;
18943}
18944
18945void b2World::DestroyJoint(b2Joint* j) {
18946 b2Assert(IsLocked() == false);
18947 if (IsLocked()) {
18948 return;
18949 }
18950
18951 bool collideConnected = j->m_collideConnected;
18952
18953 // Remove from the doubly linked list.
18954 if (j->m_prev) {
18955 j->m_prev->m_next = j->m_next;
18956 }
18957
18958 if (j->m_next) {
18959 j->m_next->m_prev = j->m_prev;
18960 }
18961
18962 if (j == m_jointList) {
18963 m_jointList = j->m_next;
18964 }
18965
18966 // Disconnect from island graph.
18967 b2Body* bodyA = j->m_bodyA;
18968 b2Body* bodyB = j->m_bodyB;
18969
18970 // Wake up connected bodies.
18971 bodyA->SetAwake(true);
18972 bodyB->SetAwake(true);
18973
18974 // Remove from body 1.
18975 if (j->m_edgeA.prev) {
18976 j->m_edgeA.prev->next = j->m_edgeA.next;
18977 }
18978
18979 if (j->m_edgeA.next) {
18980 j->m_edgeA.next->prev = j->m_edgeA.prev;
18981 }
18982
18983 if (&j->m_edgeA == bodyA->m_jointList) {
18984 bodyA->m_jointList = j->m_edgeA.next;
18985 }
18986
18987 j->m_edgeA.prev = nullptr;
18988 j->m_edgeA.next = nullptr;
18989
18990 // Remove from body 2
18991 if (j->m_edgeB.prev) {
18992 j->m_edgeB.prev->next = j->m_edgeB.next;
18993 }
18994
18995 if (j->m_edgeB.next) {
18996 j->m_edgeB.next->prev = j->m_edgeB.prev;
18997 }
18998
18999 if (&j->m_edgeB == bodyB->m_jointList) {
19000 bodyB->m_jointList = j->m_edgeB.next;
19001 }
19002
19003 j->m_edgeB.prev = nullptr;
19004 j->m_edgeB.next = nullptr;
19005
19006 b2Joint::Destroy(j, &m_blockAllocator);
19007
19008 b2Assert(m_jointCount > 0);
19009 --m_jointCount;
19010
19011 // If the joint prevents collisions, then flag any contacts for filtering.
19012 if (collideConnected == false) {
19013 b2ContactEdge* edge = bodyB->GetContactList();
19014 while (edge) {
19015 if (edge->other == bodyA) {
19016 // Flag the contact for filtering at the next time step (where either
19017 // body is awake).
19018 edge->contact->FlagForFiltering();
19019 }
19020
19021 edge = edge->next;
19022 }
19023 }
19024}
19025
19026//
19027void b2World::SetAllowSleeping(bool flag) {
19028 if (flag == m_allowSleep) {
19029 return;
19030 }
19031
19032 m_allowSleep = flag;
19033 if (m_allowSleep == false) {
19034 for (b2Body* b = m_bodyList; b; b = b->m_next) {
19035 b->SetAwake(true);
19036 }
19037 }
19038}
19039
19040// Find islands, integrate and solve constraints, solve position constraints
19041void b2World::Solve(const b2TimeStep& step) {
19042 m_profile.solveInit = 0.0f;
19043 m_profile.solveVelocity = 0.0f;
19044 m_profile.solvePosition = 0.0f;
19045
19046 // Size the island for the worst case.
19047 b2Island island(m_bodyCount,
19048 m_contactManager.m_contactCount,
19049 m_jointCount,
19050 &m_stackAllocator,
19051 m_contactManager.m_contactListener);
19052
19053 // Clear all the island flags.
19054 for (b2Body* b = m_bodyList; b; b = b->m_next) {
19055 b->m_flags &= ~b2Body::e_islandFlag;
19056 }
19057 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) {
19058 c->m_flags &= ~b2Contact::e_islandFlag;
19059 }
19060 for (b2Joint* j = m_jointList; j; j = j->m_next) {
19061 j->m_islandFlag = false;
19062 }
19063
19064 // Build and simulate all awake islands.
19065 int32 stackSize = m_bodyCount;
19066 b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
19067 for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) {
19068 if (seed->m_flags & b2Body::e_islandFlag) {
19069 continue;
19070 }
19071
19072 if (seed->IsAwake() == false || seed->IsEnabled() == false) {
19073 continue;
19074 }
19075
19076 // The seed can be dynamic or kinematic.
19077 if (seed->GetType() == b2_staticBody) {
19078 continue;
19079 }
19080
19081 // Reset island and stack.
19082 island.Clear();
19083 int32 stackCount = 0;
19084 stack[stackCount++] = seed;
19085 seed->m_flags |= b2Body::e_islandFlag;
19086
19087 // Perform a depth first search (DFS) on the constraint graph.
19088 while (stackCount > 0) {
19089 // Grab the next body off the stack and add it to the island.
19090 b2Body* b = stack[--stackCount];
19091 b2Assert(b->IsEnabled() == true);
19092 island.Add(b);
19093
19094 // To keep islands as small as possible, we don't
19095 // propagate islands across static bodies.
19096 if (b->GetType() == b2_staticBody) {
19097 continue;
19098 }
19099
19100 // Make sure the body is awake (without resetting sleep timer).
19101 b->m_flags |= b2Body::e_awakeFlag;
19102
19103 // Search all contacts connected to this body.
19104 for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) {
19105 b2Contact* contact = ce->contact;
19106
19107 // Has this contact already been added to an island?
19108 if (contact->m_flags & b2Contact::e_islandFlag) {
19109 continue;
19110 }
19111
19112 // Is this contact solid and touching?
19113 if (contact->IsEnabled() == false ||
19114 contact->IsTouching() == false) {
19115 continue;
19116 }
19117
19118 // Skip sensors.
19119 bool sensorA = contact->m_fixtureA->m_isSensor;
19120 bool sensorB = contact->m_fixtureB->m_isSensor;
19121 if (sensorA || sensorB) {
19122 continue;
19123 }
19124
19125 island.Add(contact);
19126 contact->m_flags |= b2Contact::e_islandFlag;
19127
19128 b2Body* other = ce->other;
19129
19130 // Was the other body already added to this island?
19131 if (other->m_flags & b2Body::e_islandFlag) {
19132 continue;
19133 }
19134
19135 b2Assert(stackCount < stackSize);
19136 stack[stackCount++] = other;
19137 other->m_flags |= b2Body::e_islandFlag;
19138 }
19139
19140 // Search all joints connect to this body.
19141 for (b2JointEdge* je = b->m_jointList; je; je = je->next) {
19142 if (je->joint->m_islandFlag == true) {
19143 continue;
19144 }
19145
19146 b2Body* other = je->other;
19147
19148 // Don't simulate joints connected to diabled bodies.
19149 if (other->IsEnabled() == false) {
19150 continue;
19151 }
19152
19153 island.Add(je->joint);
19154 je->joint->m_islandFlag = true;
19155
19156 if (other->m_flags & b2Body::e_islandFlag) {
19157 continue;
19158 }
19159
19160 b2Assert(stackCount < stackSize);
19161 stack[stackCount++] = other;
19162 other->m_flags |= b2Body::e_islandFlag;
19163 }
19164 }
19165
19166 b2Profile profile;
19167 island.Solve(&profile, step, m_gravity, m_allowSleep);
19168 m_profile.solveInit += profile.solveInit;
19169 m_profile.solveVelocity += profile.solveVelocity;
19170 m_profile.solvePosition += profile.solvePosition;
19171
19172 // Post solve cleanup.
19173 for (int32 i = 0; i < island.m_bodyCount; ++i) {
19174 // Allow static bodies to participate in other islands.
19175 b2Body* b = island.m_bodies[i];
19176 if (b->GetType() == b2_staticBody) {
19177 b->m_flags &= ~b2Body::e_islandFlag;
19178 }
19179 }
19180 }
19181
19182 m_stackAllocator.Free(stack);
19183
19184 {
19185 b2Timer timer;
19186 // Synchronize fixtures, check for out of range bodies.
19187 for (b2Body* b = m_bodyList; b; b = b->GetNext()) {
19188 // If a body was not in an island then it did not move.
19189 if ((b->m_flags & b2Body::e_islandFlag) == 0) {
19190 continue;
19191 }
19192
19193 if (b->GetType() == b2_staticBody) {
19194 continue;
19195 }
19196
19197 // Update fixtures (for broad-phase).
19198 b->SynchronizeFixtures();
19199 }
19200
19201 // Look for new contacts.
19202 m_contactManager.FindNewContacts();
19203 m_profile.broadphase = timer.GetMilliseconds();
19204 }
19205}
19206
19207// Find TOI contacts and solve them.
19208void b2World::SolveTOI(const b2TimeStep& step) {
19209 b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
19210
19211 if (m_stepComplete) {
19212 for (b2Body* b = m_bodyList; b; b = b->m_next) {
19213 b->m_flags &= ~b2Body::e_islandFlag;
19214 b->m_sweep.alpha0 = 0.0f;
19215 }
19216
19217 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) {
19218 // Invalidate TOI
19219 c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
19220 c->m_toiCount = 0;
19221 c->m_toi = 1.0f;
19222 }
19223 }
19224
19225 // Find TOI events and solve them.
19226 for (;;) {
19227 // Find the first TOI.
19228 b2Contact* minContact = nullptr;
19229 float minAlpha = 1.0f;
19230
19231 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) {
19232 // Is this contact disabled?
19233 if (c->IsEnabled() == false) {
19234 continue;
19235 }
19236
19237 // Prevent excessive sub-stepping.
19238 if (c->m_toiCount > b2_maxSubSteps) {
19239 continue;
19240 }
19241
19242 float alpha = 1.0f;
19243 if (c->m_flags & b2Contact::e_toiFlag) {
19244 // This contact has a valid cached TOI.
19245 alpha = c->m_toi;
19246 } else {
19247 b2Fixture* fA = c->GetFixtureA();
19248 b2Fixture* fB = c->GetFixtureB();
19249
19250 // Is there a sensor?
19251 if (fA->IsSensor() || fB->IsSensor()) {
19252 continue;
19253 }
19254
19255 b2Body* bA = fA->GetBody();
19256 b2Body* bB = fB->GetBody();
19257
19258 b2BodyType typeA = bA->m_type;
19259 b2BodyType typeB = bB->m_type;
19260 b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
19261
19262 bool activeA = bA->IsAwake() && typeA != b2_staticBody;
19263 bool activeB = bB->IsAwake() && typeB != b2_staticBody;
19264
19265 // Is at least one body active (awake and dynamic or kinematic)?
19266 if (activeA == false && activeB == false) {
19267 continue;
19268 }
19269
19270 bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
19271 bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
19272
19273 // Are these two non-bullet dynamic bodies?
19274 if (collideA == false && collideB == false) {
19275 continue;
19276 }
19277
19278 // Compute the TOI for this contact.
19279 // Put the sweeps onto the same time interval.
19280 float alpha0 = bA->m_sweep.alpha0;
19281
19282 if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) {
19283 alpha0 = bB->m_sweep.alpha0;
19284 bA->m_sweep.Advance(alpha0);
19285 } else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) {
19286 alpha0 = bA->m_sweep.alpha0;
19287 bB->m_sweep.Advance(alpha0);
19288 }
19289
19290 b2Assert(alpha0 < 1.0f);
19291
19292 int32 indexA = c->GetChildIndexA();
19293 int32 indexB = c->GetChildIndexB();
19294
19295 // Compute the time of impact in interval [0, minTOI]
19296 b2TOIInput input;
19297 input.proxyA.Set(fA->GetShape(), indexA);
19298 input.proxyB.Set(fB->GetShape(), indexB);
19299 input.sweepA = bA->m_sweep;
19300 input.sweepB = bB->m_sweep;
19301 input.tMax = 1.0f;
19302
19303 b2TOIOutput output;
19304 b2TimeOfImpact(&output, &input);
19305
19306 // Beta is the fraction of the remaining portion of the .
19307 float beta = output.t;
19308 if (output.state == b2TOIOutput::e_touching) {
19309 alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
19310 } else {
19311 alpha = 1.0f;
19312 }
19313
19314 c->m_toi = alpha;
19315 c->m_flags |= b2Contact::e_toiFlag;
19316 }
19317
19318 if (alpha < minAlpha) {
19319 // This is the minimum TOI found so far.
19320 minContact = c;
19321 minAlpha = alpha;
19322 }
19323 }
19324
19325 if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha) {
19326 // No more TOI events. Done!
19327 m_stepComplete = true;
19328 break;
19329 }
19330
19331 // Advance the bodies to the TOI.
19332 b2Fixture* fA = minContact->GetFixtureA();
19333 b2Fixture* fB = minContact->GetFixtureB();
19334 b2Body* bA = fA->GetBody();
19335 b2Body* bB = fB->GetBody();
19336
19337 b2Sweep backup1 = bA->m_sweep;
19338 b2Sweep backup2 = bB->m_sweep;
19339
19340 bA->Advance(minAlpha);
19341 bB->Advance(minAlpha);
19342
19343 // The TOI contact likely has some new contact points.
19344 minContact->Update(m_contactManager.m_contactListener);
19345 minContact->m_flags &= ~b2Contact::e_toiFlag;
19346 ++minContact->m_toiCount;
19347
19348 // Is the contact solid?
19349 if (minContact->IsEnabled() == false || minContact->IsTouching() == false) {
19350 // Restore the sweeps.
19351 minContact->SetEnabled(false);
19352 bA->m_sweep = backup1;
19353 bB->m_sweep = backup2;
19354 bA->SynchronizeTransform();
19355 bB->SynchronizeTransform();
19356 continue;
19357 }
19358
19359 bA->SetAwake(true);
19360 bB->SetAwake(true);
19361
19362 // Build the island
19363 island.Clear();
19364 island.Add(bA);
19365 island.Add(bB);
19366 island.Add(minContact);
19367
19368 bA->m_flags |= b2Body::e_islandFlag;
19369 bB->m_flags |= b2Body::e_islandFlag;
19370 minContact->m_flags |= b2Contact::e_islandFlag;
19371
19372 // Get contacts on bodyA and bodyB.
19373 b2Body* bodies[2] = { bA, bB };
19374 for (int32 i = 0; i < 2; ++i) {
19375 b2Body* body = bodies[i];
19376 if (body->m_type == b2_dynamicBody) {
19377 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) {
19378 if (island.m_bodyCount == island.m_bodyCapacity) {
19379 break;
19380 }
19381
19382 if (island.m_contactCount == island.m_contactCapacity) {
19383 break;
19384 }
19385
19386 b2Contact* contact = ce->contact;
19387
19388 // Has this contact already been added to the island?
19389 if (contact->m_flags & b2Contact::e_islandFlag) {
19390 continue;
19391 }
19392
19393 // Only add static, kinematic, or bullet bodies.
19394 b2Body* other = ce->other;
19395 if (other->m_type == b2_dynamicBody &&
19396 body->IsBullet() == false && other->IsBullet() == false) {
19397 continue;
19398 }
19399
19400 // Skip sensors.
19401 bool sensorA = contact->m_fixtureA->m_isSensor;
19402 bool sensorB = contact->m_fixtureB->m_isSensor;
19403 if (sensorA || sensorB) {
19404 continue;
19405 }
19406
19407 // Tentatively advance the body to the TOI.
19408 b2Sweep backup = other->m_sweep;
19409 if ((other->m_flags & b2Body::e_islandFlag) == 0) {
19410 other->Advance(minAlpha);
19411 }
19412
19413 // Update the contact points
19414 contact->Update(m_contactManager.m_contactListener);
19415
19416 // Was the contact disabled by the user?
19417 if (contact->IsEnabled() == false) {
19418 other->m_sweep = backup;
19419 other->SynchronizeTransform();
19420 continue;
19421 }
19422
19423 // Are there contact points?
19424 if (contact->IsTouching() == false) {
19425 other->m_sweep = backup;
19426 other->SynchronizeTransform();
19427 continue;
19428 }
19429
19430 // Add the contact to the island
19431 contact->m_flags |= b2Contact::e_islandFlag;
19432 island.Add(contact);
19433
19434 // Has the other body already been added to the island?
19435 if (other->m_flags & b2Body::e_islandFlag) {
19436 continue;
19437 }
19438
19439 // Add the other body to the island.
19440 other->m_flags |= b2Body::e_islandFlag;
19441
19442 if (other->m_type != b2_staticBody) {
19443 other->SetAwake(true);
19444 }
19445
19446 island.Add(other);
19447 }
19448 }
19449 }
19450
19451 b2TimeStep subStep;
19452 subStep.dt = (1.0f - minAlpha) * step.dt;
19453 subStep.inv_dt = 1.0f / subStep.dt;
19454 subStep.dtRatio = 1.0f;
19455 subStep.positionIterations = 20;
19456 subStep.velocityIterations = step.velocityIterations;
19457 subStep.warmStarting = false;
19458 island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
19459
19460 // Reset island flags and synchronize broad-phase proxies.
19461 for (int32 i = 0; i < island.m_bodyCount; ++i) {
19462 b2Body* body = island.m_bodies[i];
19463 body->m_flags &= ~b2Body::e_islandFlag;
19464
19465 if (body->m_type != b2_dynamicBody) {
19466 continue;
19467 }
19468
19469 body->SynchronizeFixtures();
19470
19471 // Invalidate all contact TOIs on this displaced body.
19472 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) {
19473 ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
19474 }
19475 }
19476
19477 // Commit fixture proxy movements to the broad-phase so that new contacts are created.
19478 // Also, some contacts can be destroyed.
19479 m_contactManager.FindNewContacts();
19480
19481 if (m_subStepping) {
19482 m_stepComplete = false;
19483 break;
19484 }
19485 }
19486}
19487
19488void b2World::Step(float dt, int32 velocityIterations, int32 positionIterations) {
19489 b2Timer stepTimer;
19490
19491 // If new fixtures were added, we need to find the new contacts.
19492 if (m_newContacts) {
19493 m_contactManager.FindNewContacts();
19494 m_newContacts = false;
19495 }
19496
19497 m_locked = true;
19498
19499 b2TimeStep step;
19500 step.dt = dt;
19501 step.velocityIterations = velocityIterations;
19502 step.positionIterations = positionIterations;
19503 if (dt > 0.0f) {
19504 step.inv_dt = 1.0f / dt;
19505 } else {
19506 step.inv_dt = 0.0f;
19507 }
19508
19509 step.dtRatio = m_inv_dt0 * dt;
19510
19511 step.warmStarting = m_warmStarting;
19512
19513 // Update contacts. This is where some contacts are destroyed.
19514 {
19515 b2Timer timer;
19516 m_contactManager.Collide();
19517 m_profile.collide = timer.GetMilliseconds();
19518 }
19519
19520 // Integrate velocities, solve velocity constraints, and integrate positions.
19521 if (m_stepComplete && step.dt > 0.0f) {
19522 b2Timer timer;
19523 Solve(step);
19524 m_profile.solve = timer.GetMilliseconds();
19525 }
19526
19527 // Handle TOI events.
19528 if (m_continuousPhysics && step.dt > 0.0f) {
19529 b2Timer timer;
19530 SolveTOI(step);
19531 m_profile.solveTOI = timer.GetMilliseconds();
19532 }
19533
19534 if (step.dt > 0.0f) {
19535 m_inv_dt0 = step.inv_dt;
19536 }
19537
19538 if (m_clearForces) {
19539 ClearForces();
19540 }
19541
19542 m_locked = false;
19543
19544 m_profile.step = stepTimer.GetMilliseconds();
19545}
19546
19547void b2World::ClearForces() {
19548 for (b2Body* body = m_bodyList; body; body = body->GetNext()) {
19549 body->m_force.SetZero();
19550 body->m_torque = 0.0f;
19551 }
19552}
19553
19554struct b2WorldQueryWrapper {
19555 bool QueryCallback(int32 proxyId) {
19556 b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
19557 return callback->ReportFixture(proxy->fixture);
19558 }
19559
19560 const b2BroadPhase* broadPhase;
19561 b2QueryCallback* callback;
19562};
19563
19564void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const {
19565 b2WorldQueryWrapper wrapper;
19566 wrapper.broadPhase = &m_contactManager.m_broadPhase;
19567 wrapper.callback = callback;
19568 m_contactManager.m_broadPhase.Query(&wrapper, aabb);
19569}
19570
19571struct b2WorldRayCastWrapper {
19572 float RayCastCallback(const b2RayCastInput& input, int32 proxyId) {
19573 void* userData = broadPhase->GetUserData(proxyId);
19574 b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
19575 b2Fixture* fixture = proxy->fixture;
19576 int32 index = proxy->childIndex;
19577 b2RayCastOutput output;
19578 bool hit = fixture->RayCast(&output, input, index);
19579
19580 if (hit) {
19581 float fraction = output.fraction;
19582 b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
19583 return callback->ReportFixture(fixture, point, output.normal, fraction);
19584 }
19585
19586 return input.maxFraction;
19587 }
19588
19589 const b2BroadPhase* broadPhase;
19590 b2RayCastCallback* callback;
19591};
19592
19593void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const {
19594 b2WorldRayCastWrapper wrapper;
19595 wrapper.broadPhase = &m_contactManager.m_broadPhase;
19596 wrapper.callback = callback;
19597 b2RayCastInput input;
19598 input.maxFraction = 1.0f;
19599 input.p1 = point1;
19600 input.p2 = point2;
19601 m_contactManager.m_broadPhase.RayCast(&wrapper, input);
19602}
19603
19604void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) {
19605 switch (fixture->GetType()) {
19606 case b2Shape::e_circle:
19607 {
19608 b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
19609
19610 b2Vec2 center = b2Mul(xf, circle->m_p);
19611 float radius = circle->m_radius;
19612 b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
19613
19614 m_debugDraw->DrawSolidCircle(center, radius, axis, color);
19615 }
19616 break;
19617
19618 case b2Shape::e_edge:
19619 {
19620 b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
19621 b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
19622 b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
19623 m_debugDraw->DrawSegment(v1, v2, color);
19624
19625 if (edge->m_oneSided == false) {
19626 m_debugDraw->DrawPoint(v1, 4.0f, color);
19627 m_debugDraw->DrawPoint(v2, 4.0f, color);
19628 }
19629 }
19630 break;
19631
19632 case b2Shape::e_chain:
19633 {
19634 b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
19635 int32 count = chain->m_count;
19636 const b2Vec2* vertices = chain->m_vertices;
19637
19638 b2Vec2 v1 = b2Mul(xf, vertices[0]);
19639 for (int32 i = 1; i < count; ++i) {
19640 b2Vec2 v2 = b2Mul(xf, vertices[i]);
19641 m_debugDraw->DrawSegment(v1, v2, color);
19642 v1 = v2;
19643 }
19644 }
19645 break;
19646
19647 case b2Shape::e_polygon:
19648 {
19649 b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
19650 int32 vertexCount = poly->m_count;
19651 b2Assert(vertexCount <= b2_maxPolygonVertices);
19652 b2Vec2 vertices[b2_maxPolygonVertices];
19653
19654 for (int32 i = 0; i < vertexCount; ++i) {
19655 vertices[i] = b2Mul(xf, poly->m_vertices[i]);
19656 }
19657
19658 m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
19659 }
19660 break;
19661
19662 default:
19663 break;
19664 }
19665}
19666
19667void b2World::DebugDraw() {
19668 if (m_debugDraw == nullptr) {
19669 return;
19670 }
19671
19672 uint32 flags = m_debugDraw->GetFlags();
19673
19674 if (flags & b2Draw::e_shapeBit) {
19675 for (b2Body* b = m_bodyList; b; b = b->GetNext()) {
19676 const b2Transform& xf = b->GetTransform();
19677 for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) {
19678 if (b->GetType() == b2_dynamicBody && b->m_mass == 0.0f) {
19679 // Bad body
19680 DrawShape(f, xf, b2Color(1.0f, 0.0f, 0.0f));
19681 } else if (b->IsEnabled() == false) {
19682 DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
19683 } else if (b->GetType() == b2_staticBody) {
19684 DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
19685 } else if (b->GetType() == b2_kinematicBody) {
19686 DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
19687 } else if (b->IsAwake() == false) {
19688 DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));
19689 } else {
19690 DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));
19691 }
19692 }
19693 }
19694 }
19695
19696 if (flags & b2Draw::e_jointBit) {
19697 for (b2Joint* j = m_jointList; j; j = j->GetNext()) {
19698 j->Draw(m_debugDraw);
19699 }
19700 }
19701
19702 if (flags & b2Draw::e_pairBit) {
19703 b2Color color(0.3f, 0.9f, 0.9f);
19704 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) {
19705 b2Fixture* fixtureA = c->GetFixtureA();
19706 b2Fixture* fixtureB = c->GetFixtureB();
19707 int32 indexA = c->GetChildIndexA();
19708 int32 indexB = c->GetChildIndexB();
19709 b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter();
19710 b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter();
19711
19712 m_debugDraw->DrawSegment(cA, cB, color);
19713 }
19714 }
19715
19716 if (flags & b2Draw::e_aabbBit) {
19717 b2Color color(0.9f, 0.3f, 0.9f);
19718 b2BroadPhase* bp = &m_contactManager.m_broadPhase;
19719
19720 for (b2Body* b = m_bodyList; b; b = b->GetNext()) {
19721 if (b->IsEnabled() == false) {
19722 continue;
19723 }
19724
19725 for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) {
19726 for (int32 i = 0; i < f->m_proxyCount; ++i) {
19727 b2FixtureProxy* proxy = f->m_proxies + i;
19728 b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
19729 b2Vec2 vs[4];
19730 vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
19731 vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
19732 vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
19733 vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
19734
19735 m_debugDraw->DrawPolygon(vs, 4, color);
19736 }
19737 }
19738 }
19739 }
19740
19741 if (flags & b2Draw::e_centerOfMassBit) {
19742 for (b2Body* b = m_bodyList; b; b = b->GetNext()) {
19743 b2Transform xf = b->GetTransform();
19744 xf.p = b->GetWorldCenter();
19745 m_debugDraw->DrawTransform(xf);
19746 }
19747 }
19748}
19749
19750int32 b2World::GetProxyCount() const {
19751 return m_contactManager.m_broadPhase.GetProxyCount();
19752}
19753
19754int32 b2World::GetTreeHeight() const {
19755 return m_contactManager.m_broadPhase.GetTreeHeight();
19756}
19757
19758int32 b2World::GetTreeBalance() const {
19759 return m_contactManager.m_broadPhase.GetTreeBalance();
19760}
19761
19762float b2World::GetTreeQuality() const {
19763 return m_contactManager.m_broadPhase.GetTreeQuality();
19764}
19765
19766void b2World::ShiftOrigin(const b2Vec2& newOrigin) {
19767 b2Assert(m_locked == false);
19768 if (m_locked) {
19769 return;
19770 }
19771
19772 for (b2Body* b = m_bodyList; b; b = b->m_next) {
19773 b->m_xf.p -= newOrigin;
19774 b->m_sweep.c0 -= newOrigin;
19775 b->m_sweep.c -= newOrigin;
19776 }
19777
19778 for (b2Joint* j = m_jointList; j; j = j->m_next) {
19779 j->ShiftOrigin(newOrigin);
19780 }
19781
19782 m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
19783}
19784
19785void b2World::Dump() {
19786 if (m_locked) {
19787 return;
19788 }
19789
19790 b2OpenDump("box2d_dump.inl");
19791
19792 b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y);
19793 b2Dump("m_world->SetGravity(g);\n");
19794
19795 b2Dump("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
19796 b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
19797
19798 int32 i = 0;
19799 for (b2Body* b = m_bodyList; b; b = b->m_next) {
19800 b->m_islandIndex = i;
19801 b->Dump();
19802 ++i;
19803 }
19804
19805 i = 0;
19806 for (b2Joint* j = m_jointList; j; j = j->m_next) {
19807 j->m_index = i;
19808 ++i;
19809 }
19810
19811 // First pass on joints, skip gear joints.
19812 for (b2Joint* j = m_jointList; j; j = j->m_next) {
19813 if (j->m_type == e_gearJoint) {
19814 continue;
19815 }
19816
19817 b2Dump("{\n");
19818 j->Dump();
19819 b2Dump("}\n");
19820 }
19821
19822 // Second pass on joints, only gear joints.
19823 for (b2Joint* j = m_jointList; j; j = j->m_next) {
19824 if (j->m_type != e_gearJoint) {
19825 continue;
19826 }
19827
19828 b2Dump("{\n");
19829 j->Dump();
19830 b2Dump("}\n");
19831 }
19832
19833 b2Dump("b2Free(joints);\n");
19834 b2Dump("b2Free(bodies);\n");
19835 b2Dump("joints = nullptr;\n");
19836 b2Dump("bodies = nullptr;\n");
19837
19838 b2CloseDump();
19839}
19840// MIT License
19841
19842// Copyright (c) 2019 Erin Catto
19843
19844// Permission is hereby granted, free of charge, to any person obtaining a copy
19845// of this software and associated documentation files (the "Software"), to deal
19846// in the Software without restriction, including without limitation the rights
19847// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19848// copies of the Software, and to permit persons to whom the Software is
19849// furnished to do so, subject to the following conditions:
19850
19851// The above copyright notice and this permission notice shall be included in all
19852// copies or substantial portions of the Software.
19853
19854// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19855// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19856// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19857// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19858// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19859// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19860// SOFTWARE.
19861
19862#include "box2d/b2_fixture.h"
19863#include "box2d/b2_world_callbacks.h"
19864
19865// Return true if contact calculations should be performed between these two shapes.
19866// If you implement your own collision filter you may want to build from this implementation.
19867bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) {
19868 const b2Filter& filterA = fixtureA->GetFilterData();
19869 const b2Filter& filterB = fixtureB->GetFilterData();
19870
19871 if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) {
19872 return filterA.groupIndex > 0;
19873 }
19874
19875 bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
19876 return collide;
19877}
19878// MIT License
19879
19880// Copyright (c) 2019 Erin Catto
19881
19882// Permission is hereby granted, free of charge, to any person obtaining a copy
19883// of this software and associated documentation files (the "Software"), to deal
19884// in the Software without restriction, including without limitation the rights
19885// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19886// copies of the Software, and to permit persons to whom the Software is
19887// furnished to do so, subject to the following conditions:
19888
19889// The above copyright notice and this permission notice shall be included in all
19890// copies or substantial portions of the Software.
19891
19892// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19893// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19894// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19895// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19896// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19897// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19898// SOFTWARE.
19899
19900#include "box2d/b2_draw.h"
19901#include "box2d/b2_rope.h"
19902
19903#include <stdio.h>
19904
19905struct b2RopeStretch {
19906 int32 i1, i2;
19907 float invMass1, invMass2;
19908 float L;
19909 float lambda;
19910 float spring;
19911 float damper;
19912};
19913
19914struct b2RopeBend {
19915 int32 i1, i2, i3;
19916 float invMass1, invMass2, invMass3;
19917 float invEffectiveMass;
19918 float lambda;
19919 float L1, L2;
19920 float alpha1, alpha2;
19921 float spring;
19922 float damper;
19923};
19924
19925b2Rope::b2Rope() {
19926 m_position.SetZero();
19927 m_count = 0;
19928 m_stretchCount = 0;
19929 m_bendCount = 0;
19930 m_stretchConstraints = nullptr;
19931 m_bendConstraints = nullptr;
19932 m_bindPositions = nullptr;
19933 m_ps = nullptr;
19934 m_p0s = nullptr;
19935 m_vs = nullptr;
19936 m_invMasses = nullptr;
19937 m_gravity.SetZero();
19938}
19939
19940b2Rope::~b2Rope() {
19941 b2Free(m_stretchConstraints);
19942 b2Free(m_bendConstraints);
19943 b2Free(m_bindPositions);
19944 b2Free(m_ps);
19945 b2Free(m_p0s);
19946 b2Free(m_vs);
19947 b2Free(m_invMasses);
19948}
19949
19950void b2Rope::Create(const b2RopeDef& def) {
19951 b2Assert(def.count >= 3);
19952 m_position = def.position;
19953 m_count = def.count;
19954 m_bindPositions = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
19955 m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
19956 m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
19957 m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
19958 m_invMasses = (float*)b2Alloc(m_count * sizeof(float));
19959
19960 for (int32 i = 0; i < m_count; ++i) {
19961 m_bindPositions[i] = def.vertices[i];
19962 m_ps[i] = def.vertices[i] + m_position;
19963 m_p0s[i] = def.vertices[i] + m_position;
19964 m_vs[i].SetZero();
19965
19966 float m = def.masses[i];
19967 if (m > 0.0f) {
19968 m_invMasses[i] = 1.0f / m;
19969 } else {
19970 m_invMasses[i] = 0.0f;
19971 }
19972 }
19973
19974 m_stretchCount = m_count - 1;
19975 m_bendCount = m_count - 2;
19976
19977 m_stretchConstraints = (b2RopeStretch*)b2Alloc(m_stretchCount * sizeof(b2RopeStretch));
19978 m_bendConstraints = (b2RopeBend*)b2Alloc(m_bendCount * sizeof(b2RopeBend));
19979
19980 for (int32 i = 0; i < m_stretchCount; ++i) {
19981 b2RopeStretch& c = m_stretchConstraints[i];
19982
19983 b2Vec2 p1 = m_ps[i];
19984 b2Vec2 p2 = m_ps[i + 1];
19985
19986 c.i1 = i;
19987 c.i2 = i + 1;
19988 c.L = b2Distance(p1, p2);
19989 c.invMass1 = m_invMasses[i];
19990 c.invMass2 = m_invMasses[i + 1];
19991 c.lambda = 0.0f;
19992 c.damper = 0.0f;
19993 c.spring = 0.0f;
19994 }
19995
19996 for (int32 i = 0; i < m_bendCount; ++i) {
19997 b2RopeBend& c = m_bendConstraints[i];
19998
19999 b2Vec2 p1 = m_ps[i];
20000 b2Vec2 p2 = m_ps[i + 1];
20001 b2Vec2 p3 = m_ps[i + 2];
20002
20003 c.i1 = i;
20004 c.i2 = i + 1;
20005 c.i3 = i + 2;
20006 c.invMass1 = m_invMasses[i];
20007 c.invMass2 = m_invMasses[i + 1];
20008 c.invMass3 = m_invMasses[i + 2];
20009 c.invEffectiveMass = 0.0f;
20010 c.L1 = b2Distance(p1, p2);
20011 c.L2 = b2Distance(p2, p3);
20012 c.lambda = 0.0f;
20013
20014 // Pre-compute effective mass (TODO use flattened config)
20015 b2Vec2 e1 = p2 - p1;
20016 b2Vec2 e2 = p3 - p2;
20017 float L1sqr = e1.LengthSquared();
20018 float L2sqr = e2.LengthSquared();
20019
20020 if (L1sqr * L2sqr == 0.0f) {
20021 continue;
20022 }
20023
20024 b2Vec2 Jd1 = (-1.0f / L1sqr) * e1.Skew();
20025 b2Vec2 Jd2 = (1.0f / L2sqr) * e2.Skew();
20026
20027 b2Vec2 J1 = -Jd1;
20028 b2Vec2 J2 = Jd1 - Jd2;
20029 b2Vec2 J3 = Jd2;
20030
20031 c.invEffectiveMass = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
20032
20033 b2Vec2 r = p3 - p1;
20034
20035 float rr = r.LengthSquared();
20036 if (rr == 0.0f) {
20037 continue;
20038 }
20039
20040 // a1 = h2 / (h1 + h2)
20041 // a2 = h1 / (h1 + h2)
20042 c.alpha1 = b2Dot(e2, r) / rr;
20043 c.alpha2 = b2Dot(e1, r) / rr;
20044 }
20045
20046 m_gravity = def.gravity;
20047
20048 SetTuning(def.tuning);
20049}
20050
20051void b2Rope::SetTuning(const b2RopeTuning& tuning) {
20052 m_tuning = tuning;
20053
20054 // Pre-compute spring and damper values based on tuning
20055
20056 const float bendOmega = 2.0f * b2_pi * m_tuning.bendHertz;
20057
20058 for (int32 i = 0; i < m_bendCount; ++i) {
20059 b2RopeBend& c = m_bendConstraints[i];
20060
20061 float L1sqr = c.L1 * c.L1;
20062 float L2sqr = c.L2 * c.L2;
20063
20064 if (L1sqr * L2sqr == 0.0f) {
20065 c.spring = 0.0f;
20066 c.damper = 0.0f;
20067 continue;
20068 }
20069
20070 // Flatten the triangle formed by the two edges
20071 float J2 = 1.0f / c.L1 + 1.0f / c.L2;
20072 float sum = c.invMass1 / L1sqr + c.invMass2 * J2 * J2 + c.invMass3 / L2sqr;
20073 if (sum == 0.0f) {
20074 c.spring = 0.0f;
20075 c.damper = 0.0f;
20076 continue;
20077 }
20078
20079 float mass = 1.0f / sum;
20080
20081 c.spring = mass * bendOmega * bendOmega;
20082 c.damper = 2.0f * mass * m_tuning.bendDamping * bendOmega;
20083 }
20084
20085 const float stretchOmega = 2.0f * b2_pi * m_tuning.stretchHertz;
20086
20087 for (int32 i = 0; i < m_stretchCount; ++i) {
20088 b2RopeStretch& c = m_stretchConstraints[i];
20089
20090 float sum = c.invMass1 + c.invMass2;
20091 if (sum == 0.0f) {
20092 continue;
20093 }
20094
20095 float mass = 1.0f / sum;
20096
20097 c.spring = mass * stretchOmega * stretchOmega;
20098 c.damper = 2.0f * mass * m_tuning.stretchDamping * stretchOmega;
20099 }
20100}
20101
20102void b2Rope::Step(float dt, int32 iterations, const b2Vec2& position) {
20103 if (dt == 0.0) {
20104 return;
20105 }
20106
20107 const float inv_dt = 1.0f / dt;
20108 float d = expf(-dt * m_tuning.damping);
20109
20110 // Apply gravity and damping
20111 for (int32 i = 0; i < m_count; ++i) {
20112 if (m_invMasses[i] > 0.0f) {
20113 m_vs[i] *= d;
20114 m_vs[i] += dt * m_gravity;
20115 } else {
20116 m_vs[i] = inv_dt * (m_bindPositions[i] + position - m_p0s[i]);
20117 }
20118 }
20119
20120 // Apply bending spring
20121 if (m_tuning.bendingModel == b2_springAngleBendingModel) {
20122 ApplyBendForces(dt);
20123 }
20124
20125 for (int32 i = 0; i < m_bendCount; ++i) {
20126 m_bendConstraints[i].lambda = 0.0f;
20127 }
20128
20129 for (int32 i = 0; i < m_stretchCount; ++i) {
20130 m_stretchConstraints[i].lambda = 0.0f;
20131 }
20132
20133 // Update position
20134 for (int32 i = 0; i < m_count; ++i) {
20135 m_ps[i] += dt * m_vs[i];
20136 }
20137
20138 // Solve constraints
20139 for (int32 i = 0; i < iterations; ++i) {
20140 if (m_tuning.bendingModel == b2_pbdAngleBendingModel) {
20141 SolveBend_PBD_Angle();
20142 } else if (m_tuning.bendingModel == b2_xpbdAngleBendingModel) {
20143 SolveBend_XPBD_Angle(dt);
20144 } else if (m_tuning.bendingModel == b2_pbdDistanceBendingModel) {
20145 SolveBend_PBD_Distance();
20146 } else if (m_tuning.bendingModel == b2_pbdHeightBendingModel) {
20147 SolveBend_PBD_Height();
20148 } else if (m_tuning.bendingModel == b2_pbdTriangleBendingModel) {
20149 SolveBend_PBD_Triangle();
20150 }
20151
20152 if (m_tuning.stretchingModel == b2_pbdStretchingModel) {
20153 SolveStretch_PBD();
20154 } else if (m_tuning.stretchingModel == b2_xpbdStretchingModel) {
20155 SolveStretch_XPBD(dt);
20156 }
20157 }
20158
20159 // Constrain velocity
20160 for (int32 i = 0; i < m_count; ++i) {
20161 m_vs[i] = inv_dt * (m_ps[i] - m_p0s[i]);
20162 m_p0s[i] = m_ps[i];
20163 }
20164}
20165
20166void b2Rope::Reset(const b2Vec2& position) {
20167 m_position = position;
20168
20169 for (int32 i = 0; i < m_count; ++i) {
20170 m_ps[i] = m_bindPositions[i] + m_position;
20171 m_p0s[i] = m_bindPositions[i] + m_position;
20172 m_vs[i].SetZero();
20173 }
20174
20175 for (int32 i = 0; i < m_bendCount; ++i) {
20176 m_bendConstraints[i].lambda = 0.0f;
20177 }
20178
20179 for (int32 i = 0; i < m_stretchCount; ++i) {
20180 m_stretchConstraints[i].lambda = 0.0f;
20181 }
20182}
20183
20184void b2Rope::SolveStretch_PBD() {
20185 const float stiffness = m_tuning.stretchStiffness;
20186
20187 for (int32 i = 0; i < m_stretchCount; ++i) {
20188 const b2RopeStretch& c = m_stretchConstraints[i];
20189
20190 b2Vec2 p1 = m_ps[c.i1];
20191 b2Vec2 p2 = m_ps[c.i2];
20192
20193 b2Vec2 d = p2 - p1;
20194 float L = d.Normalize();
20195
20196 float sum = c.invMass1 + c.invMass2;
20197 if (sum == 0.0f) {
20198 continue;
20199 }
20200
20201 float s1 = c.invMass1 / sum;
20202 float s2 = c.invMass2 / sum;
20203
20204 p1 -= stiffness * s1 * (c.L - L) * d;
20205 p2 += stiffness * s2 * (c.L - L) * d;
20206
20207 m_ps[c.i1] = p1;
20208 m_ps[c.i2] = p2;
20209 }
20210}
20211
20212void b2Rope::SolveStretch_XPBD(float dt) {
20213 b2Assert(dt > 0.0f);
20214
20215 for (int32 i = 0; i < m_stretchCount; ++i) {
20216 b2RopeStretch& c = m_stretchConstraints[i];
20217
20218 b2Vec2 p1 = m_ps[c.i1];
20219 b2Vec2 p2 = m_ps[c.i2];
20220
20221 b2Vec2 dp1 = p1 - m_p0s[c.i1];
20222 b2Vec2 dp2 = p2 - m_p0s[c.i2];
20223
20224 b2Vec2 u = p2 - p1;
20225 float L = u.Normalize();
20226
20227 b2Vec2 J1 = -u;
20228 b2Vec2 J2 = u;
20229
20230 float sum = c.invMass1 + c.invMass2;
20231 if (sum == 0.0f) {
20232 continue;
20233 }
20234
20235 const float alpha = 1.0f / (c.spring * dt * dt); // 1 / kg
20236 const float beta = dt * dt * c.damper; // kg * s
20237 const float sigma = alpha * beta / dt; // non-dimensional
20238 float C = L - c.L;
20239
20240 // This is using the initial velocities
20241 float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2);
20242
20243 float B = C + alpha * c.lambda + sigma * Cdot;
20244 float sum2 = (1.0f + sigma) * sum + alpha;
20245
20246 float impulse = -B / sum2;
20247
20248 p1 += (c.invMass1 * impulse) * J1;
20249 p2 += (c.invMass2 * impulse) * J2;
20250
20251 m_ps[c.i1] = p1;
20252 m_ps[c.i2] = p2;
20253 c.lambda += impulse;
20254 }
20255}
20256
20257void b2Rope::SolveBend_PBD_Angle() {
20258 const float stiffness = m_tuning.bendStiffness;
20259
20260 for (int32 i = 0; i < m_bendCount; ++i) {
20261 const b2RopeBend& c = m_bendConstraints[i];
20262
20263 b2Vec2 p1 = m_ps[c.i1];
20264 b2Vec2 p2 = m_ps[c.i2];
20265 b2Vec2 p3 = m_ps[c.i3];
20266
20267 b2Vec2 d1 = p2 - p1;
20268 b2Vec2 d2 = p3 - p2;
20269 float a = b2Cross(d1, d2);
20270 float b = b2Dot(d1, d2);
20271
20272 float angle = b2Atan2(a, b);
20273
20274 float L1sqr, L2sqr;
20275
20276 if (m_tuning.isometric) {
20277 L1sqr = c.L1 * c.L1;
20278 L2sqr = c.L2 * c.L2;
20279 } else {
20280 L1sqr = d1.LengthSquared();
20281 L2sqr = d2.LengthSquared();
20282 }
20283
20284 if (L1sqr * L2sqr == 0.0f) {
20285 continue;
20286 }
20287
20288 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
20289 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
20290
20291 b2Vec2 J1 = -Jd1;
20292 b2Vec2 J2 = Jd1 - Jd2;
20293 b2Vec2 J3 = Jd2;
20294
20295 float sum;
20296 if (m_tuning.fixedEffectiveMass) {
20297 sum = c.invEffectiveMass;
20298 } else {
20299 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
20300 }
20301
20302 if (sum == 0.0f) {
20303 sum = c.invEffectiveMass;
20304 }
20305
20306 float impulse = -stiffness * angle / sum;
20307
20308 p1 += (c.invMass1 * impulse) * J1;
20309 p2 += (c.invMass2 * impulse) * J2;
20310 p3 += (c.invMass3 * impulse) * J3;
20311
20312 m_ps[c.i1] = p1;
20313 m_ps[c.i2] = p2;
20314 m_ps[c.i3] = p3;
20315 }
20316}
20317
20318void b2Rope::SolveBend_XPBD_Angle(float dt) {
20319 b2Assert(dt > 0.0f);
20320
20321 for (int32 i = 0; i < m_bendCount; ++i) {
20322 b2RopeBend& c = m_bendConstraints[i];
20323
20324 b2Vec2 p1 = m_ps[c.i1];
20325 b2Vec2 p2 = m_ps[c.i2];
20326 b2Vec2 p3 = m_ps[c.i3];
20327
20328 b2Vec2 dp1 = p1 - m_p0s[c.i1];
20329 b2Vec2 dp2 = p2 - m_p0s[c.i2];
20330 b2Vec2 dp3 = p3 - m_p0s[c.i3];
20331
20332 b2Vec2 d1 = p2 - p1;
20333 b2Vec2 d2 = p3 - p2;
20334
20335 float L1sqr, L2sqr;
20336
20337 if (m_tuning.isometric) {
20338 L1sqr = c.L1 * c.L1;
20339 L2sqr = c.L2 * c.L2;
20340 } else {
20341 L1sqr = d1.LengthSquared();
20342 L2sqr = d2.LengthSquared();
20343 }
20344
20345 if (L1sqr * L2sqr == 0.0f) {
20346 continue;
20347 }
20348
20349 float a = b2Cross(d1, d2);
20350 float b = b2Dot(d1, d2);
20351
20352 float angle = b2Atan2(a, b);
20353
20354 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
20355 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
20356
20357 b2Vec2 J1 = -Jd1;
20358 b2Vec2 J2 = Jd1 - Jd2;
20359 b2Vec2 J3 = Jd2;
20360
20361 float sum;
20362 if (m_tuning.fixedEffectiveMass) {
20363 sum = c.invEffectiveMass;
20364 } else {
20365 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
20366 }
20367
20368 if (sum == 0.0f) {
20369 continue;
20370 }
20371
20372 const float alpha = 1.0f / (c.spring * dt * dt);
20373 const float beta = dt * dt * c.damper;
20374 const float sigma = alpha * beta / dt;
20375 float C = angle;
20376
20377 // This is using the initial velocities
20378 float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2) + b2Dot(J3, dp3);
20379
20380 float B = C + alpha * c.lambda + sigma * Cdot;
20381 float sum2 = (1.0f + sigma) * sum + alpha;
20382
20383 float impulse = -B / sum2;
20384
20385 p1 += (c.invMass1 * impulse) * J1;
20386 p2 += (c.invMass2 * impulse) * J2;
20387 p3 += (c.invMass3 * impulse) * J3;
20388
20389 m_ps[c.i1] = p1;
20390 m_ps[c.i2] = p2;
20391 m_ps[c.i3] = p3;
20392 c.lambda += impulse;
20393 }
20394}
20395
20396void b2Rope::ApplyBendForces(float dt) {
20397 // omega = 2 * pi * hz
20398 const float omega = 2.0f * b2_pi * m_tuning.bendHertz;
20399
20400 for (int32 i = 0; i < m_bendCount; ++i) {
20401 const b2RopeBend& c = m_bendConstraints[i];
20402
20403 b2Vec2 p1 = m_ps[c.i1];
20404 b2Vec2 p2 = m_ps[c.i2];
20405 b2Vec2 p3 = m_ps[c.i3];
20406
20407 b2Vec2 v1 = m_vs[c.i1];
20408 b2Vec2 v2 = m_vs[c.i2];
20409 b2Vec2 v3 = m_vs[c.i3];
20410
20411 b2Vec2 d1 = p2 - p1;
20412 b2Vec2 d2 = p3 - p2;
20413
20414 float L1sqr, L2sqr;
20415
20416 if (m_tuning.isometric) {
20417 L1sqr = c.L1 * c.L1;
20418 L2sqr = c.L2 * c.L2;
20419 } else {
20420 L1sqr = d1.LengthSquared();
20421 L2sqr = d2.LengthSquared();
20422 }
20423
20424 if (L1sqr * L2sqr == 0.0f) {
20425 continue;
20426 }
20427
20428 float a = b2Cross(d1, d2);
20429 float b = b2Dot(d1, d2);
20430
20431 float angle = b2Atan2(a, b);
20432
20433 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
20434 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
20435
20436 b2Vec2 J1 = -Jd1;
20437 b2Vec2 J2 = Jd1 - Jd2;
20438 b2Vec2 J3 = Jd2;
20439
20440 float sum;
20441 if (m_tuning.fixedEffectiveMass) {
20442 sum = c.invEffectiveMass;
20443 } else {
20444 sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3);
20445 }
20446
20447 if (sum == 0.0f) {
20448 continue;
20449 }
20450
20451 float mass = 1.0f / sum;
20452
20453 const float spring = mass * omega * omega;
20454 const float damper = 2.0f * mass * m_tuning.bendDamping * omega;
20455
20456 float C = angle;
20457 float Cdot = b2Dot(J1, v1) + b2Dot(J2, v2) + b2Dot(J3, v3);
20458
20459 float impulse = -dt * (spring * C + damper * Cdot);
20460
20461 m_vs[c.i1] += (c.invMass1 * impulse) * J1;
20462 m_vs[c.i2] += (c.invMass2 * impulse) * J2;
20463 m_vs[c.i3] += (c.invMass3 * impulse) * J3;
20464 }
20465}
20466
20467void b2Rope::SolveBend_PBD_Distance() {
20468 const float stiffness = m_tuning.bendStiffness;
20469
20470 for (int32 i = 0; i < m_bendCount; ++i) {
20471 const b2RopeBend& c = m_bendConstraints[i];
20472
20473 int32 i1 = c.i1;
20474 int32 i2 = c.i3;
20475
20476 b2Vec2 p1 = m_ps[i1];
20477 b2Vec2 p2 = m_ps[i2];
20478
20479 b2Vec2 d = p2 - p1;
20480 float L = d.Normalize();
20481
20482 float sum = c.invMass1 + c.invMass3;
20483 if (sum == 0.0f) {
20484 continue;
20485 }
20486
20487 float s1 = c.invMass1 / sum;
20488 float s2 = c.invMass3 / sum;
20489
20490 p1 -= stiffness * s1 * (c.L1 + c.L2 - L) * d;
20491 p2 += stiffness * s2 * (c.L1 + c.L2 - L) * d;
20492
20493 m_ps[i1] = p1;
20494 m_ps[i2] = p2;
20495 }
20496}
20497
20498// Constraint based implementation of:
20499// P. Volino: Simple Linear Bending Stiffness in Particle Systems
20500void b2Rope::SolveBend_PBD_Height() {
20501 const float stiffness = m_tuning.bendStiffness;
20502
20503 for (int32 i = 0; i < m_bendCount; ++i) {
20504 const b2RopeBend& c = m_bendConstraints[i];
20505
20506 b2Vec2 p1 = m_ps[c.i1];
20507 b2Vec2 p2 = m_ps[c.i2];
20508 b2Vec2 p3 = m_ps[c.i3];
20509
20510 // Barycentric coordinates are held constant
20511 b2Vec2 d = c.alpha1 * p1 + c.alpha2 * p3 - p2;
20512 float dLen = d.Length();
20513
20514 if (dLen == 0.0f) {
20515 continue;
20516 }
20517
20518 b2Vec2 dHat = (1.0f / dLen) * d;
20519
20520 b2Vec2 J1 = c.alpha1 * dHat;
20521 b2Vec2 J2 = -dHat;
20522 b2Vec2 J3 = c.alpha2 * dHat;
20523
20524 float sum = c.invMass1 * c.alpha1 * c.alpha1 + c.invMass2 + c.invMass3 * c.alpha2 * c.alpha2;
20525
20526 if (sum == 0.0f) {
20527 continue;
20528 }
20529
20530 float C = dLen;
20531 float mass = 1.0f / sum;
20532 float impulse = -stiffness * mass * C;
20533
20534 p1 += (c.invMass1 * impulse) * J1;
20535 p2 += (c.invMass2 * impulse) * J2;
20536 p3 += (c.invMass3 * impulse) * J3;
20537
20538 m_ps[c.i1] = p1;
20539 m_ps[c.i2] = p2;
20540 m_ps[c.i3] = p3;
20541 }
20542}
20543
20544// M. Kelager: A Triangle Bending Constraint Model for PBD
20545void b2Rope::SolveBend_PBD_Triangle() {
20546 const float stiffness = m_tuning.bendStiffness;
20547
20548 for (int32 i = 0; i < m_bendCount; ++i) {
20549 const b2RopeBend& c = m_bendConstraints[i];
20550
20551 b2Vec2 b0 = m_ps[c.i1];
20552 b2Vec2 v = m_ps[c.i2];
20553 b2Vec2 b1 = m_ps[c.i3];
20554
20555 float wb0 = c.invMass1;
20556 float wv = c.invMass2;
20557 float wb1 = c.invMass3;
20558
20559 float W = wb0 + wb1 + 2.0f * wv;
20560 float invW = stiffness / W;
20561
20562 b2Vec2 d = v - (1.0f / 3.0f) * (b0 + v + b1);
20563
20564 b2Vec2 db0 = 2.0f * wb0 * invW * d;
20565 b2Vec2 dv = -4.0f * wv * invW * d;
20566 b2Vec2 db1 = 2.0f * wb1 * invW * d;
20567
20568 b0 += db0;
20569 v += dv;
20570 b1 += db1;
20571
20572 m_ps[c.i1] = b0;
20573 m_ps[c.i2] = v;
20574 m_ps[c.i3] = b1;
20575 }
20576}
20577
20578void b2Rope::Draw(b2Draw* draw) const {
20579 b2Color c(0.4f, 0.5f, 0.7f);
20580 b2Color pg(0.1f, 0.8f, 0.1f);
20581 b2Color pd(0.7f, 0.2f, 0.4f);
20582
20583 for (int32 i = 0; i < m_count - 1; ++i) {
20584 draw->DrawSegment(m_ps[i], m_ps[i + 1], c);
20585
20586 const b2Color& pc = m_invMasses[i] > 0.0f ? pd : pg;
20587 draw->DrawPoint(m_ps[i], 5.0f, pc);
20588 }
20589
20590 const b2Color& pc = m_invMasses[m_count - 1] > 0.0f ? pd : pg;
20591 draw->DrawPoint(m_ps[m_count - 1], 5.0f, pc);
20592}
20593
20594#endif