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

| Box2D 2.4.1 A 2D physics engine for games

Loading...
Searching...
No Matches
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 <math.h>
27
28#include "b2_api.h"
29#include "b2_settings.h"
30
32inline bool b2IsValid(float x)
33{
34 return isfinite(x);
35}
36
37#define b2Sqrt(x) sqrtf(x)
38#define b2Atan2(y, x) atan2f(y, x)
39
41 struct B2_API b2Vec2
42{
44 b2Vec2() = default;
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() = default;
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() = default;
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() = default;
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() = default;
290
292 explicit b2Rot(float angle)
293 {
295 s = sinf(angle);
296 c = cosf(angle);
297 }
298
300 void Set(float angle)
301 {
303 s = sinf(angle);
304 c = cosf(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() = default;
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{
370 b2Sweep() = default;
371
375 void GetTransform(b2Transform* transform, float beta) const;
376
379 void Advance(float alpha);
380
382 void Normalize();
383
384 b2Vec2 localCenter;
385 b2Vec2 c0, c;
386 float a0, a;
387
390 float alpha0;
391};
392
394extern B2_API const b2Vec2 b2Vec2_zero;
395
397inline float b2Dot(const b2Vec2& a, const b2Vec2& b)
398{
399 return a.x * b.x + a.y * b.y;
400}
401
403inline float b2Cross(const b2Vec2& a, const b2Vec2& b)
404{
405 return a.x * b.y - a.y * b.x;
406}
407
410inline b2Vec2 b2Cross(const b2Vec2& a, float s)
411{
412 return b2Vec2(s * a.y, -s * a.x);
413}
414
417inline b2Vec2 b2Cross(float s, const b2Vec2& a)
418{
419 return b2Vec2(-s * a.y, s * a.x);
420}
421
424inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
425{
426 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
427}
428
431inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
432{
433 return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
434}
435
437inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
438{
439 return b2Vec2(a.x + b.x, a.y + b.y);
440}
441
443inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
444{
445 return b2Vec2(a.x - b.x, a.y - b.y);
446}
447
448inline b2Vec2 operator * (float s, const b2Vec2& a)
449{
450 return b2Vec2(s * a.x, s * a.y);
451}
452
453inline bool operator == (const b2Vec2& a, const b2Vec2& b)
454{
455 return a.x == b.x && a.y == b.y;
456}
457
458inline bool operator != (const b2Vec2& a, const b2Vec2& b)
459{
460 return a.x != b.x || a.y != b.y;
461}
462
463inline float b2Distance(const b2Vec2& a, const b2Vec2& b)
464{
465 b2Vec2 c = a - b;
466 return c.Length();
467}
468
469inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
470{
471 b2Vec2 c = a - b;
472 return b2Dot(c, c);
473}
474
475inline b2Vec3 operator * (float s, const b2Vec3& a)
476{
477 return b2Vec3(s * a.x, s * a.y, s * a.z);
478}
479
481inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
482{
483 return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
484}
485
487inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
488{
489 return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
490}
491
493inline float b2Dot(const b2Vec3& a, const b2Vec3& b)
494{
495 return a.x * b.x + a.y * b.y + a.z * b.z;
496}
497
499inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
500{
501 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);
502}
503
504inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
505{
506 return b2Mat22(A.ex + B.ex, A.ey + B.ey);
507}
508
509// A * B
510inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
511{
512 return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
513}
514
515// A^T * B
516inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
517{
518 b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
519 b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
520 return b2Mat22(c1, c2);
521}
522
524inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
525{
526 return v.x * A.ex + v.y * A.ey + v.z * A.ez;
527}
528
530inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
531{
532 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
533}
534
536inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
537{
538 // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
539 // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
540 // s = qs * rc + qc * rs
541 // c = qc * rc - qs * rs
542 b2Rot qr;
543 qr.s = q.s * r.c + q.c * r.s;
544 qr.c = q.c * r.c - q.s * r.s;
545 return qr;
546}
547
549inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
550{
551 // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
552 // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
553 // s = qc * rs - qs * rc
554 // c = qc * rc + qs * rs
555 b2Rot qr;
556 qr.s = q.c * r.s - q.s * r.c;
557 qr.c = q.c * r.c + q.s * r.s;
558 return qr;
559}
560
562inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
563{
564 return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
565}
566
568inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
569{
570 return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
571}
572
573inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
574{
575 float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
576 float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
577
578 return b2Vec2(x, y);
579}
580
581inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
582{
583 float px = v.x - T.p.x;
584 float py = v.y - T.p.y;
585 float x = (T.q.c * px + T.q.s * py);
586 float y = (-T.q.s * px + T.q.c * py);
587
588 return b2Vec2(x, y);
589}
590
591// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
592// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
593inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
594{
595 b2Transform C;
596 C.q = b2Mul(A.q, B.q);
597 C.p = b2Mul(A.q, B.p) + A.p;
598 return C;
599}
600
601// v2 = A.q' * (B.q * v1 + B.p - A.p)
602// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
603inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
604{
605 b2Transform C;
606 C.q = b2MulT(A.q, B.q);
607 C.p = b2MulT(A.q, B.p - A.p);
608 return C;
609}
610
611template <typename T>
612inline T b2Abs(T a)
613{
614 return a > T(0) ? a : -a;
615}
616
617inline b2Vec2 b2Abs(const b2Vec2& a)
618{
619 return b2Vec2(b2Abs(a.x), b2Abs(a.y));
620}
621
622inline b2Mat22 b2Abs(const b2Mat22& A)
623{
624 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
625}
626
627template <typename T>
628inline T b2Min(T a, T b)
629{
630 return a < b ? a : b;
631}
632
633inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
634{
635 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
636}
637
638template <typename T>
639inline T b2Max(T a, T b)
640{
641 return a > b ? a : b;
642}
643
644inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
645{
646 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
647}
648
649template <typename T>
650inline T b2Clamp(T a, T low, T high)
651{
652 return b2Max(low, b2Min(a, high));
653}
654
655inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
656{
657 return b2Max(low, b2Min(a, high));
658}
659
660template<typename T> inline void b2Swap(T& a, T& b)
661{
662 T tmp = a;
663 a = b;
664 b = tmp;
665}
666
672inline uint32 b2NextPowerOfTwo(uint32 x)
673{
674 x |= (x >> 1);
675 x |= (x >> 2);
676 x |= (x >> 4);
677 x |= (x >> 8);
678 x |= (x >> 16);
679 return x + 1;
680}
681
682inline bool b2IsPowerOfTwo(uint32 x)
683{
684 bool result = x > 0 && (x & (x - 1)) == 0;
685 return result;
686}
687
688// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
689 inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const
690{
691 xf->p = (1.0f - beta) * c0 + beta * c;
692 float angle = (1.0f - beta) * a0 + beta * a;
693 xf->q.Set(angle);
694
695 // Shift to origin
696 xf->p -= b2Mul(xf->q, localCenter);
697}
698
699 inline void b2Sweep::Advance(float alpha)
700{
701 b2Assert(alpha0 < 1.0f);
702 float beta = (alpha - alpha0) / (1.0f - alpha0);
703 c0 += beta * (c - c0);
704 a0 += beta * (a - a0);
705 alpha0 = alpha;
706}
707
709 inline void b2Sweep::Normalize()
710{
711 float twoPi = 2.0f * b2_pi;
712 float d = twoPi * floorf(a0 / twoPi);
713 a0 -= d;
714 a -= d;
715}
716
717#endif
b2_settings.h
b2Mat22
A 2-by-2 matrix. Stored in column-major order.
Definition b2_math.h:172
b2Mat22::b2Mat22
b2Mat22(float a11, float a12, float a21, float a22)
Construct this matrix using scalars.
Definition b2_math.h:184
b2Mat22::Solve
b2Vec2 Solve(const b2Vec2 &b) const
Definition b2_math.h:227
b2Mat22::SetIdentity
void SetIdentity()
Set this to the identity matrix.
Definition b2_math.h:198
b2Mat22::SetZero
void SetZero()
Set this matrix to all zeros.
Definition b2_math.h:205
b2Mat22::b2Mat22
b2Mat22()=default
The default constructor does nothing (for performance).
b2Mat22::b2Mat22
b2Mat22(const b2Vec2 &c1, const b2Vec2 &c2)
Construct this matrix using columns.
Definition b2_math.h:177
b2Mat22::Set
void Set(const b2Vec2 &c1, const b2Vec2 &c2)
Initialize this matrix using columns.
Definition b2_math.h:191
b2Mat33
A 3-by-3 matrix. Stored in column-major order.
Definition b2_math.h:246
b2Mat33::GetSymInverse33
void GetSymInverse33(b2Mat33 *M) const
b2Mat33::Solve33
b2Vec3 Solve33(const b2Vec3 &b) const
b2Mat33::b2Mat33
b2Mat33(const b2Vec3 &c1, const b2Vec3 &c2, const b2Vec3 &c3)
Construct this matrix using columns.
Definition b2_math.h:251
b2Mat33::SetZero
void SetZero()
Set this matrix to all zeros.
Definition b2_math.h:259
b2Mat33::GetInverse22
void GetInverse22(b2Mat33 *M) const
b2Mat33::b2Mat33
b2Mat33()=default
The default constructor does nothing (for performance).
b2Mat33::Solve22
b2Vec2 Solve22(const b2Vec2 &b) const
b2Rot
Rotation.
Definition b2_math.h:288
b2Rot::Set
void Set(float angle)
Set using an angle in radians.
Definition b2_math.h:300
b2Rot::s
float s
Sine and cosine.
Definition b2_math.h:333
b2Rot::GetAngle
float GetAngle() const
Get the angle in radians.
Definition b2_math.h:315
b2Rot::SetIdentity
void SetIdentity()
Set to the identity rotation.
Definition b2_math.h:308
b2Rot::GetXAxis
b2Vec2 GetXAxis() const
Get the x-axis.
Definition b2_math.h:321
b2Rot::b2Rot
b2Rot(float angle)
Initialize from an angle in radians.
Definition b2_math.h:292
b2Rot::GetYAxis
b2Vec2 GetYAxis() const
Get the u-axis.
Definition b2_math.h:327
b2Sweep
Definition b2_math.h:369
b2Sweep::c
b2Vec2 c
center world positions
Definition b2_math.h:385
b2Sweep::Advance
void Advance(float alpha)
Definition b2_math.h:699
b2Sweep::localCenter
b2Vec2 localCenter
local center of mass position
Definition b2_math.h:384
b2Sweep::a
float a
world angles
Definition b2_math.h:386
b2Sweep::GetTransform
void GetTransform(b2Transform *transform, float beta) const
Definition b2_math.h:689
b2Sweep::Normalize
void Normalize()
Normalize the angles.
Definition b2_math.h:709
b2Sweep::alpha0
float alpha0
Definition b2_math.h:390
b2Transform
Definition b2_math.h:339
b2Transform::Set
void Set(const b2Vec2 &position, float angle)
Set this based on the position and angle.
Definition b2_math.h:354
b2Transform::b2Transform
b2Transform(const b2Vec2 &position, const b2Rot &rotation)
Initialize using a position vector and a rotation.
Definition b2_math.h:344
b2Transform::b2Transform
b2Transform()=default
The default constructor does nothing.
b2Transform::SetIdentity
void SetIdentity()
Set this to the identity transform.
Definition b2_math.h:347
b2Vec2
A 2D column vector.
Definition b2_math.h:42
b2Vec2::b2Vec2
b2Vec2()=default
Default constructor does nothing (for performance).
b2Vec2::SetZero
void SetZero()
Set this vector to all zeros.
Definition b2_math.h:50
b2Vec2::Set
void Set(float x_, float y_)
Set this vector to some specified coordinates.
Definition b2_math.h:53
b2Vec2::Length
float Length() const
Get the length of this vector (the norm).
Definition b2_math.h:89
b2Vec2::Skew
b2Vec2 Skew() const
Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
Definition b2_math.h:123
b2Vec2::b2Vec2
b2Vec2(float xIn, float yIn)
Construct using coordinates.
Definition b2_math.h:47
b2Vec2::IsValid
bool IsValid() const
Does this vector contain finite coordinates?
Definition b2_math.h:117
b2Vec2::LengthSquared
float LengthSquared() const
Definition b2_math.h:96
b2Vec2::Normalize
float Normalize()
Convert this vector into a unit vector. Returns the length.
Definition b2_math.h:102
b2Vec3
A 2D column vector with 3 elements.
Definition b2_math.h:133
b2Vec3::b2Vec3
b2Vec3()=default
Default constructor does nothing (for performance).
b2Vec3::SetZero
void SetZero()
Set this vector to all zeros.
Definition b2_math.h:141
b2Vec3::b2Vec3
b2Vec3(float xIn, float yIn, float zIn)
Construct using coordinates.
Definition b2_math.h:138
b2Vec3::Set
void Set(float x_, float y_, float z_)
Set this vector to some specified coordinates.
Definition b2_math.h:144