00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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
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
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
00543
00544
00545
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
00556
00557
00558
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
00596
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
00606
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
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