![]() |
Box2D
2.2.1
A 2D Physics Engine for Games
|
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