b2Math.h
Go to the documentation of this file.
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 #include <math.h>
00024 
00026 inline bool b2IsValid(float32 x)
00027 {
00028         int32 ix = *reinterpret_cast<int32*>(&x);
00029         return (ix & 0x7f800000) != 0x7f800000;
00030 }
00031 
00033 inline float32 b2InvSqrt(float32 x)
00034 {
00035         union
00036         {
00037                 float32 x;
00038                 int32 i;
00039         } convert;
00040 
00041         convert.x = x;
00042         float32 xhalf = 0.5f * x;
00043         convert.i = 0x5f3759df - (convert.i >> 1);
00044         x = convert.x;
00045         x = x * (1.5f - xhalf * x * x);
00046         return x;
00047 }
00048 
00049 #define b2Sqrt(x)       sqrtf(x)
00050 #define b2Atan2(y, x)   atan2f(y, x)
00051 
00053 struct b2Vec2
00054 {
00056         b2Vec2() {}
00057 
00059         b2Vec2(float32 x, float32 y) : x(x), y(y) {}
00060 
00062         void SetZero() { x = 0.0f; y = 0.0f; }
00063 
00065         void Set(float32 x_, float32 y_) { x = x_; y = y_; }
00066 
00068         b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
00069         
00071         float32 operator () (int32 i) const
00072         {
00073                 return (&x)[i];
00074         }
00075 
00077         float32& operator () (int32 i)
00078         {
00079                 return (&x)[i];
00080         }
00081 
00083         void operator += (const b2Vec2& v)
00084         {
00085                 x += v.x; y += v.y;
00086         }
00087         
00089         void operator -= (const b2Vec2& v)
00090         {
00091                 x -= v.x; y -= v.y;
00092         }
00093 
00095         void operator *= (float32 a)
00096         {
00097                 x *= a; y *= a;
00098         }
00099 
00101         float32 Length() const
00102         {
00103                 return b2Sqrt(x * x + y * y);
00104         }
00105 
00108         float32 LengthSquared() const
00109         {
00110                 return x * x + y * y;
00111         }
00112 
00114         float32 Normalize()
00115         {
00116                 float32 length = Length();
00117                 if (length < b2_epsilon)
00118                 {
00119                         return 0.0f;
00120                 }
00121                 float32 invLength = 1.0f / length;
00122                 x *= invLength;
00123                 y *= invLength;
00124 
00125                 return length;
00126         }
00127 
00129         bool IsValid() const
00130         {
00131                 return b2IsValid(x) && b2IsValid(y);
00132         }
00133 
00135         b2Vec2 Skew() const
00136         {
00137                 return b2Vec2(-y, x);
00138         }
00139 
00140         float32 x, y;
00141 };
00142 
00144 struct b2Vec3
00145 {
00147         b2Vec3() {}
00148 
00150         b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
00151 
00153         void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
00154 
00156         void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
00157 
00159         b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
00160 
00162         void operator += (const b2Vec3& v)
00163         {
00164                 x += v.x; y += v.y; z += v.z;
00165         }
00166 
00168         void operator -= (const b2Vec3& v)
00169         {
00170                 x -= v.x; y -= v.y; z -= v.z;
00171         }
00172 
00174         void operator *= (float32 s)
00175         {
00176                 x *= s; y *= s; z *= s;
00177         }
00178 
00179         float32 x, y, z;
00180 };
00181 
00183 struct b2Mat22
00184 {
00186         b2Mat22() {}
00187 
00189         b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
00190         {
00191                 ex = c1;
00192                 ey = c2;
00193         }
00194 
00196         b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
00197         {
00198                 ex.x = a11; ex.y = a21;
00199                 ey.x = a12; ey.y = a22;
00200         }
00201 
00203         void Set(const b2Vec2& c1, const b2Vec2& c2)
00204         {
00205                 ex = c1;
00206                 ey = c2;
00207         }
00208 
00210         void SetIdentity()
00211         {
00212                 ex.x = 1.0f; ey.x = 0.0f;
00213                 ex.y = 0.0f; ey.y = 1.0f;
00214         }
00215 
00217         void SetZero()
00218         {
00219                 ex.x = 0.0f; ey.x = 0.0f;
00220                 ex.y = 0.0f; ey.y = 0.0f;
00221         }
00222 
00223         b2Mat22 GetInverse() const
00224         {
00225                 float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
00226                 b2Mat22 B;
00227                 float32 det = a * d - b * c;
00228                 if (det != 0.0f)
00229                 {
00230                         det = 1.0f / det;
00231                 }
00232                 B.ex.x =  det * d;      B.ey.x = -det * b;
00233                 B.ex.y = -det * c;      B.ey.y =  det * a;
00234                 return B;
00235         }
00236 
00239         b2Vec2 Solve(const b2Vec2& b) const
00240         {
00241                 float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
00242                 float32 det = a11 * a22 - a12 * a21;
00243                 if (det != 0.0f)
00244                 {
00245                         det = 1.0f / det;
00246                 }
00247                 b2Vec2 x;
00248                 x.x = det * (a22 * b.x - a12 * b.y);
00249                 x.y = det * (a11 * b.y - a21 * b.x);
00250                 return x;
00251         }
00252 
00253         b2Vec2 ex, ey;
00254 };
00255 
00257 struct b2Mat33
00258 {
00260         b2Mat33() {}
00261 
00263         b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
00264         {
00265                 ex = c1;
00266                 ey = c2;
00267                 ez = c3;
00268         }
00269 
00271         void SetZero()
00272         {
00273                 ex.SetZero();
00274                 ey.SetZero();
00275                 ez.SetZero();
00276         }
00277 
00280         b2Vec3 Solve33(const b2Vec3& b) const;
00281 
00285         b2Vec2 Solve22(const b2Vec2& b) const;
00286 
00289         void GetInverse22(b2Mat33* M) const;
00290 
00293         void GetSymInverse33(b2Mat33* M) const;
00294 
00295         b2Vec3 ex, ey, ez;
00296 };
00297 
00299 struct b2Rot
00300 {
00301         b2Rot() {}
00302 
00304         explicit b2Rot(float32 angle)
00305         {
00307                 s = sinf(angle);
00308                 c = cosf(angle);
00309         }
00310 
00312         void Set(float32 angle)
00313         {
00315                 s = sinf(angle);
00316                 c = cosf(angle);
00317         }
00318 
00320         void SetIdentity()
00321         {
00322                 s = 0.0f;
00323                 c = 1.0f;
00324         }
00325 
00327         float32 GetAngle() const
00328         {
00329                 return b2Atan2(s, c);
00330         }
00331 
00333         b2Vec2 GetXAxis() const
00334         {
00335                 return b2Vec2(c, s);
00336         }
00337 
00339         b2Vec2 GetYAxis() const
00340         {
00341                 return b2Vec2(-s, c);
00342         }
00343 
00345         float32 s, c;
00346 };
00347 
00350 struct b2Transform
00351 {
00353         b2Transform() {}
00354 
00356         b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
00357 
00359         void SetIdentity()
00360         {
00361                 p.SetZero();
00362                 q.SetIdentity();
00363         }
00364 
00366         void Set(const b2Vec2& position, float32 angle)
00367         {
00368                 p = position;
00369                 q.Set(angle);
00370         }
00371 
00372         b2Vec2 p;
00373         b2Rot q;
00374 };
00375 
00380 struct b2Sweep
00381 {
00384         void GetTransform(b2Transform* xfb, float32 beta) const;
00385 
00388         void Advance(float32 alpha);
00389 
00391         void Normalize();
00392 
00393         b2Vec2 localCenter;     
00394         b2Vec2 c0, c;           
00395         float32 a0, a;          
00396 
00399         float32 alpha0;
00400 };
00401 
00403 extern const b2Vec2 b2Vec2_zero;
00404 
00406 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
00407 {
00408         return a.x * b.x + a.y * b.y;
00409 }
00410 
00412 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
00413 {
00414         return a.x * b.y - a.y * b.x;
00415 }
00416 
00419 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
00420 {
00421         return b2Vec2(s * a.y, -s * a.x);
00422 }
00423 
00426 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
00427 {
00428         return b2Vec2(-s * a.y, s * a.x);
00429 }
00430 
00433 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
00434 {
00435         return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
00436 }
00437 
00440 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
00441 {
00442         return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
00443 }
00444 
00446 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
00447 {
00448         return b2Vec2(a.x + b.x, a.y + b.y);
00449 }
00450 
00452 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
00453 {
00454         return b2Vec2(a.x - b.x, a.y - b.y);
00455 }
00456 
00457 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
00458 {
00459         return b2Vec2(s * a.x, s * a.y);
00460 }
00461 
00462 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
00463 {
00464         return a.x == b.x && a.y == b.y;
00465 }
00466 
00467 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
00468 {
00469         b2Vec2 c = a - b;
00470         return c.Length();
00471 }
00472 
00473 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
00474 {
00475         b2Vec2 c = a - b;
00476         return b2Dot(c, c);
00477 }
00478 
00479 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
00480 {
00481         return b2Vec3(s * a.x, s * a.y, s * a.z);
00482 }
00483 
00485 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
00486 {
00487         return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
00488 }
00489 
00491 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
00492 {
00493         return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
00494 }
00495 
00497 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
00498 {
00499         return a.x * b.x + a.y * b.y + a.z * b.z;
00500 }
00501 
00503 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
00504 {
00505         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);
00506 }
00507 
00508 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
00509 {
00510         return b2Mat22(A.ex + B.ex, A.ey + B.ey);
00511 }
00512 
00513 // A * B
00514 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
00515 {
00516         return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
00517 }
00518 
00519 // A^T * B
00520 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
00521 {
00522         b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
00523         b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
00524         return b2Mat22(c1, c2);
00525 }
00526 
00528 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
00529 {
00530         return v.x * A.ex + v.y * A.ey + v.z * A.ez;
00531 }
00532 
00534 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
00535 {
00536         return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
00537 }
00538 
00540 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
00541 {
00542         // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
00543         // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
00544         // s = qs * rc + qc * rs
00545         // c = qc * rc - qs * rs
00546         b2Rot qr;
00547         qr.s = q.s * r.c + q.c * r.s;
00548         qr.c = q.c * r.c - q.s * r.s;
00549         return qr;
00550 }
00551 
00553 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
00554 {
00555         // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
00556         // [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
00557         // s = qc * rs - qs * rc
00558         // c = qc * rc + qs * rs
00559         b2Rot qr;
00560         qr.s = q.c * r.s - q.s * r.c;
00561         qr.c = q.c * r.c + q.s * r.s;
00562         return qr;
00563 }
00564 
00566 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
00567 {
00568         return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
00569 }
00570 
00572 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
00573 {
00574         return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
00575 }
00576 
00577 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
00578 {
00579         float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
00580         float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
00581 
00582         return b2Vec2(x, y);
00583 }
00584 
00585 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
00586 {
00587         float32 px = v.x - T.p.x;
00588         float32 py = v.y - T.p.y;
00589         float32 x = (T.q.c * px + T.q.s * py);
00590         float32 y = (-T.q.s * px + T.q.c * py);
00591 
00592         return b2Vec2(x, y);
00593 }
00594 
00595 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
00596 //    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
00597 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
00598 {
00599         b2Transform C;
00600         C.q = b2Mul(A.q, B.q);
00601         C.p = b2Mul(A.q, B.p) + A.p;
00602         return C;
00603 }
00604 
00605 // v2 = A.q' * (B.q * v1 + B.p - A.p)
00606 //    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
00607 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
00608 {
00609         b2Transform C;
00610         C.q = b2MulT(A.q, B.q);
00611         C.p = b2MulT(A.q, B.p - A.p);
00612         return C;
00613 }
00614 
00615 template <typename T>
00616 inline T b2Abs(T a)
00617 {
00618         return a > T(0) ? a : -a;
00619 }
00620 
00621 inline b2Vec2 b2Abs(const b2Vec2& a)
00622 {
00623         return b2Vec2(b2Abs(a.x), b2Abs(a.y));
00624 }
00625 
00626 inline b2Mat22 b2Abs(const b2Mat22& A)
00627 {
00628         return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
00629 }
00630 
00631 template <typename T>
00632 inline T b2Min(T a, T b)
00633 {
00634         return a < b ? a : b;
00635 }
00636 
00637 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
00638 {
00639         return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
00640 }
00641 
00642 template <typename T>
00643 inline T b2Max(T a, T b)
00644 {
00645         return a > b ? a : b;
00646 }
00647 
00648 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
00649 {
00650         return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
00651 }
00652 
00653 template <typename T>
00654 inline T b2Clamp(T a, T low, T high)
00655 {
00656         return b2Max(low, b2Min(a, high));
00657 }
00658 
00659 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
00660 {
00661         return b2Max(low, b2Min(a, high));
00662 }
00663 
00664 template<typename T> inline void b2Swap(T& a, T& b)
00665 {
00666         T tmp = a;
00667         a = b;
00668         b = tmp;
00669 }
00670 
00676 inline uint32 b2NextPowerOfTwo(uint32 x)
00677 {
00678         x |= (x >> 1);
00679         x |= (x >> 2);
00680         x |= (x >> 4);
00681         x |= (x >> 8);
00682         x |= (x >> 16);
00683         return x + 1;
00684 }
00685 
00686 inline bool b2IsPowerOfTwo(uint32 x)
00687 {
00688         bool result = x > 0 && (x & (x - 1)) == 0;
00689         return result;
00690 }
00691 
00692 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
00693 {
00694         xf->p = (1.0f - beta) * c0 + beta * c;
00695         float32 angle = (1.0f - beta) * a0 + beta * a;
00696         xf->q.Set(angle);
00697 
00698         // Shift to origin
00699         xf->p -= b2Mul(xf->q, localCenter);
00700 }
00701 
00702 inline void b2Sweep::Advance(float32 alpha)
00703 {
00704         b2Assert(alpha0 < 1.0f);
00705         float32 beta = (alpha - alpha0) / (1.0f - alpha0);
00706         c0 += beta * (c - c0);
00707         a0 += beta * (a - a0);
00708         alpha0 = alpha;
00709 }
00710 
00712 inline void b2Sweep::Normalize()
00713 {
00714         float32 twoPi = 2.0f * b2_pi;
00715         float32 d =  twoPi * floorf(a0 / twoPi);
00716         a0 -= d;
00717         a -= d;
00718 }
00719 
00720 #endif


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34