Box2D  2.3.0
A 2D Physics Engine for Games
b2Math.h
1 /*
2 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
3 *
4 * This software is provided 'as-is', without any express or implied
5 * warranty. In no event will the authors be held liable for any damages
6 * arising from the use of this software.
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 * 1. The origin of this software must not be misrepresented; you must not
11 * claim that you wrote the original software. If you use this software
12 * in a product, an acknowledgment in the product documentation would be
13 * appreciated but is not required.
14 * 2. Altered source versions must be plainly marked as such, and must not be
15 * misrepresented as being the original software.
16 * 3. This notice may not be removed or altered from any source distribution.
17 */
18 
19 #ifndef B2_MATH_H
20 #define B2_MATH_H
21 
23 #include <math.h>
24 
26 inline bool b2IsValid(float32 x)
27 {
28  int32 ix = *reinterpret_cast<int32*>(&x);
29  return (ix & 0x7f800000) != 0x7f800000;
30 }
31 
33 inline float32 b2InvSqrt(float32 x)
34 {
35  union
36  {
37  float32 x;
38  int32 i;
39  } convert;
40 
41  convert.x = x;
42  float32 xhalf = 0.5f * x;
43  convert.i = 0x5f3759df - (convert.i >> 1);
44  x = convert.x;
45  x = x * (1.5f - xhalf * x * x);
46  return x;
47 }
48 
49 #define b2Sqrt(x) sqrtf(x)
50 #define b2Atan2(y, x) atan2f(y, x)
51 
53 struct b2Vec2
54 {
56  b2Vec2() {}
57 
59  b2Vec2(float32 x, float32 y) : x(x), y(y) {}
60 
62  void SetZero() { x = 0.0f; y = 0.0f; }
63 
65  void Set(float32 x_, float32 y_) { x = x_; y = y_; }
66 
68  b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
69 
71  float32 operator () (int32 i) const
72  {
73  return (&x)[i];
74  }
75 
77  float32& operator () (int32 i)
78  {
79  return (&x)[i];
80  }
81 
83  void operator += (const b2Vec2& v)
84  {
85  x += v.x; y += v.y;
86  }
87 
89  void operator -= (const b2Vec2& v)
90  {
91  x -= v.x; y -= v.y;
92  }
93 
95  void operator *= (float32 a)
96  {
97  x *= a; y *= a;
98  }
99 
101  float32 Length() const
102  {
103  return b2Sqrt(x * x + y * y);
104  }
105 
108  float32 LengthSquared() const
109  {
110  return x * x + y * y;
111  }
112 
114  float32 Normalize()
115  {
116  float32 length = Length();
117  if (length < b2_epsilon)
118  {
119  return 0.0f;
120  }
121  float32 invLength = 1.0f / length;
122  x *= invLength;
123  y *= invLength;
124 
125  return length;
126  }
127 
129  bool IsValid() const
130  {
131  return b2IsValid(x) && b2IsValid(y);
132  }
133 
135  b2Vec2 Skew() const
136  {
137  return b2Vec2(-y, x);
138  }
139 
140  float32 x, y;
141 };
142 
144 struct b2Vec3
145 {
147  b2Vec3() {}
148 
150  b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
151 
153  void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
154 
156  void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
157 
159  b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
160 
162  void operator += (const b2Vec3& v)
163  {
164  x += v.x; y += v.y; z += v.z;
165  }
166 
168  void operator -= (const b2Vec3& v)
169  {
170  x -= v.x; y -= v.y; z -= v.z;
171  }
172 
174  void operator *= (float32 s)
175  {
176  x *= s; y *= s; z *= s;
177  }
178 
179  float32 x, y, z;
180 };
181 
183 struct b2Mat22
184 {
186  b2Mat22() {}
187 
189  b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
190  {
191  ex = c1;
192  ey = c2;
193  }
194 
196  b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
197  {
198  ex.x = a11; ex.y = a21;
199  ey.x = a12; ey.y = a22;
200  }
201 
203  void Set(const b2Vec2& c1, const b2Vec2& c2)
204  {
205  ex = c1;
206  ey = c2;
207  }
208 
210  void SetIdentity()
211  {
212  ex.x = 1.0f; ey.x = 0.0f;
213  ex.y = 0.0f; ey.y = 1.0f;
214  }
215 
217  void SetZero()
218  {
219  ex.x = 0.0f; ey.x = 0.0f;
220  ex.y = 0.0f; ey.y = 0.0f;
221  }
222 
223  b2Mat22 GetInverse() const
224  {
225  float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
226  b2Mat22 B;
227  float32 det = a * d - b * c;
228  if (det != 0.0f)
229  {
230  det = 1.0f / det;
231  }
232  B.ex.x = det * d; B.ey.x = -det * b;
233  B.ex.y = -det * c; B.ey.y = det * a;
234  return B;
235  }
236 
239  b2Vec2 Solve(const b2Vec2& b) const
240  {
241  float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
242  float32 det = a11 * a22 - a12 * a21;
243  if (det != 0.0f)
244  {
245  det = 1.0f / det;
246  }
247  b2Vec2 x;
248  x.x = det * (a22 * b.x - a12 * b.y);
249  x.y = det * (a11 * b.y - a21 * b.x);
250  return x;
251  }
252 
253  b2Vec2 ex, ey;
254 };
255 
257 struct b2Mat33
258 {
260  b2Mat33() {}
261 
263  b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
264  {
265  ex = c1;
266  ey = c2;
267  ez = c3;
268  }
269 
271  void SetZero()
272  {
273  ex.SetZero();
274  ey.SetZero();
275  ez.SetZero();
276  }
277 
280  b2Vec3 Solve33(const b2Vec3& b) const;
281 
285  b2Vec2 Solve22(const b2Vec2& b) const;
286 
289  void GetInverse22(b2Mat33* M) const;
290 
293  void GetSymInverse33(b2Mat33* M) const;
294 
295  b2Vec3 ex, ey, ez;
296 };
297 
299 struct b2Rot
300 {
301  b2Rot() {}
302 
304  explicit b2Rot(float32 angle)
305  {
307  s = sinf(angle);
308  c = cosf(angle);
309  }
310 
312  void Set(float32 angle)
313  {
315  s = sinf(angle);
316  c = cosf(angle);
317  }
318 
320  void SetIdentity()
321  {
322  s = 0.0f;
323  c = 1.0f;
324  }
325 
327  float32 GetAngle() const
328  {
329  return b2Atan2(s, c);
330  }
331 
333  b2Vec2 GetXAxis() const
334  {
335  return b2Vec2(c, s);
336  }
337 
339  b2Vec2 GetYAxis() const
340  {
341  return b2Vec2(-s, c);
342  }
343 
345  float32 s, c;
346 };
347 
351 {
354 
356  b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
357 
359  void SetIdentity()
360  {
361  p.SetZero();
362  q.SetIdentity();
363  }
364 
366  void Set(const b2Vec2& position, float32 angle)
367  {
368  p = position;
369  q.Set(angle);
370  }
371 
372  b2Vec2 p;
373  b2Rot q;
374 };
375 
380 struct b2Sweep
381 {
384  void GetTransform(b2Transform* xfb, float32 beta) const;
385 
388  void Advance(float32 alpha);
389 
391  void Normalize();
392 
393  b2Vec2 localCenter;
394  b2Vec2 c0, c;
395  float32 a0, a;
396 
399  float32 alpha0;
400 };
401 
403 extern const b2Vec2 b2Vec2_zero;
404 
406 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
407 {
408  return a.x * b.x + a.y * b.y;
409 }
410 
412 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
413 {
414  return a.x * b.y - a.y * b.x;
415 }
416 
419 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
420 {
421  return b2Vec2(s * a.y, -s * a.x);
422 }
423 
426 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
427 {
428  return b2Vec2(-s * a.y, s * a.x);
429 }
430 
433 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
434 {
435  return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
436 }
437 
440 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
441 {
442  return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
443 }
444 
446 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
447 {
448  return b2Vec2(a.x + b.x, a.y + b.y);
449 }
450 
452 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
453 {
454  return b2Vec2(a.x - b.x, a.y - b.y);
455 }
456 
457 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
458 {
459  return b2Vec2(s * a.x, s * a.y);
460 }
461 
462 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
463 {
464  return a.x == b.x && a.y == b.y;
465 }
466 
467 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
468 {
469  b2Vec2 c = a - b;
470  return c.Length();
471 }
472 
473 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
474 {
475  b2Vec2 c = a - b;
476  return b2Dot(c, c);
477 }
478 
479 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
480 {
481  return b2Vec3(s * a.x, s * a.y, s * a.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 b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
492 {
493  return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
494 }
495 
497 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
498 {
499  return a.x * b.x + a.y * b.y + a.z * b.z;
500 }
501 
503 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
504 {
505  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);
506 }
507 
508 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
509 {
510  return b2Mat22(A.ex + B.ex, A.ey + B.ey);
511 }
512 
513 // A * B
514 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
515 {
516  return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
517 }
518 
519 // A^T * B
520 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
521 {
522  b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
523  b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
524  return b2Mat22(c1, c2);
525 }
526 
528 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
529 {
530  return v.x * A.ex + v.y * A.ey + v.z * A.ez;
531 }
532 
534 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
535 {
536  return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
537 }
538 
540 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
541 {
542  // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
543  // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
544  // s = qs * rc + qc * rs
545  // c = qc * rc - qs * rs
546  b2Rot qr;
547  qr.s = q.s * r.c + q.c * r.s;
548  qr.c = q.c * r.c - q.s * r.s;
549  return qr;
550 }
551 
553 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
554 {
555  // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
556  // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
557  // s = qc * rs - qs * rc
558  // c = qc * rc + qs * rs
559  b2Rot qr;
560  qr.s = q.c * r.s - q.s * r.c;
561  qr.c = q.c * r.c + q.s * r.s;
562  return qr;
563 }
564 
566 inline b2Vec2 b2Mul(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 
572 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
573 {
574  return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
575 }
576 
577 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
578 {
579  float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
580  float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
581 
582  return b2Vec2(x, y);
583 }
584 
585 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
586 {
587  float32 px = v.x - T.p.x;
588  float32 py = v.y - T.p.y;
589  float32 x = (T.q.c * px + T.q.s * py);
590  float32 y = (-T.q.s * px + T.q.c * py);
591 
592  return b2Vec2(x, y);
593 }
594 
595 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
596 // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
597 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
598 {
599  b2Transform C;
600  C.q = b2Mul(A.q, B.q);
601  C.p = b2Mul(A.q, B.p) + A.p;
602  return C;
603 }
604 
605 // v2 = A.q' * (B.q * v1 + B.p - A.p)
606 // = A.q' * B.q * v1 + A.q' * (B.p - A.p)
607 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
608 {
609  b2Transform C;
610  C.q = b2MulT(A.q, B.q);
611  C.p = b2MulT(A.q, B.p - A.p);
612  return C;
613 }
614 
615 template <typename T>
616 inline T b2Abs(T a)
617 {
618  return a > T(0) ? a : -a;
619 }
620 
621 inline b2Vec2 b2Abs(const b2Vec2& a)
622 {
623  return b2Vec2(b2Abs(a.x), b2Abs(a.y));
624 }
625 
626 inline b2Mat22 b2Abs(const b2Mat22& A)
627 {
628  return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
629 }
630 
631 template <typename T>
632 inline T b2Min(T a, T b)
633 {
634  return a < b ? a : b;
635 }
636 
637 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
638 {
639  return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
640 }
641 
642 template <typename T>
643 inline T b2Max(T a, T b)
644 {
645  return a > b ? a : b;
646 }
647 
648 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
649 {
650  return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
651 }
652 
653 template <typename T>
654 inline T b2Clamp(T a, T low, T high)
655 {
656  return b2Max(low, b2Min(a, high));
657 }
658 
659 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
660 {
661  return b2Max(low, b2Min(a, high));
662 }
663 
664 template<typename T> inline void b2Swap(T& a, T& b)
665 {
666  T tmp = a;
667  a = b;
668  b = tmp;
669 }
670 
676 inline uint32 b2NextPowerOfTwo(uint32 x)
677 {
678  x |= (x >> 1);
679  x |= (x >> 2);
680  x |= (x >> 4);
681  x |= (x >> 8);
682  x |= (x >> 16);
683  return x + 1;
684 }
685 
686 inline bool b2IsPowerOfTwo(uint32 x)
687 {
688  bool result = x > 0 && (x & (x - 1)) == 0;
689  return result;
690 }
691 
692 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
693 {
694  xf->p = (1.0f - beta) * c0 + beta * c;
695  float32 angle = (1.0f - beta) * a0 + beta * a;
696  xf->q.Set(angle);
697 
698  // Shift to origin
699  xf->p -= b2Mul(xf->q, localCenter);
700 }
701 
702 inline void b2Sweep::Advance(float32 alpha)
703 {
704  b2Assert(alpha0 < 1.0f);
705  float32 beta = (alpha - alpha0) / (1.0f - alpha0);
706  c0 += beta * (c - c0);
707  a0 += beta * (a - a0);
708  alpha0 = alpha;
709 }
710 
712 inline void b2Sweep::Normalize()
713 {
714  float32 twoPi = 2.0f * b2_pi;
715  float32 d = twoPi * floorf(a0 / twoPi);
716  a0 -= d;
717  a -= d;
718 }
719 
720 #endif