git.maemo.org Git - blok/blob - Box2D/Source/Common/b2Math.h

   1 /*
   2 * Copyright (c) 2006-2007 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
  19 #ifndef B2_MATH_H
  20 #define B2_MATH_H
  21
  22 #include "b2Settings.h"
  23 #include <cmath>
  24 #include <cfloat>
  25 #include <cstdlib>
  26
  27 #include <stdio.h>
  28
  29 #ifdef TARGET_FLOAT32_IS_FIXED
  30
  31 inline Fixed b2Min(const Fixed& a, const Fixed& b)
  32 {
  33   return a < b ? a : b;
  34 }
  35
  36 inline Fixed b2Max(const Fixed& a, const Fixed& b)
  37 {
  38   return a > b ? a : b;
  39 }
  40
  41 inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
  42 {
  43         return b2Max(low, b2Min(a, high));
  44 }
  45
  46 inline bool b2IsValid(Fixed x)
  47 {
  48         return true;
  49 }
  50
  51 #define b2Sqrt(x)       sqrt(x)
  52 #define b2Atan2(y, x)   atan2(y, x)
  53
  54 #else
  55
  56 /// This function is used to ensure that a floating point number is
  57 /// not a NaN or infinity.
  58 inline bool b2IsValid(float32 x)
  59 {
  60 #ifdef _MSC_VER
  61         return _finite(x) != 0;
  62 #else
  63         return finite(x) != 0;
  64 #endif
  65 }
  66
  67 /// This is a approximate yet fast inverse square-root.
  68 inline float32 b2InvSqrt(float32 x)
  69 {
  70         union
  71         {
  72                 float32 x;
  73                 int32 i;
  74         } convert;
  75
  76         convert.x = x;
  77         float32 xhalf = 0.5f * x;
  78         convert.i = 0x5f3759df - (convert.i >> 1);
  79         x = convert.x;
  80         x = x * (1.5f - xhalf * x * x);
  81         return x;
  82 }
  83
  84 #define b2Sqrt(x)       sqrtf(x)
  85 #define b2Atan2(y, x)   atan2f(y, x)
  86
  87 #endif
  88
  89 inline float32 b2Abs(float32 a)
  90 {
  91         return a > 0.0f ? a : -a;
  92 }
  93
  94 /// A 2D column vector.
  95
  96 struct b2Vec2
  97 {
  98         /// Default constructor does nothing (for performance).
  99         b2Vec2() {}
 100
 101         /// Construct using coordinates.
 102         b2Vec2(float32 x, float32 y) : x(x), y(y) {}
 103
 104         /// Set this vector to all zeros.
 105         void SetZero() { x = 0.0f; y = 0.0f; }
 106
 107         /// Set this vector to some specified coordinates.
 108         void Set(float32 x_, float32 y_) { x = x_; y = y_; }
 109
 110         /// Negate this vector.
 111         b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
 112
 113         /// Add a vector to this vector.
 114         void operator += (const b2Vec2& v)
 115         {
 116                 x += v.x; y += v.y;
 117         }
 118
 119         /// Subtract a vector from this vector.
 120         void operator -= (const b2Vec2& v)
 121         {
 122                 x -= v.x; y -= v.y;
 123         }
 124
 125         /// Multiply this vector by a scalar.
 126         void operator *= (float32 a)
 127         {
 128                 x *= a; y *= a;
 129         }
 130
 131         /// Get the length of this vector (the norm).
 132         float32 Length() const
 133         {
 134 #ifdef TARGET_FLOAT32_IS_FIXED
 135                 float est = b2Abs(x) + b2Abs(y);
 136                 if(est == 0.0f) {
 137                         return 0.0;
 138                 } else if(est < 0.1) {
 139                         return (1.0/256.0) * b2Vec2(x<<8, y<<8).Length();
 140                 } else if(est < 180.0f) {
 141                         return b2Sqrt(x * x + y * y);
 142                 } else {
 143                         return 256.0 * (b2Vec2(x>>8, y>>8).Length());
 144                 }
 145 #else
 146                 return b2Sqrt(x * x + y * y);
 147 #endif
 148         }
 149
 150         /// Get the length squared. For performance, use this instead of
 151         /// b2Vec2::Length (if possible).
 152         float32 LengthSquared() const
 153         {
 154                 return x * x + y * y;
 155         }
 156
 157         /// Convert this vector into a unit vector. Returns the length.
 158 #ifdef TARGET_FLOAT32_IS_FIXED
 159         float32 Normalize()
 160         {
 161                 float32 length = Length();
 162                 if (length < B2_FLT_EPSILON)
 163                 {
 164                         return 0.0f;
 165                 }
 166 #ifdef NORMALIZE_BY_INVERT_MULTIPLY
 167                 if (length < (1.0/16.0)) {
 168                         x = x << 4;
 169                         y = y << 4;
 170                         return (1.0/16.0)*Normalize();
 171                 } else if(length > 16.0) {
 172                         x = x >> 4;
 173                         y = y >> 4;
 174                         return 16.0*Normalize();
 175                 }
 176                 float32 invLength = 1.0f / length;
 177                 x *= invLength;
 178                 y *= invLength;
 179 #else
 180                 x /= length;
 181                 y /= length;
 182 #endif
 183                 return length;
 184         }
 185 #else
 186         float32 Normalize()
 187         {
 188                 float32 length = Length();
 189                 if (length < B2_FLT_EPSILON)
 190                 {
 191                         return 0.0f;
 192                 }
 193                 float32 invLength = 1.0f / length;
 194                 x *= invLength;
 195                 y *= invLength;
 196
 197                 return length;
 198         }
 199 #endif
 200
 201         /// Does this vector contain finite coordinates?
 202         bool IsValid() const
 203         {
 204                 return b2IsValid(x) && b2IsValid(y);
 205         }
 206
 207         float32 x, y;
 208 };
 209
 210 /// A 2-by-2 matrix. Stored in column-major order.
 211 struct b2Mat22
 212 {
 213         /// The default constructor does nothing (for performance).
 214         b2Mat22() {}
 215
 216         /// Construct this matrix using columns.
 217         b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
 218         {
 219                 col1 = c1;
 220                 col2 = c2;
 221         }
 222
 223         /// Construct this matrix using scalars.
 224         b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
 225         {
 226                 col1.x = a11; col1.y = a21;
 227                 col2.x = a12; col2.y = a22;
 228         }
 229
 230         /// Construct this matrix using an angle. This matrix becomes
 231         /// an orthonormal rotation matrix.
 232         explicit b2Mat22(float32 angle)
 233         {
 234                 float32 c = cosf(angle), s = sinf(angle);
 235                 col1.x = c; col2.x = -s;
 236                 col1.y = s; col2.y = c;
 237         }
 238
 239         /// Initialize this matrix using columns.
 240         void Set(const b2Vec2& c1, const b2Vec2& c2)
 241         {
 242                 col1 = c1;
 243                 col2 = c2;
 244         }
 245
 246         /// Initialize this matrix using an angle. This matrix becomes
 247         /// an orthonormal rotation matrix.
 248         void Set(float32 angle)
 249         {
 250                 float32 c = cosf(angle), s = sinf(angle);
 251                 col1.x = c; col2.x = -s;
 252                 col1.y = s; col2.y = c;
 253         }
 254
 255         /// Set this to the identity matrix.
 256         void SetIdentity()
 257         {
 258                 col1.x = 1.0f; col2.x = 0.0f;
 259                 col1.y = 0.0f; col2.y = 1.0f;
 260         }
 261
 262         /// Set this matrix to all zeros.
 263         void SetZero()
 264         {
 265                 col1.x = 0.0f; col2.x = 0.0f;
 266                 col1.y = 0.0f; col2.y = 0.0f;
 267         }
 268
 269         /// Extract the angle from this matrix (assumed to be
 270         /// a rotation matrix).
 271         float32 GetAngle() const
 272         {
 273                 return b2Atan2(col1.y, col1.x);
 274         }
 275
 276 #ifdef TARGET_FLOAT32_IS_FIXED
 277
 278         /// Compute the inverse of this matrix, such that inv(A) * A = identity.
 279         b2Mat22 Invert() const
 280         {
 281                 float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
 282                 float32 det = a * d - b * c;
 283                 b2Mat22 B;
 284                 int n = 0;
 285
 286                 if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
 287                 {
 288                         n = 3;
 289                         a = a<<n; b = b<<n;
 290                         c = c<<n; d = d<<n;
 291                         det = a * d - b * c;
 292                         b2Assert(det != 0.0f);
 293                         det = float32(1) / det;
 294                         B.col1.x = ( det * d) << n;     B.col2.x = (-det * b) << n;
 295                         B.col1.y = (-det * c) << n;     B.col2.y = ( det * a) << n;
 296                 }
 297                 else
 298                 {
 299                         n = (b2Abs(det) >= 16.0)? 4 : 0;
 300                         b2Assert(det != 0.0f);
 301                         det = float32(1<<n) / det;
 302                         B.col1.x = ( det * d) >> n;     B.col2.x = (-det * b) >> n;
 303                         B.col1.y = (-det * c) >> n;     B.col2.y = ( det * a) >> n;
 304                 }
 305
 306                 return B;
 307         }
 308
 309         // Solve A * x = b
 310         b2Vec2 Solve(const b2Vec2& b) const
 311         {
 312                 float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
 313                 float32 det = a11 * a22 - a12 * a21;
 314                 int n = 0;
 315                 b2Vec2 x;
 316
 317
 318                 if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
 319                 {
 320                         n = 3;
 321                         a11 = col1.x<<n; a12 = col2.x<<n;
 322                         a21 = col1.y<<n; a22 = col2.y<<n;
 323                         det = a11 * a22 - a12 * a21;
 324                         b2Assert(det != 0.0f);
 325                         det = float32(1) / det;
 326                         x.x = (det * (a22 * b.x - a12 * b.y)) << n;
 327                         x.y = (det * (a11 * b.y - a21 * b.x)) << n;
 328                 }
 329                 else
 330                 {
 331                         n = (b2Abs(det) >= 16.0) ? 4 : 0;
 332                         b2Assert(det != 0.0f);
 333                         det = float32(1<<n) / det;
 334                         x.x = (det * (a22 * b.x - a12 * b.y)) >> n;
 335                         x.y = (det * (a11 * b.y - a21 * b.x)) >> n;
 336                 }
 337
 338                 return x;
 339         }
 340
 341 #else
 342         b2Mat22 Invert() const
 343         {
 344                 float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
 345                 b2Mat22 B;
 346                 float32 det = a * d - b * c;
 347                 b2Assert(det != 0.0f);
 348                 det = float32(1.0f) / det;
 349                 B.col1.x =  det * d;    B.col2.x = -det * b;
 350                 B.col1.y = -det * c;    B.col2.y =  det * a;
 351                 return B;
 352         }
 353
 354         /// Solve A * x = b, where b is a column vector. This is more efficient
 355         /// than computing the inverse in one-shot cases.
 356         b2Vec2 Solve(const b2Vec2& b) const
 357         {
 358                 float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
 359                 float32 det = a11 * a22 - a12 * a21;
 360                 b2Assert(det != 0.0f);
 361                 det = 1.0f / det;
 362                 b2Vec2 x;
 363                 x.x = det * (a22 * b.x - a12 * b.y);
 364                 x.y = det * (a11 * b.y - a21 * b.x);
 365                 return x;
 366         }
 367 #endif
 368
 369         b2Vec2 col1, col2;
 370 };
 371
 372 /// A transform contains translation and rotation. It is used to represent
 373 /// the position and orientation of rigid frames.
 374 struct b2XForm
 375 {
 376         /// The default constructor does nothing (for performance).
 377         b2XForm() {}
 378
 379         /// Initialize using a position vector and a rotation matrix.
 380         b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
 381
 382         /// Set this to the identity transform.
 383         void SetIdentity()
 384         {
 385                 position.SetZero();
 386                 R.SetIdentity();
 387         }
 388
 389         b2Vec2 position;
 390         b2Mat22 R;
 391 };
 392
 393 /// This describes the motion of a body/shape for TOI computation.
 394 /// Shapes are defined with respect to the body origin, which may
 395 /// no coincide with the center of mass. However, to support dynamics
 396 /// we must interpolate the center of mass position.
 397 struct b2Sweep
 398 {
 399         /// Get the interpolated transform at a specific time.
 400         /// @param t the normalized time in [0,1].
 401         void GetXForm(b2XForm* xf, float32 t) const;
 402
 403         /// Advance the sweep forward, yielding a new initial state.
 404         /// @param t the new initial time.
 405         void Advance(float32 t);
 406
 407         b2Vec2 localCenter;     ///< local center of mass position
 408         b2Vec2 c0, c;           ///< center world positions
 409         float32 a0, a;          ///< world angles
 410         float32 t0;                     ///< time interval = [t0,1], where t0 is in [0,1]
 411 };
 412
 413
 414 extern const b2Vec2 b2Vec2_zero;
 415 extern const b2Mat22 b2Mat22_identity;
 416 extern const b2XForm b2XForm_identity;
 417
 418 /// Peform the dot product on two vectors.
 419 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
 420 {
 421         return a.x * b.x + a.y * b.y;
 422 }
 423
 424 /// Perform the cross product on two vectors. In 2D this produces a scalar.
 425 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
 426 {
 427         return a.x * b.y - a.y * b.x;
 428 }
 429
 430 /// Perform the cross product on a vector and a scalar. In 2D this produces
 431 /// a vector.
 432 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
 433 {
 434         b2Vec2 v; v.Set(s * a.y, -s * a.x);
 435         return v;
 436 }
 437
 438 /// Perform the cross product on a scalar and a vector. In 2D this produces
 439 /// a vector.
 440 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
 441 {
 442         b2Vec2 v; v.Set(-s * a.y, s * a.x);
 443         return v;
 444 }
 445
 446 /// Multiply a matrix times a vector. If a rotation matrix is provided,
 447 /// then this transforms the vector from one frame to another.
 448 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
 449 {
 450         b2Vec2 u;
 451         u.Set(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
 452         return u;
 453 }
 454
 455 /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
 456 /// then this transforms the vector from one frame to another (inverse transform).
 457 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
 458 {
 459         b2Vec2 u;
 460         u.Set(b2Dot(v, A.col1), b2Dot(v, A.col2));
 461         return u;
 462 }
 463
 464 /// Add two vectors component-wise.
 465 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
 466 {
 467         b2Vec2 v; v.Set(a.x + b.x, a.y + b.y);
 468         return v;
 469 }
 470
 471 /// Subtract two vectors component-wise.
 472 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
 473 {
 474         b2Vec2 v; v.Set(a.x - b.x, a.y - b.y);
 475         return v;
 476 }
 477
 478 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
 479 {
 480         b2Vec2 v; v.Set(s * a.x, s * a.y);
 481         return v;
 482 }
 483
 484 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
 485 {
 486         return a.x == b.x && a.y == b.y;
 487 }
 488
 489 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
 490 {
 491         b2Vec2 c = a - b;
 492         return c.Length();
 493 }
 494
 495 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
 496 {
 497         b2Vec2 c = a - b;
 498         return b2Dot(c, c);
 499 }
 500
 501 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
 502 {
 503         b2Mat22 C;
 504         C.Set(A.col1 + B.col1, A.col2 + B.col2);
 505         return C;
 506 }
 507
 508 // A * B
 509 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
 510 {
 511         b2Mat22 C;
 512         C.Set(b2Mul(A, B.col1), b2Mul(A, B.col2));
 513         return C;
 514 }
 515
 516 // A^T * B
 517 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
 518 {
 519         b2Vec2 c1; c1.Set(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
 520         b2Vec2 c2; c2.Set(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
 521         b2Mat22 C;
 522         C.Set(c1, c2);
 523         return C;
 524 }
 525
 526 inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
 527 {
 528         return T.position + b2Mul(T.R, v);
 529 }
 530
 531 inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& v)
 532 {
 533         return b2MulT(T.R, v - T.position);
 534 }
 535
 536 inline b2Vec2 b2Abs(const b2Vec2& a)
 537 {
 538         b2Vec2 b; b.Set(b2Abs(a.x), b2Abs(a.y));
 539         return b;
 540 }
 541
 542 inline b2Mat22 b2Abs(const b2Mat22& A)
 543 {
 544         b2Mat22 B;
 545         B.Set(b2Abs(A.col1), b2Abs(A.col2));
 546         return B;
 547 }
 548
 549 template <typename T>
 550 inline T b2Min(T a, T b)
 551 {
 552         return a < b ? a : b;
 553 }
 554
 555 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
 556 {
 557         b2Vec2 c;
 558         c.x = b2Min(a.x, b.x);
 559         c.y = b2Min(a.y, b.y);
 560         return c;
 561 }
 562
 563 template <typename T>
 564 inline T b2Max(T a, T b)
 565 {
 566         return a > b ? a : b;
 567 }
 568
 569 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
 570 {
 571         b2Vec2 c;
 572         c.x = b2Max(a.x, b.x);
 573         c.y = b2Max(a.y, b.y);
 574         return c;
 575 }
 576
 577 template <typename T>
 578 inline T b2Clamp(T a, T low, T high)
 579 {
 580         return b2Max(low, b2Min(a, high));
 581 }
 582
 583 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
 584 {
 585         return b2Max(low, b2Min(a, high));
 586 }
 587
 588 template<typename T> inline void b2Swap(T& a, T& b)
 589 {
 590         T tmp = a;
 591         a = b;
 592         b = tmp;
 593 }
 594
 595 #define RAND_LIMIT      32767
 596
 597 // Random number in range [-1,1]
 598 inline float32 b2Random()
 599 {
 600         float32 r = (float32)(rand() & (RAND_LIMIT));
 601         r /= RAND_LIMIT;
 602         r = 2.0f * r - 1.0f;
 603         return r;
 604 }
 605
 606 /// Random floating point number in range [lo, hi]
 607 inline float32 b2Random(float32 lo, float32 hi)
 608 {
 609         float32 r = (float32)(rand() & (RAND_LIMIT));
 610         r /= RAND_LIMIT;
 611         r = (hi - lo) * r + lo;
 612         return r;
 613 }
 614
 615 /// "Next Largest Power of 2
 616 /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
 617 /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
 618 /// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
 619 /// largest power of 2. For a 32-bit value:"
 620 inline uint32 b2NextPowerOfTwo(uint32 x)
 621 {
 622         x |= (x >> 1);
 623         x |= (x >> 2);
 624         x |= (x >> 4);
 625         x |= (x >> 8);
 626         x |= (x >> 16);
 627         return x + 1;
 628 }
 629
 630 inline bool b2IsPowerOfTwo(uint32 x)
 631 {
 632         bool result = x > 0 && (x & (x - 1)) == 0;
 633         return result;
 634 }
 635
 636 #endif