b2Math.h
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:15k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  3. *
  4. * This software is provided 'as-is', without any express or implied
  5. * warranty.  In no event will the authors be held liable for any damages
  6. * arising from the use of this software.
  7. * Permission is granted to anyone to use this software for any purpose,
  8. * including commercial applications, and to alter it and redistribute it
  9. * freely, subject to the following restrictions:
  10. * 1. The origin of this software must not be misrepresented; you must not
  11. * claim that you wrote the original software. If you use this software
  12. * in a product, an acknowledgment in the product documentation would be
  13. * appreciated but is not required.
  14. * 2. Altered source versions must be plainly marked as such, and must not be
  15. * misrepresented as being the original software.
  16. * 3. This notice may not be removed or altered from any source distribution.
  17. */
  18. #ifndef B2_MATH_H
  19. #define B2_MATH_H
  20. #include <Box2D/Common/b2Settings.h>
  21. #include <cmath>
  22. #include <cfloat>
  23. #include <cstddef>
  24. #include <limits>
  25. /// This function is used to ensure that a floating point number is
  26. /// not a NaN or infinity.
  27. inline bool b2IsValid(float32 x)
  28. {
  29. if (x != x)
  30. {
  31. // NaN.
  32. return false;
  33. }
  34. float32 infinity = std::numeric_limits<float32>::infinity();
  35. return -infinity < x && x < infinity;
  36. }
  37. /// This is a approximate yet fast inverse square-root.
  38. inline float32 b2InvSqrt(float32 x)
  39. {
  40. union
  41. {
  42. float32 x;
  43. int32 i;
  44. } convert;
  45. convert.x = x;
  46. float32 xhalf = 0.5f * x;
  47. convert.i = 0x5f3759df - (convert.i >> 1);
  48. x = convert.x;
  49. x = x * (1.5f - xhalf * x * x);
  50. return x;
  51. }
  52. #define b2Sqrt(x) sqrtf(x)
  53. #define b2Atan2(y, x) atan2f(y, x)
  54. inline float32 b2Abs(float32 a)
  55. {
  56. return a > 0.0f ? a : -a;
  57. }
  58. /// A 2D column vector.
  59. struct b2Vec2
  60. {
  61. /// Default constructor does nothing (for performance).
  62. b2Vec2() {}
  63. /// Construct using coordinates.
  64. b2Vec2(float32 x, float32 y) : x(x), y(y) {}
  65. /// Set this vector to all zeros.
  66. void SetZero() { x = 0.0f; y = 0.0f; }
  67. /// Set this vector to some specified coordinates.
  68. void Set(float32 x_, float32 y_) { x = x_; y = y_; }
  69. /// Negate this vector.
  70. b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
  71. /// Read from and indexed element.
  72. float32 operator () (int32 i) const
  73. {
  74. return (&x)[i];
  75. }
  76. /// Write to an indexed element.
  77. float32& operator () (int32 i)
  78. {
  79. return (&x)[i];
  80. }
  81. /// Add a vector to this vector.
  82. void operator += (const b2Vec2& v)
  83. {
  84. x += v.x; y += v.y;
  85. }
  86. /// Subtract a vector from this vector.
  87. void operator -= (const b2Vec2& v)
  88. {
  89. x -= v.x; y -= v.y;
  90. }
  91. /// Multiply this vector by a scalar.
  92. void operator *= (float32 a)
  93. {
  94. x *= a; y *= a;
  95. }
  96. /// Get the length of this vector (the norm).
  97. float32 Length() const
  98. {
  99. return b2Sqrt(x * x + y * y);
  100. }
  101. /// Get the length squared. For performance, use this instead of
  102. /// b2Vec2::Length (if possible).
  103. float32 LengthSquared() const
  104. {
  105. return x * x + y * y;
  106. }
  107. /// Convert this vector into a unit vector. Returns the length.
  108. float32 Normalize()
  109. {
  110. float32 length = Length();
  111. if (length < b2_epsilon)
  112. {
  113. return 0.0f;
  114. }
  115. float32 invLength = 1.0f / length;
  116. x *= invLength;
  117. y *= invLength;
  118. return length;
  119. }
  120. /// Does this vector contain finite coordinates?
  121. bool IsValid() const
  122. {
  123. return b2IsValid(x) && b2IsValid(y);
  124. }
  125. float32 x, y;
  126. };
  127. /// A 2D column vector with 3 elements.
  128. struct b2Vec3
  129. {
  130. /// Default constructor does nothing (for performance).
  131. b2Vec3() {}
  132. /// Construct using coordinates.
  133. b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
  134. /// Set this vector to all zeros.
  135. void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
  136. /// Set this vector to some specified coordinates.
  137. void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
  138. /// Negate this vector.
  139. b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
  140. /// Add a vector to this vector.
  141. void operator += (const b2Vec3& v)
  142. {
  143. x += v.x; y += v.y; z += v.z;
  144. }
  145. /// Subtract a vector from this vector.
  146. void operator -= (const b2Vec3& v)
  147. {
  148. x -= v.x; y -= v.y; z -= v.z;
  149. }
  150. /// Multiply this vector by a scalar.
  151. void operator *= (float32 s)
  152. {
  153. x *= s; y *= s; z *= s;
  154. }
  155. float32 x, y, z;
  156. };
  157. /// A 2-by-2 matrix. Stored in column-major order.
  158. struct b2Mat22
  159. {
  160. /// The default constructor does nothing (for performance).
  161. b2Mat22() {}
  162. /// Construct this matrix using columns.
  163. b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
  164. {
  165. col1 = c1;
  166. col2 = c2;
  167. }
  168. /// Construct this matrix using scalars.
  169. b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
  170. {
  171. col1.x = a11; col1.y = a21;
  172. col2.x = a12; col2.y = a22;
  173. }
  174. /// Construct this matrix using an angle. This matrix becomes
  175. /// an orthonormal rotation matrix.
  176. explicit b2Mat22(float32 angle)
  177. {
  178. // TODO_ERIN compute sin+cos together.
  179. float32 c = cosf(angle), s = sinf(angle);
  180. col1.x = c; col2.x = -s;
  181. col1.y = s; col2.y = c;
  182. }
  183. /// Initialize this matrix using columns.
  184. void Set(const b2Vec2& c1, const b2Vec2& c2)
  185. {
  186. col1 = c1;
  187. col2 = c2;
  188. }
  189. /// Initialize this matrix using an angle. This matrix becomes
  190. /// an orthonormal rotation matrix.
  191. void Set(float32 angle)
  192. {
  193. float32 c = cosf(angle), s = sinf(angle);
  194. col1.x = c; col2.x = -s;
  195. col1.y = s; col2.y = c;
  196. }
  197. /// Set this to the identity matrix.
  198. void SetIdentity()
  199. {
  200. col1.x = 1.0f; col2.x = 0.0f;
  201. col1.y = 0.0f; col2.y = 1.0f;
  202. }
  203. /// Set this matrix to all zeros.
  204. void SetZero()
  205. {
  206. col1.x = 0.0f; col2.x = 0.0f;
  207. col1.y = 0.0f; col2.y = 0.0f;
  208. }
  209. /// Extract the angle from this matrix (assumed to be
  210. /// a rotation matrix).
  211. float32 GetAngle() const
  212. {
  213. return b2Atan2(col1.y, col1.x);
  214. }
  215. b2Mat22 GetInverse() const
  216. {
  217. float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
  218. b2Mat22 B;
  219. float32 det = a * d - b * c;
  220. if (det != 0.0f)
  221. {
  222. det = 1.0f / det;
  223. }
  224. B.col1.x =  det * d; B.col2.x = -det * b;
  225. B.col1.y = -det * c; B.col2.y =  det * a;
  226. return B;
  227. }
  228. /// Solve A * x = b, where b is a column vector. This is more efficient
  229. /// than computing the inverse in one-shot cases.
  230. b2Vec2 Solve(const b2Vec2& b) const
  231. {
  232. float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
  233. float32 det = a11 * a22 - a12 * a21;
  234. if (det != 0.0f)
  235. {
  236. det = 1.0f / det;
  237. }
  238. b2Vec2 x;
  239. x.x = det * (a22 * b.x - a12 * b.y);
  240. x.y = det * (a11 * b.y - a21 * b.x);
  241. return x;
  242. }
  243. b2Vec2 col1, col2;
  244. };
  245. /// A 3-by-3 matrix. Stored in column-major order.
  246. struct b2Mat33
  247. {
  248. /// The default constructor does nothing (for performance).
  249. b2Mat33() {}
  250. /// Construct this matrix using columns.
  251. b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
  252. {
  253. col1 = c1;
  254. col2 = c2;
  255. col3 = c3;
  256. }
  257. /// Set this matrix to all zeros.
  258. void SetZero()
  259. {
  260. col1.SetZero();
  261. col2.SetZero();
  262. col3.SetZero();
  263. }
  264. /// Solve A * x = b, where b is a column vector. This is more efficient
  265. /// than computing the inverse in one-shot cases.
  266. b2Vec3 Solve33(const b2Vec3& b) const;
  267. /// Solve A * x = b, where b is a column vector. This is more efficient
  268. /// than computing the inverse in one-shot cases. Solve only the upper
  269. /// 2-by-2 matrix equation.
  270. b2Vec2 Solve22(const b2Vec2& b) const;
  271. b2Vec3 col1, col2, col3;
  272. };
  273. /// A transform contains translation and rotation. It is used to represent
  274. /// the position and orientation of rigid frames.
  275. struct b2Transform
  276. {
  277. /// The default constructor does nothing (for performance).
  278. b2Transform() {}
  279. /// Initialize using a position vector and a rotation matrix.
  280. b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
  281. /// Set this to the identity transform.
  282. void SetIdentity()
  283. {
  284. position.SetZero();
  285. R.SetIdentity();
  286. }
  287. /// Set this based on the position and angle.
  288. void Set(const b2Vec2& p, float32 angle)
  289. {
  290. position = p;
  291. R.Set(angle);
  292. }
  293. /// Calculate the angle that the rotation matrix represents.
  294. float32 GetAngle() const
  295. {
  296. return b2Atan2(R.col1.y, R.col1.x);
  297. }
  298. b2Vec2 position;
  299. b2Mat22 R;
  300. };
  301. /// This describes the motion of a body/shape for TOI computation.
  302. /// Shapes are defined with respect to the body origin, which may
  303. /// no coincide with the center of mass. However, to support dynamics
  304. /// we must interpolate the center of mass position.
  305. struct b2Sweep
  306. {
  307. /// Get the interpolated transform at a specific time.
  308. /// @param alpha is a factor in [0,1], where 0 indicates t0.
  309. void GetTransform(b2Transform* xf, float32 alpha) const;
  310. /// Advance the sweep forward, yielding a new initial state.
  311. /// @param t the new initial time.
  312. void Advance(float32 t);
  313. /// Normalize the angles.
  314. void Normalize();
  315. b2Vec2 localCenter; ///< local center of mass position
  316. b2Vec2 c0, c; ///< center world positions
  317. float32 a0, a; ///< world angles
  318. };
  319. extern const b2Vec2 b2Vec2_zero;
  320. extern const b2Mat22 b2Mat22_identity;
  321. extern const b2Transform b2Transform_identity;
  322. /// Perform the dot product on two vectors.
  323. inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
  324. {
  325. return a.x * b.x + a.y * b.y;
  326. }
  327. /// Perform the cross product on two vectors. In 2D this produces a scalar.
  328. inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
  329. {
  330. return a.x * b.y - a.y * b.x;
  331. }
  332. /// Perform the cross product on a vector and a scalar. In 2D this produces
  333. /// a vector.
  334. inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
  335. {
  336. return b2Vec2(s * a.y, -s * a.x);
  337. }
  338. /// Perform the cross product on a scalar and a vector. In 2D this produces
  339. /// a vector.
  340. inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
  341. {
  342. return b2Vec2(-s * a.y, s * a.x);
  343. }
  344. /// Multiply a matrix times a vector. If a rotation matrix is provided,
  345. /// then this transforms the vector from one frame to another.
  346. inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
  347. {
  348. return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
  349. }
  350. /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
  351. /// then this transforms the vector from one frame to another (inverse transform).
  352. inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
  353. {
  354. return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
  355. }
  356. /// Add two vectors component-wise.
  357. inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
  358. {
  359. return b2Vec2(a.x + b.x, a.y + b.y);
  360. }
  361. /// Subtract two vectors component-wise.
  362. inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
  363. {
  364. return b2Vec2(a.x - b.x, a.y - b.y);
  365. }
  366. inline b2Vec2 operator * (float32 s, const b2Vec2& a)
  367. {
  368. return b2Vec2(s * a.x, s * a.y);
  369. }
  370. inline bool operator == (const b2Vec2& a, const b2Vec2& b)
  371. {
  372. return a.x == b.x && a.y == b.y;
  373. }
  374. inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
  375. {
  376. b2Vec2 c = a - b;
  377. return c.Length();
  378. }
  379. inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
  380. {
  381. b2Vec2 c = a - b;
  382. return b2Dot(c, c);
  383. }
  384. inline b2Vec3 operator * (float32 s, const b2Vec3& a)
  385. {
  386. return b2Vec3(s * a.x, s * a.y, s * a.z);
  387. }
  388. /// Add two vectors component-wise.
  389. inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
  390. {
  391. return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
  392. }
  393. /// Subtract two vectors component-wise.
  394. inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
  395. {
  396. return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
  397. }
  398. /// Perform the dot product on two vectors.
  399. inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
  400. {
  401. return a.x * b.x + a.y * b.y + a.z * b.z;
  402. }
  403. /// Perform the cross product on two vectors.
  404. inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
  405. {
  406. 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);
  407. }
  408. inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
  409. {
  410. return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
  411. }
  412. // A * B
  413. inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
  414. {
  415. return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
  416. }
  417. // A^T * B
  418. inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
  419. {
  420. b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
  421. b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
  422. return b2Mat22(c1, c2);
  423. }
  424. /// Multiply a matrix times a vector.
  425. inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
  426. {
  427. return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
  428. }
  429. inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
  430. {
  431. float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
  432. float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
  433. return b2Vec2(x, y);
  434. }
  435. inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
  436. {
  437. return b2MulT(T.R, v - T.position);
  438. }
  439. inline b2Vec2 b2Abs(const b2Vec2& a)
  440. {
  441. return b2Vec2(b2Abs(a.x), b2Abs(a.y));
  442. }
  443. inline b2Mat22 b2Abs(const b2Mat22& A)
  444. {
  445. return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
  446. }
  447. template <typename T>
  448. inline T b2Min(T a, T b)
  449. {
  450. return a < b ? a : b;
  451. }
  452. inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
  453. {
  454. return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
  455. }
  456. template <typename T>
  457. inline T b2Max(T a, T b)
  458. {
  459. return a > b ? a : b;
  460. }
  461. inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
  462. {
  463. return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
  464. }
  465. template <typename T>
  466. inline T b2Clamp(T a, T low, T high)
  467. {
  468. return b2Max(low, b2Min(a, high));
  469. }
  470. inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
  471. {
  472. return b2Max(low, b2Min(a, high));
  473. }
  474. template<typename T> inline void b2Swap(T& a, T& b)
  475. {
  476. T tmp = a;
  477. a = b;
  478. b = tmp;
  479. }
  480. /// "Next Largest Power of 2
  481. /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
  482. /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
  483. /// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
  484. /// largest power of 2. For a 32-bit value:"
  485. inline uint32 b2NextPowerOfTwo(uint32 x)
  486. {
  487. x |= (x >> 1);
  488. x |= (x >> 2);
  489. x |= (x >> 4);
  490. x |= (x >> 8);
  491. x |= (x >> 16);
  492. return x + 1;
  493. }
  494. inline bool b2IsPowerOfTwo(uint32 x)
  495. {
  496. bool result = x > 0 && (x & (x - 1)) == 0;
  497. return result;
  498. }
  499. inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
  500. {
  501. xf->position = (1.0f - alpha) * c0 + alpha * c;
  502. float32 angle = (1.0f - alpha) * a0 + alpha * a;
  503. xf->R.Set(angle);
  504. // Shift to origin
  505. xf->position -= b2Mul(xf->R, localCenter);
  506. }
  507. inline void b2Sweep::Advance(float32 t)
  508. {
  509. c0 = (1.0f - t) * c0 + t * c;
  510. a0 = (1.0f - t) * a0 + t * a;
  511. }
  512. /// Normalize an angle in radians to be between -pi and pi
  513. inline void b2Sweep::Normalize()
  514. {
  515. float32 twoPi = 2.0f * b2_pi;
  516. float32 d =  twoPi * floorf(a0 / twoPi);
  517. a0 -= d;
  518. a -= d;
  519. }
  520. #endif