Box2D  2.2.1
A 2D Physics Engine for Games
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