Box2D  2.2.0
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 
00298         b2Vec3 ex, ey, ez;
00299 };
00300 
00302 struct b2Rot
00303 {
00304         b2Rot() {}
00305 
00307         explicit b2Rot(float32 angle)
00308         {
00310                 s = sinf(angle);
00311                 c = cosf(angle);
00312         }
00313 
00315         void Set(float32 angle)
00316         {
00318                 s = sinf(angle);
00319                 c = cosf(angle);
00320         }
00321 
00323         void SetIdentity()
00324         {
00325                 s = 0.0f;
00326                 c = 1.0f;
00327         }
00328 
00330         float32 GetAngle() const
00331         {
00332                 return b2Atan2(s, c);
00333         }
00334 
00336         b2Vec2 GetXAxis() const
00337         {
00338                 return b2Vec2(c, s);
00339         }
00340 
00342         b2Vec2 GetYAxis() const
00343         {
00344                 return b2Vec2(-s, c);
00345         }
00346 
00348         float32 s, c;
00349 };
00350 
00353 struct b2Transform
00354 {
00356         b2Transform() {}
00357 
00359         b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
00360 
00362         void SetIdentity()
00363         {
00364                 p.SetZero();
00365                 q.SetIdentity();
00366         }
00367 
00369         void Set(const b2Vec2& position, float32 angle)
00370         {
00371                 p = position;
00372                 q.Set(angle);
00373         }
00374 
00375         b2Vec2 p;
00376         b2Rot q;
00377 };
00378 
00383 struct b2Sweep
00384 {
00387         void GetTransform(b2Transform* xfb, float32 beta) const;
00388 
00391         void Advance(float32 alpha);
00392 
00394         void Normalize();
00395 
00396         b2Vec2 localCenter;     
00397         b2Vec2 c0, c;           
00398         float32 a0, a;          
00399 
00402         float32 alpha0;
00403 };
00404 
00406 extern const b2Vec2 b2Vec2_zero;
00407 
00409 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
00410 {
00411         return a.x * b.x + a.y * b.y;
00412 }
00413 
00415 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
00416 {
00417         return a.x * b.y - a.y * b.x;
00418 }
00419 
00422 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
00423 {
00424         return b2Vec2(s * a.y, -s * a.x);
00425 }
00426 
00429 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
00430 {
00431         return b2Vec2(-s * a.y, s * a.x);
00432 }
00433 
00436 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
00437 {
00438         return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
00439 }
00440 
00443 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
00444 {
00445         return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
00446 }
00447 
00449 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
00450 {
00451         return b2Vec2(a.x + b.x, a.y + b.y);
00452 }
00453 
00455 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
00456 {
00457         return b2Vec2(a.x - b.x, a.y - b.y);
00458 }
00459 
00460 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
00461 {
00462         return b2Vec2(s * a.x, s * a.y);
00463 }
00464 
00465 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
00466 {
00467         return a.x == b.x && a.y == b.y;
00468 }
00469 
00470 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
00471 {
00472         b2Vec2 c = a - b;
00473         return c.Length();
00474 }
00475 
00476 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
00477 {
00478         b2Vec2 c = a - b;
00479         return b2Dot(c, c);
00480 }
00481 
00482 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
00483 {
00484         return b2Vec3(s * a.x, s * a.y, s * a.z);
00485 }
00486 
00488 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
00489 {
00490         return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
00491 }
00492 
00494 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
00495 {
00496         return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
00497 }
00498 
00500 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
00501 {
00502         return a.x * b.x + a.y * b.y + a.z * b.z;
00503 }
00504 
00506 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
00507 {
00508         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);
00509 }
00510 
00511 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
00512 {
00513         return b2Mat22(A.ex + B.ex, A.ey + B.ey);
00514 }
00515 
00516 // A * B
00517 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
00518 {
00519         return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
00520 }
00521 
00522 // A^T * B
00523 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
00524 {
00525         b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
00526         b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
00527         return b2Mat22(c1, c2);
00528 }
00529 
00531 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
00532 {
00533         return v.x * A.ex + v.y * A.ey + v.z * A.ez;
00534 }
00535 
00537 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
00538 {
00539         // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
00540         // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
00541         // s = qs * rc + qc * rs
00542         // c = qc * rc - qs * rs
00543         b2Rot qr;
00544         qr.s = q.s * r.c + q.c * r.s;
00545         qr.c = q.c * r.c - q.s * r.s;
00546         return qr;
00547 }
00548 
00550 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
00551 {
00552         // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
00553         // [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
00554         // s = qc * rs - qs * rc
00555         // c = qc * rc + qs * rs
00556         b2Rot qr;
00557         qr.s = q.c * r.s - q.s * r.c;
00558         qr.c = q.c * r.c + q.s * r.s;
00559         return qr;
00560 }
00561 
00563 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
00564 {
00565         return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
00566 }
00567 
00569 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
00570 {
00571         return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
00572 }
00573 
00574 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
00575 {
00576         float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
00577         float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
00578 
00579         return b2Vec2(x, y);
00580 }
00581 
00582 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
00583 {
00584         float32 px = v.x - T.p.x;
00585         float32 py = v.y - T.p.y;
00586         float32 x = (T.q.c * px + T.q.s * py);
00587         float32 y = (-T.q.s * px + T.q.c * py);
00588 
00589         return b2Vec2(x, y);
00590 }
00591 
00592 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
00593 //    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
00594 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
00595 {
00596         b2Transform C;
00597         C.q = b2Mul(A.q, B.q);
00598         C.p = b2Mul(A.q, B.p) + A.p;
00599         return C;
00600 }
00601 
00602 // v2 = A.q' * (B.q * v1 + B.p - A.p)
00603 //    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
00604 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
00605 {
00606         b2Transform C;
00607         C.q = b2MulT(A.q, B.q);
00608         C.p = b2MulT(A.q, B.p - A.p);
00609         return C;
00610 }
00611 
00612 template <typename T>
00613 inline T b2Abs(T a)
00614 {
00615         return a > T(0) ? a : -a;
00616 }
00617 
00618 inline b2Vec2 b2Abs(const b2Vec2& a)
00619 {
00620         return b2Vec2(b2Abs(a.x), b2Abs(a.y));
00621 }
00622 
00623 inline b2Mat22 b2Abs(const b2Mat22& A)
00624 {
00625         return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
00626 }
00627 
00628 template <typename T>
00629 inline T b2Min(T a, T b)
00630 {
00631         return a < b ? a : b;
00632 }
00633 
00634 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
00635 {
00636         return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
00637 }
00638 
00639 template <typename T>
00640 inline T b2Max(T a, T b)
00641 {
00642         return a > b ? a : b;
00643 }
00644 
00645 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
00646 {
00647         return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
00648 }
00649 
00650 template <typename T>
00651 inline T b2Clamp(T a, T low, T high)
00652 {
00653         return b2Max(low, b2Min(a, high));
00654 }
00655 
00656 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
00657 {
00658         return b2Max(low, b2Min(a, high));
00659 }
00660 
00661 template<typename T> inline void b2Swap(T& a, T& b)
00662 {
00663         T tmp = a;
00664         a = b;
00665         b = tmp;
00666 }
00667 
00673 inline uint32 b2NextPowerOfTwo(uint32 x)
00674 {
00675         x |= (x >> 1);
00676         x |= (x >> 2);
00677         x |= (x >> 4);
00678         x |= (x >> 8);
00679         x |= (x >> 16);
00680         return x + 1;
00681 }
00682 
00683 inline bool b2IsPowerOfTwo(uint32 x)
00684 {
00685         bool result = x > 0 && (x & (x - 1)) == 0;
00686         return result;
00687 }
00688 
00689 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
00690 {
00691         xf->p = (1.0f - beta) * c0 + beta * c;
00692         float32 angle = (1.0f - beta) * a0 + beta * a;
00693         xf->q.Set(angle);
00694 
00695         // Shift to origin
00696         xf->p -= b2Mul(xf->q, localCenter);
00697 }
00698 
00699 inline void b2Sweep::Advance(float32 alpha)
00700 {
00701         b2Assert(alpha0 < 1.0f);
00702         float32 beta = (alpha - alpha0) / (1.0f - alpha0);
00703         c0 = (1.0f - beta) * c0 + beta * c;
00704         a0 = (1.0f - beta) * a0 + beta * a;
00705         alpha0 = alpha;
00706 }
00707 
00709 inline void b2Sweep::Normalize()
00710 {
00711         float32 twoPi = 2.0f * b2_pi;
00712         float32 d =  twoPi * floorf(a0 / twoPi);
00713         a0 -= d;
00714         a -= d;
00715 }
00716 
00717 #endif
 All Classes Files Functions Variables Enumerations Enumerator Defines