![]() |
Box2D
2.2.0
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 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