LiquidFun
 All Classes Files Functions Variables Enumerations Enumerator Macros Pages
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 
24 #include <cmath>
25 #include <cfloat>
26 #include <cstddef>
27 #include <limits>
28 
31 inline bool b2IsValid(float32 x)
32 {
33  if (x != x)
34  {
35  // NaN.
36  return false;
37  }
38 
39  float32 infinity = std::numeric_limits<float32>::infinity();
40  return -infinity < x && x < infinity;
41 }
42 
44 inline float32 b2InvSqrt(float32 x)
45 {
46  union
47  {
48  float32 x;
49  int32 i;
50  } convert;
51 
52  convert.x = x;
53  float32 xhalf = 0.5f * x;
54  convert.i = 0x5f3759df - (convert.i >> 1);
55  x = convert.x;
56  x = x * (1.5f - xhalf * x * x);
57  return x;
58 }
59 
60 #define b2Sqrt(x) std::sqrt(x)
61 #define b2Atan2(y, x) std::atan2(y, x)
62 
64 struct b2Vec2
65 {
67  b2Vec2() {}
68 
70  b2Vec2(float32 x, float32 y) : x(x), y(y) {}
71 
73  void SetZero() { x = 0.0f; y = 0.0f; }
74 
76  void Set(float32 x_, float32 y_) { x = x_; y = y_; }
77 
79  b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
80 
82  float32 operator () (int32 i) const
83  {
84  return (&x)[i];
85  }
86 
88  float32& operator () (int32 i)
89  {
90  return (&x)[i];
91  }
92 
94  void operator += (const b2Vec2& v)
95  {
96  x += v.x; y += v.y;
97  }
98 
100  void operator -= (const b2Vec2& v)
101  {
102  x -= v.x; y -= v.y;
103  }
104 
106  void operator *= (float32 a)
107  {
108  x *= a; y *= a;
109  }
110 
112  float32 Length() const
113  {
114  return b2Sqrt(x * x + y * y);
115  }
116 
119  float32 LengthSquared() const
120  {
121  return x * x + y * y;
122  }
123 
125  float32 Normalize()
126  {
127  float32 length = Length();
128  if (length < b2_epsilon)
129  {
130  return 0.0f;
131  }
132  float32 invLength = 1.0f / length;
133  x *= invLength;
134  y *= invLength;
135 
136  return length;
137  }
138 
140  bool IsValid() const
141  {
142  return b2IsValid(x) && b2IsValid(y);
143  }
144 
146  b2Vec2 Skew() const
147  {
148  return b2Vec2(-y, x);
149  }
150 
151  float32 x, y;
152 };
153 
155 struct b2Vec3
156 {
158  b2Vec3() {}
159 
161  b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
162 
164  void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
165 
167  void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
168 
170  b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
171 
173  void operator += (const b2Vec3& v)
174  {
175  x += v.x; y += v.y; z += v.z;
176  }
177 
179  void operator -= (const b2Vec3& v)
180  {
181  x -= v.x; y -= v.y; z -= v.z;
182  }
183 
185  void operator *= (float32 s)
186  {
187  x *= s; y *= s; z *= s;
188  }
189 
190  float32 x, y, z;
191 };
192 
194 struct b2Mat22
195 {
197  b2Mat22() {}
198 
200  b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
201  {
202  ex = c1;
203  ey = c2;
204  }
205 
207  b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
208  {
209  ex.x = a11; ex.y = a21;
210  ey.x = a12; ey.y = a22;
211  }
212 
214  void Set(const b2Vec2& c1, const b2Vec2& c2)
215  {
216  ex = c1;
217  ey = c2;
218  }
219 
221  void SetIdentity()
222  {
223  ex.x = 1.0f; ey.x = 0.0f;
224  ex.y = 0.0f; ey.y = 1.0f;
225  }
226 
228  void SetZero()
229  {
230  ex.x = 0.0f; ey.x = 0.0f;
231  ex.y = 0.0f; ey.y = 0.0f;
232  }
233 
234  b2Mat22 GetInverse() const
235  {
236  float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
237  b2Mat22 B;
238  float32 det = a * d - b * c;
239  if (det != 0.0f)
240  {
241  det = 1.0f / det;
242  }
243  B.ex.x = det * d; B.ey.x = -det * b;
244  B.ex.y = -det * c; B.ey.y = det * a;
245  return B;
246  }
247 
250  b2Vec2 Solve(const b2Vec2& b) const
251  {
252  float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
253  float32 det = a11 * a22 - a12 * a21;
254  if (det != 0.0f)
255  {
256  det = 1.0f / det;
257  }
258  b2Vec2 x;
259  x.x = det * (a22 * b.x - a12 * b.y);
260  x.y = det * (a11 * b.y - a21 * b.x);
261  return x;
262  }
263 
264  b2Vec2 ex, ey;
265 };
266 
268 struct b2Mat33
269 {
271  b2Mat33() {}
272 
274  b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
275  {
276  ex = c1;
277  ey = c2;
278  ez = c3;
279  }
280 
282  void SetZero()
283  {
284  ex.SetZero();
285  ey.SetZero();
286  ez.SetZero();
287  }
288 
291  b2Vec3 Solve33(const b2Vec3& b) const;
292 
296  b2Vec2 Solve22(const b2Vec2& b) const;
297 
300  void GetInverse22(b2Mat33* M) const;
301 
304  void GetSymInverse33(b2Mat33* M) const;
305 
306  b2Vec3 ex, ey, ez;
307 };
308 
310 struct b2Rot
311 {
312  b2Rot() {}
313 
315  explicit b2Rot(float32 angle)
316  {
318  s = sinf(angle);
319  c = cosf(angle);
320  }
321 
323  void Set(float32 angle)
324  {
326  s = sinf(angle);
327  c = cosf(angle);
328  }
329 
331  void SetIdentity()
332  {
333  s = 0.0f;
334  c = 1.0f;
335  }
336 
338  float32 GetAngle() const
339  {
340  return b2Atan2(s, c);
341  }
342 
344  b2Vec2 GetXAxis() const
345  {
346  return b2Vec2(c, s);
347  }
348 
350  b2Vec2 GetYAxis() const
351  {
352  return b2Vec2(-s, c);
353  }
354 
356  float32 s, c;
357 };
358 
362 {
365 
367  b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
368 
370  void SetIdentity()
371  {
372  p.SetZero();
373  q.SetIdentity();
374  }
375 
377  void Set(const b2Vec2& position, float32 angle)
378  {
379  p = position;
380  q.Set(angle);
381  }
382 
383  b2Vec2 p;
384  b2Rot q;
385 };
386 
391 struct b2Sweep
392 {
395  void GetTransform(b2Transform* xfb, float32 beta) const;
396 
399  void Advance(float32 alpha);
400 
402  void Normalize();
403 
405  b2Vec2 c0, c;
406  float32 a0, a;
407 
410  float32 alpha0;
411 };
412 
414 extern const b2Vec2 b2Vec2_zero;
415 
417 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
418 {
419  return a.x * b.x + a.y * b.y;
420 }
421 
423 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
424 {
425  return a.x * b.y - a.y * b.x;
426 }
427 
430 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
431 {
432  return b2Vec2(s * a.y, -s * a.x);
433 }
434 
437 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
438 {
439  return b2Vec2(-s * a.y, s * a.x);
440 }
441 
444 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
445 {
446  return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
447 }
448 
451 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
452 {
453  return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
454 }
455 
457 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
458 {
459  return b2Vec2(a.x + b.x, a.y + b.y);
460 }
461 
463 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
464 {
465  return b2Vec2(a.x - b.x, a.y - b.y);
466 }
467 
468 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
469 {
470  return b2Vec2(s * a.x, s * a.y);
471 }
472 
473 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
474 {
475  return a.x == b.x && a.y == b.y;
476 }
477 
478 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
479 {
480  b2Vec2 c = a - b;
481  return c.Length();
482 }
483 
484 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
485 {
486  b2Vec2 c = a - b;
487  return b2Dot(c, c);
488 }
489 
490 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
491 {
492  return b2Vec3(s * a.x, s * a.y, s * a.z);
493 }
494 
496 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
497 {
498  return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
499 }
500 
502 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
503 {
504  return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
505 }
506 
508 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
509 {
510  return a.x * b.x + a.y * b.y + a.z * b.z;
511 }
512 
514 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
515 {
516  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);
517 }
518 
519 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
520 {
521  return b2Mat22(A.ex + B.ex, A.ey + B.ey);
522 }
523 
524 // A * B
525 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
526 {
527  return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
528 }
529 
530 // A^T * B
531 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
532 {
533  b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
534  b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
535  return b2Mat22(c1, c2);
536 }
537 
539 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
540 {
541  return v.x * A.ex + v.y * A.ey + v.z * A.ez;
542 }
543 
545 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
546 {
547  return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
548 }
549 
551 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
552 {
553  // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
554  // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
555  // s = qs * rc + qc * rs
556  // c = qc * rc - qs * rs
557  b2Rot qr;
558  qr.s = q.s * r.c + q.c * r.s;
559  qr.c = q.c * r.c - q.s * r.s;
560  return qr;
561 }
562 
564 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
565 {
566  // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
567  // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
568  // s = qc * rs - qs * rc
569  // c = qc * rc + qs * rs
570  b2Rot qr;
571  qr.s = q.c * r.s - q.s * r.c;
572  qr.c = q.c * r.c + q.s * r.s;
573  return qr;
574 }
575 
577 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
578 {
579  return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
580 }
581 
583 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
584 {
585  return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
586 }
587 
588 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
589 {
590  float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
591  float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
592 
593  return b2Vec2(x, y);
594 }
595 
596 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
597 {
598  float32 px = v.x - T.p.x;
599  float32 py = v.y - T.p.y;
600  float32 x = (T.q.c * px + T.q.s * py);
601  float32 y = (-T.q.s * px + T.q.c * py);
602 
603  return b2Vec2(x, y);
604 }
605 
606 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
607 // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
608 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
609 {
610  b2Transform C;
611  C.q = b2Mul(A.q, B.q);
612  C.p = b2Mul(A.q, B.p) + A.p;
613  return C;
614 }
615 
616 // v2 = A.q' * (B.q * v1 + B.p - A.p)
617 // = A.q' * B.q * v1 + A.q' * (B.p - A.p)
618 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
619 {
620  b2Transform C;
621  C.q = b2MulT(A.q, B.q);
622  C.p = b2MulT(A.q, B.p - A.p);
623  return C;
624 }
625 
626 template <typename T>
627 inline T b2Abs(T a)
628 {
629  return a > T(0) ? a : -a;
630 }
631 
632 inline b2Vec2 b2Abs(const b2Vec2& a)
633 {
634  return b2Vec2(b2Abs(a.x), b2Abs(a.y));
635 }
636 
637 inline b2Mat22 b2Abs(const b2Mat22& A)
638 {
639  return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
640 }
641 
642 template <typename T>
643 inline T b2Min(T a, T b)
644 {
645  return a < b ? a : b;
646 }
647 
648 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
649 {
650  return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
651 }
652 
653 template <typename T>
654 inline T b2Max(T a, T b)
655 {
656  return a > b ? a : b;
657 }
658 
659 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
660 {
661  return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
662 }
663 
664 template <typename T>
665 inline T b2Clamp(T a, T low, T high)
666 {
667  return b2Max(low, b2Min(a, high));
668 }
669 
670 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
671 {
672  return b2Max(low, b2Min(a, high));
673 }
674 
675 template<typename T> inline void b2Swap(T& a, T& b)
676 {
677  T tmp = a;
678  a = b;
679  b = tmp;
680 }
681 
687 inline uint32 b2NextPowerOfTwo(uint32 x)
688 {
689  x |= (x >> 1);
690  x |= (x >> 2);
691  x |= (x >> 4);
692  x |= (x >> 8);
693  x |= (x >> 16);
694  return x + 1;
695 }
696 
697 inline bool b2IsPowerOfTwo(uint32 x)
698 {
699  bool result = x > 0 && (x & (x - 1)) == 0;
700  return result;
701 }
702 
703 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
704 {
705  xf->p = (1.0f - beta) * c0 + beta * c;
706  float32 angle = (1.0f - beta) * a0 + beta * a;
707  xf->q.Set(angle);
708 
709  // Shift to origin
710  xf->p -= b2Mul(xf->q, localCenter);
711 }
712 
713 inline void b2Sweep::Advance(float32 alpha)
714 {
715  b2Assert(alpha0 < 1.0f);
716  float32 beta = (alpha - alpha0) / (1.0f - alpha0);
717  c0 += beta * (c - c0);
718  a0 += beta * (a - a0);
719  alpha0 = alpha;
720 }
721 
723 inline void b2Sweep::Normalize()
724 {
725  float32 twoPi = 2.0f * b2_pi;
726  float32 d = twoPi * floorf(a0 / twoPi);
727  a0 -= d;
728  a -= d;
729 }
730 
731 #endif
b2Vec2 Solve22(const b2Vec2 &b) const
Definition: b2Math.cpp:41
Definition: b2Math.h:361
b2Transform()
The default constructor does nothing.
Definition: b2Math.h:364
b2Vec3(float32 x, float32 y, float32 z)
Construct using coordinates.
Definition: b2Math.h:161
void GetTransform(b2Transform *xfb, float32 beta) const
Definition: b2Math.h:703
A 2D column vector with 3 elements.
Definition: b2Math.h:155
float32 GetAngle() const
Get the angle in radians.
Definition: b2Math.h:338
bool IsValid() const
Does this vector contain finite coordinates?
Definition: b2Math.h:140
b2Mat22(const b2Vec2 &c1, const b2Vec2 &c2)
Construct this matrix using columns.
Definition: b2Math.h:200
b2Vec3()
Default constructor does nothing (for performance).
Definition: b2Math.h:158
A 3-by-3 matrix. Stored in column-major order.
Definition: b2Math.h:268
b2Vec2()
Default constructor does nothing (for performance).
Definition: b2Math.h:67
void Set(float32 x_, float32 y_, float32 z_)
Set this vector to some specified coordinates.
Definition: b2Math.h:167
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:125
b2Vec2 GetYAxis() const
Get the u-axis.
Definition: b2Math.h:350
b2Mat33()
The default constructor does nothing (for performance).
Definition: b2Math.h:271
b2Vec2 operator-() const
Negate this vector.
Definition: b2Math.h:79
b2Vec2(float32 x, float32 y)
Construct using coordinates.
Definition: b2Math.h:70
Definition: b2Math.h:391
b2Mat22()
The default constructor does nothing (for performance).
Definition: b2Math.h:197
float32 s
Sine and cosine.
Definition: b2Math.h:356
float32 a
world angles
Definition: b2Math.h:406
b2Mat33(const b2Vec3 &c1, const b2Vec3 &c2, const b2Vec3 &c3)
Construct this matrix using columns.
Definition: b2Math.h:274
b2Vec2 Skew() const
Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
Definition: b2Math.h:146
b2Vec2 GetXAxis() const
Get the x-axis.
Definition: b2Math.h:344
b2Vec3 operator-() const
Negate this vector.
Definition: b2Math.h:170
void Set(float32 x_, float32 y_)
Set this vector to some specified coordinates.
Definition: b2Math.h:76
void Set(const b2Vec2 &position, float32 angle)
Set this based on the position and angle.
Definition: b2Math.h:377
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:73
void operator+=(const b2Vec2 &v)
Add a vector to this vector.
Definition: b2Math.h:94
void GetInverse22(b2Mat33 *M) const
Definition: b2Math.cpp:56
void operator*=(float32 s)
Multiply this vector by a scalar.
Definition: b2Math.h:185
void SetIdentity()
Set this to the identity transform.
Definition: b2Math.h:370
void operator-=(const b2Vec2 &v)
Subtract a vector from this vector.
Definition: b2Math.h:100
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:164
void operator-=(const b2Vec3 &v)
Subtract a vector from this vector.
Definition: b2Math.h:179
b2Rot(float32 angle)
Initialize from an angle in radians.
Definition: b2Math.h:315
void operator+=(const b2Vec3 &v)
Add a vector to this vector.
Definition: b2Math.h:173
b2Vec2 Solve(const b2Vec2 &b) const
Definition: b2Math.h:250
b2Vec2 c
center world positions
Definition: b2Math.h:405
float32 Length() const
Get the length of this vector (the norm).
Definition: b2Math.h:112
b2Vec2 localCenter
local center of mass position
Definition: b2Math.h:404
void GetSymInverse33(b2Mat33 *M) const
Returns the zero matrix if singular.
Definition: b2Math.cpp:71
A 2-by-2 matrix. Stored in column-major order.
Definition: b2Math.h:194
void Set(const b2Vec2 &c1, const b2Vec2 &c2)
Initialize this matrix using columns.
Definition: b2Math.h:214
void SetIdentity()
Set to the identity rotation.
Definition: b2Math.h:331
float32 alpha0
Definition: b2Math.h:410
void SetIdentity()
Set this to the identity matrix.
Definition: b2Math.h:221
void Normalize()
Normalize the angles.
Definition: b2Math.h:723
b2Vec3 Solve33(const b2Vec3 &b) const
Definition: b2Math.cpp:25
void SetZero()
Set this matrix to all zeros.
Definition: b2Math.h:228
A 2D column vector.
Definition: b2Math.h:64
void Advance(float32 alpha)
Definition: b2Math.h:713
float32 LengthSquared() const
Definition: b2Math.h:119
void operator*=(float32 a)
Multiply this vector by a scalar.
Definition: b2Math.h:106
void Set(float32 angle)
Set using an angle in radians.
Definition: b2Math.h:323
b2Transform(const b2Vec2 &position, const b2Rot &rotation)
Initialize using a position vector and a rotation.
Definition: b2Math.h:367
void SetZero()
Set this matrix to all zeros.
Definition: b2Math.h:282
b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
Construct this matrix using scalars.
Definition: b2Math.h:207
Rotation.
Definition: b2Math.h:310
float32 operator()(int32 i) const
Read from and indexed element.
Definition: b2Math.h:82