| Box2D 2.2.1 A 2D Physics Engine for Games

- Main Page
- Classes
- Files

- File List
- File Members

b2Math.h

    00001 /*
    00002 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
    00003 *
    00004 * This software is provided 'as-is', without any express or implied
    00005 * warranty.  In no event will the authors be held liable for any damages
    00006 * arising from the use of this software.
    00007 * Permission is granted to anyone to use this software for any purpose,
    00008 * including commercial applications, and to alter it and redistribute it
    00009 * freely, subject to the following restrictions:
    00010 * 1. The origin of this software must not be misrepresented; you must not
    00011 * claim that you wrote the original software. If you use this software
    00012 * in a product, an acknowledgment in the product documentation would be
    00013 * appreciated but is not required.
    00014 * 2. Altered source versions must be plainly marked as such, and must not be
    00015 * misrepresented as being the original software.
    00016 * 3. This notice may not be removed or altered from any source distribution.
    00017 */
    00018
    00019 #ifndef B2_MATH_H
    00020 #define B2_MATH_H
    00021
    00022 #include <Box2D/Common/b2Settings.h>
    00023
    00024 #include <cmath>
    00025 #include <cfloat>
    00026 #include <cstddef>
    00027 #include <limits>
    00028
    00031 inline bool b2IsValid(float32 x)
    00032 {
    00033         if (x != x)
    00034         {
    00035                 // NaN.
    00036                 return false;
    00037         }
    00038
    00039         float32 infinity = std::numeric_limits<float32>::infinity();
    00040         return -infinity < x && x < infinity;
    00041 }
    00042
    00044 inline float32 b2InvSqrt(float32 x)
    00045 {
    00046         union
    00047         {
    00048                 float32 x;
    00049                 int32 i;
    00050         } convert;
    00051
    00052         convert.x = x;
    00053         float32 xhalf = 0.5f * x;
    00054         convert.i = 0x5f3759df - (convert.i >> 1);
    00055         x = convert.x;
    00056         x = x * (1.5f - xhalf * x * x);
    00057         return x;
    00058 }
    00059
    00060 #define b2Sqrt(x)       std::sqrt(x)
    00061 #define b2Atan2(y, x)   std::atan2(y, x)
    00062
    00064 struct b2Vec2
    00065 {
    00067         b2Vec2() {}
    00068
    00070         b2Vec2(float32 x, float32 y) : x(x), y(y) {}
    00071
    00073         void SetZero() { x = 0.0f; y = 0.0f; }
    00074
    00076         void Set(float32 x_, float32 y_) { x = x_; y = y_; }
    00077
    00079         b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
    00080
    00082         float32 operator () (int32 i) const
    00083         {
    00084                 return (&x)[i];
    00085         }
    00086
    00088         float32& operator () (int32 i)
    00089         {
    00090                 return (&x)[i];
    00091         }
    00092
    00094         void operator += (const b2Vec2& v)
    00095         {
    00096                 x += v.x; y += v.y;
    00097         }
    00098
    00100         void operator -= (const b2Vec2& v)
    00101         {
    00102                 x -= v.x; y -= v.y;
    00103         }
    00104
    00106         void operator *= (float32 a)
    00107         {
    00108                 x *= a; y *= a;
    00109         }
    00110
    00112         float32 Length() const
    00113         {
    00114                 return b2Sqrt(x * x + y * y);
    00115         }
    00116
    00119         float32 LengthSquared() const
    00120         {
    00121                 return x * x + y * y;
    00122         }
    00123
    00125         float32 Normalize()
    00126         {
    00127                 float32 length = Length();
    00128                 if (length < b2_epsilon)
    00129                 {
    00130                         return 0.0f;
    00131                 }
    00132                 float32 invLength = 1.0f / length;
    00133                 x *= invLength;
    00134                 y *= invLength;
    00135
    00136                 return length;
    00137         }
    00138
    00140         bool IsValid() const
    00141         {
    00142                 return b2IsValid(x) && b2IsValid(y);
    00143         }
    00144
    00146         b2Vec2 Skew() const
    00147         {
    00148                 return b2Vec2(-y, x);
    00149         }
    00150
    00151         float32 x, y;
    00152 };
    00153
    00155 struct b2Vec3
    00156 {
    00158         b2Vec3() {}
    00159
    00161         b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
    00162
    00164         void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
    00165
    00167         void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
    00168
    00170         b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
    00171
    00173         void operator += (const b2Vec3& v)
    00174         {
    00175                 x += v.x; y += v.y; z += v.z;
    00176         }
    00177
    00179         void operator -= (const b2Vec3& v)
    00180         {
    00181                 x -= v.x; y -= v.y; z -= v.z;
    00182         }
    00183
    00185         void operator *= (float32 s)
    00186         {
    00187                 x *= s; y *= s; z *= s;
    00188         }
    00189
    00190         float32 x, y, z;
    00191 };
    00192
    00194 struct b2Mat22
    00195 {
    00197         b2Mat22() {}
    00198
    00200         b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
    00201         {
    00202                 ex = c1;
    00203                 ey = c2;
    00204         }
    00205
    00207         b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
    00208         {
    00209                 ex.x = a11; ex.y = a21;
    00210                 ey.x = a12; ey.y = a22;
    00211         }
    00212
    00214         void Set(const b2Vec2& c1, const b2Vec2& c2)
    00215         {
    00216                 ex = c1;
    00217                 ey = c2;
    00218         }
    00219
    00221         void SetIdentity()
    00222         {
    00223                 ex.x = 1.0f; ey.x = 0.0f;
    00224                 ex.y = 0.0f; ey.y = 1.0f;
    00225         }
    00226
    00228         void SetZero()
    00229         {
    00230                 ex.x = 0.0f; ey.x = 0.0f;
    00231                 ex.y = 0.0f; ey.y = 0.0f;
    00232         }
    00233
    00234         b2Mat22 GetInverse() const
    00235         {
    00236                 float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
    00237                 b2Mat22 B;
    00238                 float32 det = a * d - b * c;
    00239                 if (det != 0.0f)
    00240                 {
    00241                         det = 1.0f / det;
    00242                 }
    00243                 B.ex.x =  det * d;      B.ey.x = -det * b;
    00244                 B.ex.y = -det * c;      B.ey.y =  det * a;
    00245                 return B;
    00246         }
    00247
    00250         b2Vec2 Solve(const b2Vec2& b) const
    00251         {
    00252                 float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
    00253                 float32 det = a11 * a22 - a12 * a21;
    00254                 if (det != 0.0f)
    00255                 {
    00256                         det = 1.0f / det;
    00257                 }
    00258                 b2Vec2 x;
    00259                 x.x = det * (a22 * b.x - a12 * b.y);
    00260                 x.y = det * (a11 * b.y - a21 * b.x);
    00261                 return x;
    00262         }
    00263
    00264         b2Vec2 ex, ey;
    00265 };
    00266
    00268 struct b2Mat33
    00269 {
    00271         b2Mat33() {}
    00272
    00274         b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
    00275         {
    00276                 ex = c1;
    00277                 ey = c2;
    00278                 ez = c3;
    00279         }
    00280
    00282         void SetZero()
    00283         {
    00284                 ex.SetZero();
    00285                 ey.SetZero();
    00286                 ez.SetZero();
    00287         }
    00288
    00291         b2Vec3 Solve33(const b2Vec3& b) const;
    00292
    00296         b2Vec2 Solve22(const b2Vec2& b) const;
    00297
    00300         void GetInverse22(b2Mat33* M) const;
    00301
    00304         void GetSymInverse33(b2Mat33* M) const;
    00305
    00306         b2Vec3 ex, ey, ez;
    00307 };
    00308
    00310 struct b2Rot
    00311 {
    00312         b2Rot() {}
    00313
    00315         explicit b2Rot(float32 angle)
    00316         {
    00318                 s = sinf(angle);
    00319                 c = cosf(angle);
    00320         }
    00321
    00323         void Set(float32 angle)
    00324         {
    00326                 s = sinf(angle);
    00327                 c = cosf(angle);
    00328         }
    00329
    00331         void SetIdentity()
    00332         {
    00333                 s = 0.0f;
    00334                 c = 1.0f;
    00335         }
    00336
    00338         float32 GetAngle() const
    00339         {
    00340                 return b2Atan2(s, c);
    00341         }
    00342
    00344         b2Vec2 GetXAxis() const
    00345         {
    00346                 return b2Vec2(c, s);
    00347         }
    00348
    00350         b2Vec2 GetYAxis() const
    00351         {
    00352                 return b2Vec2(-s, c);
    00353         }
    00354
    00356         float32 s, c;
    00357 };
    00358
    00361 struct b2Transform
    00362 {
    00364         b2Transform() {}
    00365
    00367         b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
    00368
    00370         void SetIdentity()
    00371         {
    00372                 p.SetZero();
    00373                 q.SetIdentity();
    00374         }
    00375
    00377         void Set(const b2Vec2& position, float32 angle)
    00378         {
    00379                 p = position;
    00380                 q.Set(angle);
    00381         }
    00382
    00383         b2Vec2 p;
    00384         b2Rot q;
    00385 };
    00386
    00391 struct b2Sweep
    00392 {
    00395         void GetTransform(b2Transform* xfb, float32 beta) const;
    00396
    00399         void Advance(float32 alpha);
    00400
    00402         void Normalize();
    00403
    00404         b2Vec2 localCenter;
    00405         b2Vec2 c0, c;
    00406         float32 a0, a;
    00407
    00410         float32 alpha0;
    00411 };
    00412
    00414 extern const b2Vec2 b2Vec2_zero;
    00415
    00417 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
    00418 {
    00419         return a.x * b.x + a.y * b.y;
    00420 }
    00421
    00423 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
    00424 {
    00425         return a.x * b.y - a.y * b.x;
    00426 }
    00427
    00430 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
    00431 {
    00432         return b2Vec2(s * a.y, -s * a.x);
    00433 }
    00434
    00437 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
    00438 {
    00439         return b2Vec2(-s * a.y, s * a.x);
    00440 }
    00441
    00444 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
    00445 {
    00446         return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
    00447 }
    00448
    00451 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
    00452 {
    00453         return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
    00454 }
    00455
    00457 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
    00458 {
    00459         return b2Vec2(a.x + b.x, a.y + b.y);
    00460 }
    00461
    00463 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
    00464 {
    00465         return b2Vec2(a.x - b.x, a.y - b.y);
    00466 }
    00467
    00468 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
    00469 {
    00470         return b2Vec2(s * a.x, s * a.y);
    00471 }
    00472
    00473 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
    00474 {
    00475         return a.x == b.x && a.y == b.y;
    00476 }
    00477
    00478 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
    00479 {
    00480         b2Vec2 c = a - b;
    00481         return c.Length();
    00482 }
    00483
    00484 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
    00485 {
    00486         b2Vec2 c = a - b;
    00487         return b2Dot(c, c);
    00488 }
    00489
    00490 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
    00491 {
    00492         return b2Vec3(s * a.x, s * a.y, s * a.z);
    00493 }
    00494
    00496 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
    00497 {
    00498         return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
    00499 }
    00500
    00502 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
    00503 {
    00504         return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
    00505 }
    00506
    00508 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
    00509 {
    00510         return a.x * b.x + a.y * b.y + a.z * b.z;
    00511 }
    00512
    00514 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
    00515 {
    00516         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);
    00517 }
    00518
    00519 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
    00520 {
    00521         return b2Mat22(A.ex + B.ex, A.ey + B.ey);
    00522 }
    00523
    00524 // A * B
    00525 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
    00526 {
    00527         return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
    00528 }
    00529
    00530 // A^T * B
    00531 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
    00532 {
    00533         b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
    00534         b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
    00535         return b2Mat22(c1, c2);
    00536 }
    00537
    00539 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
    00540 {
    00541         return v.x * A.ex + v.y * A.ey + v.z * A.ez;
    00542 }
    00543
    00545 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
    00546 {
    00547         return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
    00548 }
    00549
    00551 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
    00552 {
    00553         // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
    00554         // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
    00555         // s = qs * rc + qc * rs
    00556         // c = qc * rc - qs * rs
    00557         b2Rot qr;
    00558         qr.s = q.s * r.c + q.c * r.s;
    00559         qr.c = q.c * r.c - q.s * r.s;
    00560         return qr;
    00561 }
    00562
    00564 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
    00565 {
    00566         // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
    00567         // [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
    00568         // s = qc * rs - qs * rc
    00569         // c = qc * rc + qs * rs
    00570         b2Rot qr;
    00571         qr.s = q.c * r.s - q.s * r.c;
    00572         qr.c = q.c * r.c + q.s * r.s;
    00573         return qr;
    00574 }
    00575
    00577 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
    00578 {
    00579         return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
    00580 }
    00581
    00583 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
    00584 {
    00585         return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
    00586 }
    00587
    00588 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
    00589 {
    00590         float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
    00591         float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
    00592
    00593         return b2Vec2(x, y);
    00594 }
    00595
    00596 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
    00597 {
    00598         float32 px = v.x - T.p.x;
    00599         float32 py = v.y - T.p.y;
    00600         float32 x = (T.q.c * px + T.q.s * py);
    00601         float32 y = (-T.q.s * px + T.q.c * py);
    00602
    00603         return b2Vec2(x, y);
    00604 }
    00605
    00606 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
    00607 //    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
    00608 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
    00609 {
    00610         b2Transform C;
    00611         C.q = b2Mul(A.q, B.q);
    00612         C.p = b2Mul(A.q, B.p) + A.p;
    00613         return C;
    00614 }
    00615
    00616 // v2 = A.q' * (B.q * v1 + B.p - A.p)
    00617 //    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
    00618 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
    00619 {
    00620         b2Transform C;
    00621         C.q = b2MulT(A.q, B.q);
    00622         C.p = b2MulT(A.q, B.p - A.p);
    00623         return C;
    00624 }
    00625
    00626 template <typename T>
    00627 inline T b2Abs(T a)
    00628 {
    00629         return a > T(0) ? a : -a;
    00630 }
    00631
    00632 inline b2Vec2 b2Abs(const b2Vec2& a)
    00633 {
    00634         return b2Vec2(b2Abs(a.x), b2Abs(a.y));
    00635 }
    00636
    00637 inline b2Mat22 b2Abs(const b2Mat22& A)
    00638 {
    00639         return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
    00640 }
    00641
    00642 template <typename T>
    00643 inline T b2Min(T a, T b)
    00644 {
    00645         return a < b ? a : b;
    00646 }
    00647
    00648 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
    00649 {
    00650         return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
    00651 }
    00652
    00653 template <typename T>
    00654 inline T b2Max(T a, T b)
    00655 {
    00656         return a > b ? a : b;
    00657 }
    00658
    00659 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
    00660 {
    00661         return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
    00662 }
    00663
    00664 template <typename T>
    00665 inline T b2Clamp(T a, T low, T high)
    00666 {
    00667         return b2Max(low, b2Min(a, high));
    00668 }
    00669
    00670 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
    00671 {
    00672         return b2Max(low, b2Min(a, high));
    00673 }
    00674
    00675 template<typename T> inline void b2Swap(T& a, T& b)
    00676 {
    00677         T tmp = a;
    00678         a = b;
    00679         b = tmp;
    00680 }
    00681
    00687 inline uint32 b2NextPowerOfTwo(uint32 x)
    00688 {
    00689         x |= (x >> 1);
    00690         x |= (x >> 2);
    00691         x |= (x >> 4);
    00692         x |= (x >> 8);
    00693         x |= (x >> 16);
    00694         return x + 1;
    00695 }
    00696
    00697 inline bool b2IsPowerOfTwo(uint32 x)
    00698 {
    00699         bool result = x > 0 && (x & (x - 1)) == 0;
    00700         return result;
    00701 }
    00702
    00703 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
    00704 {
    00705         xf->p = (1.0f - beta) * c0 + beta * c;
    00706         float32 angle = (1.0f - beta) * a0 + beta * a;
    00707         xf->q.Set(angle);
    00708
    00709         // Shift to origin
    00710         xf->p -= b2Mul(xf->q, localCenter);
    00711 }
    00712
    00713 inline void b2Sweep::Advance(float32 alpha)
    00714 {
    00715         b2Assert(alpha0 < 1.0f);
    00716         float32 beta = (alpha - alpha0) / (1.0f - alpha0);
    00717         c0 = (1.0f - beta) * c0 + beta * c;
    00718         a0 = (1.0f - beta) * a0 + beta * a;
    00719         alpha0 = alpha;
    00720 }
    00721
    00723 inline void b2Sweep::Normalize()
    00724 {
    00725         float32 twoPi = 2.0f * b2_pi;
    00726         float32 d =  twoPi * floorf(a0 / twoPi);
    00727         a0 -= d;
    00728         a -= d;
    00729 }
    00730
    00731 #endif

All Classes Files Functions Variables Enumerations Enumerator Defines