Box2D: /build/box2d/src/box2d-adaedf1772ede7420bc9ee569c8cba7952db91a0/include/box2d/b2_math.h Source File

| Box2D 2.4.0 A 2D physics engine for games

b2_math.h
1 // MIT License
2
3 // Copyright (c) 2019 Erin Catto
4
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11
12 // The above copyright notice and this permission notice shall be included in all
13 // copies or substantial portions of the Software.
14
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 // SOFTWARE.
22
23 #ifndef B2_MATH_H
24 #define B2_MATH_H
25
26 #include <cmath>
27
28 #include "b2_api.h"
29 #include "b2_settings.h"
30
32 inline bool b2IsValid(float x)
33 {
34 return std::isfinite(x);
35 }
36
37 #define b2Sqrt(x) std::sqrt(x)
38 #define b2Atan2(y, x) std::atan2(y, x)
39
41 struct B2_API b2Vec2
42 {
44 b2Vec2() {}
45
47 b2Vec2(float xIn, float yIn) : x(xIn), y(yIn) {}
48
50 void SetZero() { x = 0.0f; y = 0.0f; }
51
53 void Set(float x_, float y_) { x = x_; y = y_; }
54
56 b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
57
59 float operator () (int32 i) const
60 {
61 return (&x)[i];
62 }
63
65 float& operator () (int32 i)
66 {
67 return (&x)[i];
68 }
69
71 void operator += (const b2Vec2& v)
72 {
73 x += v.x; y += v.y;
74 }
75
77 void operator -= (const b2Vec2& v)
78 {
79 x -= v.x; y -= v.y;
80 }
81
83 void operator *= (float a)
84 {
85 x *= a; y *= a;
86 }
87
89 float Length() const
90 {
91 return b2Sqrt(x * x + y * y);
92 }
93
96 float LengthSquared() const
97 {
98 return x * x + y * y;
99 }
100
102 float Normalize()
103 {
104 float length = Length();
105 if (length < b2_epsilon)
106 {
107 return 0.0f;
108 }
109 float invLength = 1.0f / length;
110 x *= invLength;
111 y *= invLength;
112
113 return length;
114 }
115
117 bool IsValid() const
118 {
119 return b2IsValid(x) && b2IsValid(y);
120 }
121
123 b2Vec2 Skew() const
124 {
125 return b2Vec2(-y, x);
126 }
127
128 float x, y;
129 };
130
132 struct B2_API b2Vec3
133 {
135 b2Vec3() {}
136
138 b2Vec3(float xIn, float yIn, float zIn) : x(xIn), y(yIn), z(zIn) {}
139
141 void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
142
144 void Set(float x_, float y_, float z_) { x = x_; y = y_; z = z_; }
145
147 b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
148
150 void operator += (const b2Vec3& v)
151 {
152 x += v.x; y += v.y; z += v.z;
153 }
154
156 void operator -= (const b2Vec3& v)
157 {
158 x -= v.x; y -= v.y; z -= v.z;
159 }
160
162 void operator *= (float s)
163 {
164 x *= s; y *= s; z *= s;
165 }
166
167 float x, y, z;
168 };
169
171 struct B2_API b2Mat22
172 {
174 b2Mat22() {}
175
177 b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
178 {
179 ex = c1;
180 ey = c2;
181 }
182
184 b2Mat22(float a11, float a12, float a21, float a22)
185 {
186 ex.x = a11; ex.y = a21;
187 ey.x = a12; ey.y = a22;
188 }
189
191 void Set(const b2Vec2& c1, const b2Vec2& c2)
192 {
193 ex = c1;
194 ey = c2;
195 }
196
198 void SetIdentity()
199 {
200 ex.x = 1.0f; ey.x = 0.0f;
201 ex.y = 0.0f; ey.y = 1.0f;
202 }
203
205 void SetZero()
206 {
207 ex.x = 0.0f; ey.x = 0.0f;
208 ex.y = 0.0f; ey.y = 0.0f;
209 }
210
211 b2Mat22 GetInverse() const
212 {
213 float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
214 b2Mat22 B;
215 float det = a * d - b * c;
216 if (det != 0.0f)
217 {
218 det = 1.0f / det;
219 }
220 B.ex.x = det * d; B.ey.x = -det * b;
221 B.ex.y = -det * c; B.ey.y = det * a;
222 return B;
223 }
224
227 b2Vec2 Solve(const b2Vec2& b) const
228 {
229 float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
230 float det = a11 * a22 - a12 * a21;
231 if (det != 0.0f)
232 {
233 det = 1.0f / det;
234 }
235 b2Vec2 x;
236 x.x = det * (a22 * b.x - a12 * b.y);
237 x.y = det * (a11 * b.y - a21 * b.x);
238 return x;
239 }
240
241 b2Vec2 ex, ey;
242 };
243
245 struct B2_API b2Mat33
246 {
248 b2Mat33() {}
249
251 b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
252 {
253 ex = c1;
254 ey = c2;
255 ez = c3;
256 }
257
259 void SetZero()
260 {
261 ex.SetZero();
262 ey.SetZero();
263 ez.SetZero();
264 }
265
268 b2Vec3 Solve33(const b2Vec3& b) const;
269
273 b2Vec2 Solve22(const b2Vec2& b) const;
274
277 void GetInverse22(b2Mat33* M) const;
278
281 void GetSymInverse33(b2Mat33* M) const;
282
283 b2Vec3 ex, ey, ez;
284 };
285
287 struct B2_API b2Rot
288 {
289 b2Rot() {}
290
292 explicit b2Rot(float angle)
293 {
295 s = std::sin(angle);
296 c = std::cos(angle);
297 }
298
300 void Set(float angle)
301 {
303 s = std::sin(angle);
304 c = std::cos(angle);
305 }
306
308 void SetIdentity()
309 {
310 s = 0.0f;
311 c = 1.0f;
312 }
313
315 float GetAngle() const
316 {
317 return b2Atan2(s, c);
318 }
319
321 b2Vec2 GetXAxis() const
322 {
323 return b2Vec2(c, s);
324 }
325
327 b2Vec2 GetYAxis() const
328 {
329 return b2Vec2(-s, c);
330 }
331
333 float s, c;
334 };
335
338 struct B2_API b2Transform
339 {
341 b2Transform() {}
342
344 b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
345
347 void SetIdentity()
348 {
349 p.SetZero();
350 q.SetIdentity();
351 }
352
354 void Set(const b2Vec2& position, float angle)
355 {
356 p = position;
357 q.Set(angle);
358 }
359
360 b2Vec2 p;
361 b2Rot q;
362 };
363
368 struct B2_API b2Sweep
369 {
373 void GetTransform(b2Transform* transform, float beta) const;
374
377 void Advance(float alpha);
378
380 void Normalize();
381
382 b2Vec2 localCenter;
383 b2Vec2 c0, c;
384 float a0, a;
385
388 float alpha0;
389 };
390
392 extern B2_API const b2Vec2 b2Vec2_zero;
393
395 inline float b2Dot(const b2Vec2& a, const b2Vec2& b)
396 {
397 return a.x * b.x + a.y * b.y;
398 }
399
401 inline float b2Cross(const b2Vec2& a, const b2Vec2& b)
402 {
403 return a.x * b.y - a.y * b.x;
404 }
405
408 inline b2Vec2 b2Cross(const b2Vec2& a, float s)
409 {
410 return b2Vec2(s * a.y, -s * a.x);
411 }
412
415 inline b2Vec2 b2Cross(float s, const b2Vec2& a)
416 {
417 return b2Vec2(-s * a.y, s * a.x);
418 }
419
422 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
423 {
424 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
425 }
426
429 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
430 {
431 return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
432 }
433
435 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
436 {
437 return b2Vec2(a.x + b.x, a.y + b.y);
438 }
439
441 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
442 {
443 return b2Vec2(a.x - b.x, a.y - b.y);
444 }
445
446 inline b2Vec2 operator * (float s, const b2Vec2& a)
447 {
448 return b2Vec2(s * a.x, s * a.y);
449 }
450
451 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
452 {
453 return a.x == b.x && a.y == b.y;
454 }
455
456 inline bool operator != (const b2Vec2& a, const b2Vec2& b)
457 {
458 return a.x != b.x || a.y != b.y;
459 }
460
461 inline float b2Distance(const b2Vec2& a, const b2Vec2& b)
462 {
463 b2Vec2 c = a - b;
464 return c.Length();
465 }
466
467 inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
468 {
469 b2Vec2 c = a - b;
470 return b2Dot(c, c);
471 }
472
473 inline b2Vec3 operator * (float s, const b2Vec3& a)
474 {
475 return b2Vec3(s * a.x, s * a.y, s * a.z);
476 }
477
479 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
480 {
481 return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
482 }
483
485 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
486 {
487 return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
488 }
489
491 inline float b2Dot(const b2Vec3& a, const b2Vec3& b)
492 {
493 return a.x * b.x + a.y * b.y + a.z * b.z;
494 }
495
497 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
498 {
499 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);
500 }
501
502 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
503 {
504 return b2Mat22(A.ex + B.ex, A.ey + B.ey);
505 }
506
507 // A * B
508 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
509 {
510 return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
511 }
512
513 // A^T * B
514 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
515 {
516 b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
517 b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
518 return b2Mat22(c1, c2);
519 }
520
522 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
523 {
524 return v.x * A.ex + v.y * A.ey + v.z * A.ez;
525 }
526
528 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
529 {
530 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
531 }
532
534 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
535 {
536 // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
537 // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
538 // s = qs * rc + qc * rs
539 // c = qc * rc - qs * rs
540 b2Rot qr;
541 qr.s = q.s * r.c + q.c * r.s;
542 qr.c = q.c * r.c - q.s * r.s;
543 return qr;
544 }
545
547 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
548 {
549 // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
550 // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
551 // s = qc * rs - qs * rc
552 // c = qc * rc + qs * rs
553 b2Rot qr;
554 qr.s = q.c * r.s - q.s * r.c;
555 qr.c = q.c * r.c + q.s * r.s;
556 return qr;
557 }
558
560 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
561 {
562 return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
563 }
564
566 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
567 {
568 return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
569 }
570
571 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
572 {
573 float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
574 float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
575
576 return b2Vec2(x, y);
577 }
578
579 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
580 {
581 float px = v.x - T.p.x;
582 float py = v.y - T.p.y;
583 float x = (T.q.c * px + T.q.s * py);
584 float y = (-T.q.s * px + T.q.c * py);
585
586 return b2Vec2(x, y);
587 }
588
589 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
590 // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
591 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
592 {
593 b2Transform C;
594 C.q = b2Mul(A.q, B.q);
595 C.p = b2Mul(A.q, B.p) + A.p;
596 return C;
597 }
598
599 // v2 = A.q' * (B.q * v1 + B.p - A.p)
600 // = A.q' * B.q * v1 + A.q' * (B.p - A.p)
601 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
602 {
603 b2Transform C;
604 C.q = b2MulT(A.q, B.q);
605 C.p = b2MulT(A.q, B.p - A.p);
606 return C;
607 }
608
609 template <typename T>
610 inline T b2Abs(T a)
611 {
612 return a > T(0) ? a : -a;
613 }
614
615 inline b2Vec2 b2Abs(const b2Vec2& a)
616 {
617 return b2Vec2(b2Abs(a.x), b2Abs(a.y));
618 }
619
620 inline b2Mat22 b2Abs(const b2Mat22& A)
621 {
622 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
623 }
624
625 template <typename T>
626 inline T b2Min(T a, T b)
627 {
628 return a < b ? a : b;
629 }
630
631 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
632 {
633 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
634 }
635
636 template <typename T>
637 inline T b2Max(T a, T b)
638 {
639 return a > b ? a : b;
640 }
641
642 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
643 {
644 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
645 }
646
647 template <typename T>
648 inline T b2Clamp(T a, T low, T high)
649 {
650 return b2Max(low, b2Min(a, high));
651 }
652
653 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
654 {
655 return b2Max(low, b2Min(a, high));
656 }
657
658 template<typename T> inline void b2Swap(T& a, T& b)
659 {
660 T tmp = a;
661 a = b;
662 b = tmp;
663 }
664
670 inline uint32 b2NextPowerOfTwo(uint32 x)
671 {
672 x |= (x >> 1);
673 x |= (x >> 2);
674 x |= (x >> 4);
675 x |= (x >> 8);
676 x |= (x >> 16);
677 return x + 1;
678 }
679
680 inline bool b2IsPowerOfTwo(uint32 x)
681 {
682 bool result = x > 0 && (x & (x - 1)) == 0;
683 return result;
684 }
685
686 inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const
687 {
688 xf->p = c0 + beta * (c - c0);
689 float angle = a0 + beta * (a - a0);
690 xf->q.Set(angle);
691
692 // Shift to origin
693 xf->p -= b2Mul(xf->q, localCenter);
694 }
695
696 inline void b2Sweep::Advance(float alpha)
697 {
698 b2Assert(alpha0 < 1.0f);
699 float beta = (alpha - alpha0) / (1.0f - alpha0);
700 c0 += beta * (c - c0);
701 a0 += beta * (a - a0);
702 alpha0 = alpha;
703 }
704
706 inline void b2Sweep::Normalize()
707 {
708 float twoPi = 2.0f * b2_pi;
709 float d = twoPi * std::floor(a0 / twoPi);
710 a0 -= d;
711 a -= d;
712 }
713
714 #endif
b2Rot::GetXAxis
b2Vec2 GetXAxis() const
Get the x-axis.
Definition: b2_math.h:321
b2Vec2
A 2D column vector.
Definition: b2_math.h:42
b2Mat22::b2Mat22
b2Mat22(const b2Vec2 &c1, const b2Vec2 &c2)
Construct this matrix using columns.
Definition: b2_math.h:177
b2Mat33::GetInverse22
void GetInverse22(b2Mat33 *M) const
b2_settings.h
b2Transform::Set
void Set(const b2Vec2 &position, float angle)
Set this based on the position and angle.
Definition: b2_math.h:354
b2Rot::b2Rot
b2Rot(float angle)
Initialize from an angle in radians.
Definition: b2_math.h:292
b2Sweep::alpha0
float alpha0
Definition: b2_math.h:388
b2Mat22::Solve
b2Vec2 Solve(const b2Vec2 &b) const
Definition: b2_math.h:227
b2Mat33::SetZero
void SetZero()
Set this matrix to all zeros.
Definition: b2_math.h:259
b2Transform
Definition: b2_math.h:339
b2Transform::b2Transform
b2Transform(const b2Vec2 &position, const b2Rot &rotation)
Initialize using a position vector and a rotation.
Definition: b2_math.h:344
b2Vec3
A 2D column vector with 3 elements.
Definition: b2_math.h:133
b2Sweep
Definition: b2_math.h:369
b2Mat33
A 3-by-3 matrix. Stored in column-major order.
Definition: b2_math.h:246
b2Sweep::c
b2Vec2 c
center world positions
Definition: b2_math.h:383
b2Mat22::Set
void Set(const b2Vec2 &c1, const b2Vec2 &c2)
Initialize this matrix using columns.
Definition: b2_math.h:191
b2Mat33::b2Mat33
b2Mat33()
The default constructor does nothing (for performance).
Definition: b2_math.h:248
b2Rot
Rotation.
Definition: b2_math.h:288
b2Vec2::Set
void Set(float x_, float y_)
Set this vector to some specified coordinates.
Definition: b2_math.h:53
b2Vec3::Set
void Set(float x_, float y_, float z_)
Set this vector to some specified coordinates.
Definition: b2_math.h:144
b2Vec2::Normalize
float Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2_math.h:102
b2Sweep::GetTransform
void GetTransform(b2Transform *transform, float beta) const
Definition: b2_math.h:686
b2Vec3::b2Vec3
b2Vec3()
Default constructor does nothing (for performance).
Definition: b2_math.h:135
b2Mat22::SetIdentity
void SetIdentity()
Set this to the identity matrix.
Definition: b2_math.h:198
b2Transform::SetIdentity
void SetIdentity()
Set this to the identity transform.
Definition: b2_math.h:347
b2Vec3::b2Vec3
b2Vec3(float xIn, float yIn, float zIn)
Construct using coordinates.
Definition: b2_math.h:138
b2Rot::SetIdentity
void SetIdentity()
Set to the identity rotation.
Definition: b2_math.h:308
b2Sweep::Advance
void Advance(float alpha)
Definition: b2_math.h:696
b2Mat22::b2Mat22
b2Mat22(float a11, float a12, float a21, float a22)
Construct this matrix using scalars.
Definition: b2_math.h:184
b2Vec2::Length
float Length() const
Get the length of this vector (the norm).
Definition: b2_math.h:89
b2Mat33::Solve33
b2Vec3 Solve33(const b2Vec3 &b) const
b2Vec2::b2Vec2
b2Vec2()
Default constructor does nothing (for performance).
Definition: b2_math.h:44
b2Mat22
A 2-by-2 matrix. Stored in column-major order.
Definition: b2_math.h:172
b2Transform::b2Transform
b2Transform()
The default constructor does nothing.
Definition: b2_math.h:341
b2Rot::Set
void Set(float angle)
Set using an angle in radians.
Definition: b2_math.h:300
b2Vec2::SetZero
void SetZero()
Set this vector to all zeros.
Definition: b2_math.h:50
b2Vec2::b2Vec2
b2Vec2(float xIn, float yIn)
Construct using coordinates.
Definition: b2_math.h:47
b2Vec2::LengthSquared
float LengthSquared() const
Definition: b2_math.h:96
b2Mat33::b2Mat33
b2Mat33(const b2Vec3 &c1, const b2Vec3 &c2, const b2Vec3 &c3)
Construct this matrix using columns.
Definition: b2_math.h:251
b2Vec3::SetZero
void SetZero()
Set this vector to all zeros.
Definition: b2_math.h:141
b2Rot::GetAngle
float GetAngle() const
Get the angle in radians.
Definition: b2_math.h:315
b2Rot::s
float s
Sine and cosine.
Definition: b2_math.h:333
b2Mat22::b2Mat22
b2Mat22()
The default constructor does nothing (for performance).
Definition: b2_math.h:174
b2Sweep::localCenter
b2Vec2 localCenter
local center of mass position
Definition: b2_math.h:382
b2Sweep::Normalize
void Normalize()
Normalize the angles.
Definition: b2_math.h:706
b2Mat22::SetZero
void SetZero()
Set this matrix to all zeros.
Definition: b2_math.h:205
b2Mat33::Solve22
b2Vec2 Solve22(const b2Vec2 &b) const
b2Mat33::GetSymInverse33
void GetSymInverse33(b2Mat33 *M) const
b2Vec2::Skew
b2Vec2 Skew() const
Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
Definition: b2_math.h:123
b2Sweep::a
float a
world angles
Definition: b2_math.h:384
b2Rot::GetYAxis
b2Vec2 GetYAxis() const
Get the u-axis.
Definition: b2_math.h:327
b2Vec2::IsValid
bool IsValid() const
Does this vector contain finite coordinates?
Definition: b2_math.h:117