Box2D: /build/box2d/src/box2d-2.4.1/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() {}
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 = 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() {}
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
392extern B2_API const b2Vec2 b2Vec2_zero;
393
395inline float b2Dot(const b2Vec2& a, const b2Vec2& b)
396{
397 return a.x * b.x + a.y * b.y;
398}
399
401inline float b2Cross(const b2Vec2& a, const b2Vec2& b)
402{
403 return a.x * b.y - a.y * b.x;
404}
405
408inline b2Vec2 b2Cross(const b2Vec2& a, float s)
409{
410 return b2Vec2(s * a.y, -s * a.x);
411}
412
415inline b2Vec2 b2Cross(float s, const b2Vec2& a)
416{
417 return b2Vec2(-s * a.y, s * a.x);
418}
419
422inline 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
429inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
430{
431 return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
432}
433
435inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
436{
437 return b2Vec2(a.x + b.x, a.y + b.y);
438}
439
441inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
442{
443 return b2Vec2(a.x - b.x, a.y - b.y);
444}
445
446inline b2Vec2 operator * (float s, const b2Vec2& a)
447{
448 return b2Vec2(s * a.x, s * a.y);
449}
450
451inline bool operator == (const b2Vec2& a, const b2Vec2& b)
452{
453 return a.x == b.x && a.y == b.y;
454}
455
456inline bool operator != (const b2Vec2& a, const b2Vec2& b)
457{
458 return a.x != b.x || a.y != b.y;
459}
460
461inline float b2Distance(const b2Vec2& a, const b2Vec2& b)
462{
463 b2Vec2 c = a - b;
464 return c.Length();
465}
466
467inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
468{
469 b2Vec2 c = a - b;
470 return b2Dot(c, c);
471}
472
473inline b2Vec3 operator * (float s, const b2Vec3& a)
474{
475 return b2Vec3(s * a.x, s * a.y, s * a.z);
476}
477
479inline 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
485inline 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
491inline 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
497inline 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
502inline 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
508inline 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
514inline 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
522inline 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
528inline 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
534inline 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
547inline 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
560inline 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
566inline 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
571inline 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
579inline 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
591inline 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)
601inline 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
609template <typename T>
610inline T b2Abs(T a)
611{
612 return a > T(0) ? a : -a;
613}
614
615inline b2Vec2 b2Abs(const b2Vec2& a)
616{
617 return b2Vec2(b2Abs(a.x), b2Abs(a.y));
618}
619
620inline b2Mat22 b2Abs(const b2Mat22& A)
621{
622 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
623}
624
625template <typename T>
626inline T b2Min(T a, T b)
627{
628 return a < b ? a : b;
629}
630
631inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
632{
633 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
634}
635
636template <typename T>
637inline T b2Max(T a, T b)
638{
639 return a > b ? a : b;
640}
641
642inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
643{
644 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
645}
646
647template <typename T>
648inline T b2Clamp(T a, T low, T high)
649{
650 return b2Max(low, b2Min(a, high));
651}
652
653inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
654{
655 return b2Max(low, b2Min(a, high));
656}
657
658template<typename T> inline void b2Swap(T& a, T& b)
659{
660 T tmp = a;
661 a = b;
662 b = tmp;
663}
664
670inline 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
680inline bool b2IsPowerOfTwo(uint32 x)
681{
682 bool result = x > 0 && (x & (x - 1)) == 0;
683 return result;
684}
685
686// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
687 inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const
688{
689 xf->p = (1.0f - beta) * c0 + beta * c;
690 float angle = (1.0f - beta) * a0 + beta * a;
691 xf->q.Set(angle);
692
693 // Shift to origin
694 xf->p -= b2Mul(xf->q, localCenter);
695}
696
697 inline void b2Sweep::Advance(float alpha)
698{
699 b2Assert(alpha0 < 1.0f);
700 float beta = (alpha - alpha0) / (1.0f - alpha0);
701 c0 += beta * (c - c0);
702 a0 += beta * (a - a0);
703 alpha0 = alpha;
704}
705
707 inline void b2Sweep::Normalize()
708{
709 float twoPi = 2.0f * b2_pi;
710 float d = twoPi * floorf(a0 / twoPi);
711 a0 -= d;
712 a -= d;
713}
714
715#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(const b2Vec2 &c1, const b2Vec2 &c2)
Construct this matrix using columns.
Definition b2_math.h:177
b2Mat22::b2Mat22
b2Mat22()
The default constructor does nothing (for performance).
Definition b2_math.h:174
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::b2Mat33
b2Mat33()
The default constructor does nothing (for performance).
Definition b2_math.h:248
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::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:383
b2Sweep::Advance
void Advance(float alpha)
Definition b2_math.h:697
b2Sweep::localCenter
b2Vec2 localCenter
local center of mass position
Definition b2_math.h:382
b2Sweep::a
float a
world angles
Definition b2_math.h:384
b2Sweep::GetTransform
void GetTransform(b2Transform *transform, float beta) const
Definition b2_math.h:687
b2Sweep::Normalize
void Normalize()
Normalize the angles.
Definition b2_math.h:707
b2Sweep::alpha0
float alpha0
Definition b2_math.h:388
b2Transform
Definition b2_math.h:339
b2Transform::b2Transform
b2Transform()
The default constructor does nothing.
Definition b2_math.h:341
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::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::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::b2Vec2
b2Vec2()
Default constructor does nothing (for performance).
Definition b2_math.h:44
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::SetZero
void SetZero()
Set this vector to all zeros.
Definition b2_math.h:141
b2Vec3::b2Vec3
b2Vec3()
Default constructor does nothing (for performance).
Definition b2_math.h:135
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