Box2D: Box2D/Box2D/Common/b2Math.h Source File

- Main Page
- Classes
- Files

- File List
- File Members

# Box2D/Box2D/Common/b2Math.h

Go to the documentation of this file.

    00001 /*
    00002 * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
    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)       sqrtf(x)
    00061 #define b2Atan2(y, x)   atan2f(y, x)
    00062
    00063 inline float32 b2Abs(float32 a)
    00064 {
    00065         return a > 0.0f ? a : -a;
    00066 }
    00067
    00069 struct b2Vec2
    00070 {
    00072         b2Vec2() {}
    00073
    00075         b2Vec2(float32 x, float32 y) : x(x), y(y) {}
    00076
    00078         void SetZero() { x = 0.0f; y = 0.0f; }
    00079
    00081         void Set(float32 x_, float32 y_) { x = x_; y = y_; }
    00082
    00084         b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
    00085
    00087         float32 operator () (int32 i) const
    00088         {
    00089                 return (&x)[i];
    00090         }
    00091
    00093         float32& operator () (int32 i)
    00094         {
    00095                 return (&x)[i];
    00096         }
    00097
    00099         void operator += (const b2Vec2& v)
    00100         {
    00101                 x += v.x; y += v.y;
    00102         }
    00103
    00105         void operator -= (const b2Vec2& v)
    00106         {
    00107                 x -= v.x; y -= v.y;
    00108         }
    00109
    00111         void operator *= (float32 a)
    00112         {
    00113                 x *= a; y *= a;
    00114         }
    00115
    00117         float32 Length() const
    00118         {
    00119                 return b2Sqrt(x * x + y * y);
    00120         }
    00121
    00124         float32 LengthSquared() const
    00125         {
    00126                 return x * x + y * y;
    00127         }
    00128
    00130         float32 Normalize()
    00131         {
    00132                 float32 length = Length();
    00133                 if (length < b2_epsilon)
    00134                 {
    00135                         return 0.0f;
    00136                 }
    00137                 float32 invLength = 1.0f / length;
    00138                 x *= invLength;
    00139                 y *= invLength;
    00140
    00141                 return length;
    00142         }
    00143
    00145         bool IsValid() const
    00146         {
    00147                 return b2IsValid(x) && b2IsValid(y);
    00148         }
    00149
    00150         float32 x, y;
    00151 };
    00152
    00154 struct b2Vec3
    00155 {
    00157         b2Vec3() {}
    00158
    00160         b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
    00161
    00163         void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
    00164
    00166         void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
    00167
    00169         b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
    00170
    00172         void operator += (const b2Vec3& v)
    00173         {
    00174                 x += v.x; y += v.y; z += v.z;
    00175         }
    00176
    00178         void operator -= (const b2Vec3& v)
    00179         {
    00180                 x -= v.x; y -= v.y; z -= v.z;
    00181         }
    00182
    00184         void operator *= (float32 s)
    00185         {
    00186                 x *= s; y *= s; z *= s;
    00187         }
    00188
    00189         float32 x, y, z;
    00190 };
    00191
    00193 struct b2Mat22
    00194 {
    00196         b2Mat22() {}
    00197
    00199         b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
    00200         {
    00201                 col1 = c1;
    00202                 col2 = c2;
    00203         }
    00204
    00206         b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
    00207         {
    00208                 col1.x = a11; col1.y = a21;
    00209                 col2.x = a12; col2.y = a22;
    00210         }
    00211
    00214         explicit b2Mat22(float32 angle)
    00215         {
    00216                 // TODO_ERIN compute sin+cos together.
    00217                 float32 c = cosf(angle), s = sinf(angle);
    00218                 col1.x = c; col2.x = -s;
    00219                 col1.y = s; col2.y = c;
    00220         }
    00221
    00223         void Set(const b2Vec2& c1, const b2Vec2& c2)
    00224         {
    00225                 col1 = c1;
    00226                 col2 = c2;
    00227         }
    00228
    00231         void Set(float32 angle)
    00232         {
    00233                 float32 c = cosf(angle), s = sinf(angle);
    00234                 col1.x = c; col2.x = -s;
    00235                 col1.y = s; col2.y = c;
    00236         }
    00237
    00239         void SetIdentity()
    00240         {
    00241                 col1.x = 1.0f; col2.x = 0.0f;
    00242                 col1.y = 0.0f; col2.y = 1.0f;
    00243         }
    00244
    00246         void SetZero()
    00247         {
    00248                 col1.x = 0.0f; col2.x = 0.0f;
    00249                 col1.y = 0.0f; col2.y = 0.0f;
    00250         }
    00251
    00254         float32 GetAngle() const
    00255         {
    00256                 return b2Atan2(col1.y, col1.x);
    00257         }
    00258
    00259         b2Mat22 GetInverse() const
    00260         {
    00261                 float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
    00262                 b2Mat22 B;
    00263                 float32 det = a * d - b * c;
    00264                 if (det != 0.0f)
    00265                 {
    00266                         det = 1.0f / det;
    00267                 }
    00268                 B.col1.x =  det * d;    B.col2.x = -det * b;
    00269                 B.col1.y = -det * c;    B.col2.y =  det * a;
    00270                 return B;
    00271         }
    00272
    00275         b2Vec2 Solve(const b2Vec2& b) const
    00276         {
    00277                 float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
    00278                 float32 det = a11 * a22 - a12 * a21;
    00279                 if (det != 0.0f)
    00280                 {
    00281                         det = 1.0f / det;
    00282                 }
    00283                 b2Vec2 x;
    00284                 x.x = det * (a22 * b.x - a12 * b.y);
    00285                 x.y = det * (a11 * b.y - a21 * b.x);
    00286                 return x;
    00287         }
    00288
    00289         b2Vec2 col1, col2;
    00290 };
    00291
    00293 struct b2Mat33
    00294 {
    00296         b2Mat33() {}
    00297
    00299         b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
    00300         {
    00301                 col1 = c1;
    00302                 col2 = c2;
    00303                 col3 = c3;
    00304         }
    00305
    00307         void SetZero()
    00308         {
    00309                 col1.SetZero();
    00310                 col2.SetZero();
    00311                 col3.SetZero();
    00312         }
    00313
    00316         b2Vec3 Solve33(const b2Vec3& b) const;
    00317
    00321         b2Vec2 Solve22(const b2Vec2& b) const;
    00322
    00323         b2Vec3 col1, col2, col3;
    00324 };
    00325
    00328 struct b2Transform
    00329 {
    00331         b2Transform() {}
    00332
    00334         b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
    00335
    00337         void SetIdentity()
    00338         {
    00339                 position.SetZero();
    00340                 R.SetIdentity();
    00341         }
    00342
    00344         void Set(const b2Vec2& p, float32 angle)
    00345         {
    00346                 position = p;
    00347                 R.Set(angle);
    00348         }
    00349
    00351         float32 GetAngle() const
    00352         {
    00353                 return b2Atan2(R.col1.y, R.col1.x);
    00354         }
    00355
    00356         b2Vec2 position;
    00357         b2Mat22 R;
    00358 };
    00359
    00364 struct b2Sweep
    00365 {
    00368         void GetTransform(b2Transform* xf, float32 alpha) const;
    00369
    00372         void Advance(float32 t);
    00373
    00374         b2Vec2 localCenter;
    00375         b2Vec2 c0, c;
    00376         float32 a0, a;
    00377 };
    00378
    00379
    00380 extern const b2Vec2 b2Vec2_zero;
    00381 extern const b2Mat22 b2Mat22_identity;
    00382 extern const b2Transform b2Transform_identity;
    00383
    00385 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
    00386 {
    00387         return a.x * b.x + a.y * b.y;
    00388 }
    00389
    00391 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
    00392 {
    00393         return a.x * b.y - a.y * b.x;
    00394 }
    00395
    00398 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
    00399 {
    00400         return b2Vec2(s * a.y, -s * a.x);
    00401 }
    00402
    00405 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
    00406 {
    00407         return b2Vec2(-s * a.y, s * a.x);
    00408 }
    00409
    00412 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
    00413 {
    00414         return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
    00415 }
    00416
    00419 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
    00420 {
    00421         return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
    00422 }
    00423
    00425 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
    00426 {
    00427         return b2Vec2(a.x + b.x, a.y + b.y);
    00428 }
    00429
    00431 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
    00432 {
    00433         return b2Vec2(a.x - b.x, a.y - b.y);
    00434 }
    00435
    00436 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
    00437 {
    00438         return b2Vec2(s * a.x, s * a.y);
    00439 }
    00440
    00441 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
    00442 {
    00443         return a.x == b.x && a.y == b.y;
    00444 }
    00445
    00446 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
    00447 {
    00448         b2Vec2 c = a - b;
    00449         return c.Length();
    00450 }
    00451
    00452 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
    00453 {
    00454         b2Vec2 c = a - b;
    00455         return b2Dot(c, c);
    00456 }
    00457
    00458 inline b2Vec3 operator * (float32 s, const b2Vec3& a)
    00459 {
    00460         return b2Vec3(s * a.x, s * a.y, s * a.z);
    00461 }
    00462
    00464 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
    00465 {
    00466         return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
    00467 }
    00468
    00470 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
    00471 {
    00472         return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
    00473 }
    00474
    00476 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
    00477 {
    00478         return a.x * b.x + a.y * b.y + a.z * b.z;
    00479 }
    00480
    00482 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
    00483 {
    00484         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);
    00485 }
    00486
    00487 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
    00488 {
    00489         return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
    00490 }
    00491
    00492 // A * B
    00493 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
    00494 {
    00495         return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
    00496 }
    00497
    00498 // A^T * B
    00499 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
    00500 {
    00501         b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
    00502         b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
    00503         return b2Mat22(c1, c2);
    00504 }
    00505
    00507 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
    00508 {
    00509         return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
    00510 }
    00511
    00512 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
    00513 {
    00514         float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
    00515         float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
    00516
    00517         return b2Vec2(x, y);
    00518 }
    00519
    00520 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
    00521 {
    00522         return b2MulT(T.R, v - T.position);
    00523 }
    00524
    00525 inline b2Vec2 b2Abs(const b2Vec2& a)
    00526 {
    00527         return b2Vec2(b2Abs(a.x), b2Abs(a.y));
    00528 }
    00529
    00530 inline b2Mat22 b2Abs(const b2Mat22& A)
    00531 {
    00532         return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
    00533 }
    00534
    00535 template <typename T>
    00536 inline T b2Min(T a, T b)
    00537 {
    00538         return a < b ? a : b;
    00539 }
    00540
    00541 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
    00542 {
    00543         return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
    00544 }
    00545
    00546 template <typename T>
    00547 inline T b2Max(T a, T b)
    00548 {
    00549         return a > b ? a : b;
    00550 }
    00551
    00552 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
    00553 {
    00554         return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
    00555 }
    00556
    00557 template <typename T>
    00558 inline T b2Clamp(T a, T low, T high)
    00559 {
    00560         return b2Max(low, b2Min(a, high));
    00561 }
    00562
    00563 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
    00564 {
    00565         return b2Max(low, b2Min(a, high));
    00566 }
    00567
    00568 template<typename T> inline void b2Swap(T& a, T& b)
    00569 {
    00570         T tmp = a;
    00571         a = b;
    00572         b = tmp;
    00573 }
    00574
    00580 inline uint32 b2NextPowerOfTwo(uint32 x)
    00581 {
    00582         x |= (x >> 1);
    00583         x |= (x >> 2);
    00584         x |= (x >> 4);
    00585         x |= (x >> 8);
    00586         x |= (x >> 16);
    00587         return x + 1;
    00588 }
    00589
    00590 inline bool b2IsPowerOfTwo(uint32 x)
    00591 {
    00592         bool result = x > 0 && (x & (x - 1)) == 0;
    00593         return result;
    00594 }
    00595
    00596 inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
    00597 {
    00598         xf->position = (1.0f - alpha) * c0 + alpha * c;
    00599         float32 angle = (1.0f - alpha) * a0 + alpha * a;
    00600         xf->R.Set(angle);
    00601
    00602         // Shift to origin
    00603         xf->position -= b2Mul(xf->R, localCenter);
    00604 }
    00605
    00606 inline void b2Sweep::Advance(float32 t)
    00607 {
    00608         c0 = (1.0f - t) * c0 + t * c;
    00609         a0 = (1.0f - t) * a0 + t * a;
    00610 }
    00611
    00612 #endif