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
b2Min(const Fixed & a,const Fixed & b)31 inline Fixed b2Min(const Fixed& a, const Fixed& b)
32 {
33 return a < b ? a : b;
34 }
35
b2Max(const Fixed & a,const Fixed & b)36 inline Fixed b2Max(const Fixed& a, const Fixed& b)
37 {
38 return a > b ? a : b;
39 }
40
b2Clamp(Fixed a,Fixed low,Fixed high)41 inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
42 {
43 return b2Max(low, b2Min(a, high));
44 }
45
b2IsValid(Fixed x)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.
b2IsValid(float32 x)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.
b2InvSqrt(float32 x)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
b2Abs(float32 a)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).
b2Vec2b2Vec299 b2Vec2() {}
100
101 /// Construct using coordinates.
b2Vec2b2Vec2102 b2Vec2(float32 x, float32 y) : x(x), y(y) {}
103
104 /// Set this vector to all zeros.
SetZerob2Vec2105 void SetZero() { x = 0.0f; y = 0.0f; }
106
107 /// Set this vector to some specified coordinates.
Setb2Vec2108 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).
Lengthb2Vec2132 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).
LengthSquaredb2Vec2152 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
Normalizeb2Vec2159 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
Normalizeb2Vec2186 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?
IsValidb2Vec2202 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).
b2Mat22b2Mat22214 b2Mat22() {}
215
216 /// Construct this matrix using columns.
b2Mat22b2Mat22217 b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
218 {
219 col1 = c1;
220 col2 = c2;
221 }
222
223 /// Construct this matrix using scalars.
b2Mat22b2Mat22224 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.
b2Mat22b2Mat22232 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.
Setb2Mat22240 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.
Setb2Mat22248 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.
SetIdentityb2Mat22256 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.
SetZerob2Mat22263 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).
GetAngleb2Mat22271 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.
Invertb2Mat22279 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
Solveb2Mat22310 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
Invertb2Mat22342 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.
Solveb2Mat22356 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).
b2XFormb2XForm377 b2XForm() {}
378
379 /// Initialize using a position vector and a rotation matrix.
b2XFormb2XForm380 b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
381
382 /// Set this to the identity transform.
SetIdentityb2XForm383 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.
b2Dot(const b2Vec2 & a,const b2Vec2 & b)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.
b2Cross(const b2Vec2 & a,const b2Vec2 & b)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.
b2Cross(const b2Vec2 & a,float32 s)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.
b2Cross(float32 s,const b2Vec2 & a)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.
b2Mul(const b2Mat22 & A,const b2Vec2 & v)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).
b2MulT(const b2Mat22 & A,const b2Vec2 & v)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
b2Distance(const b2Vec2 & a,const b2Vec2 & b)489 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
490 {
491 b2Vec2 c = a - b;
492 return c.Length();
493 }
494
b2DistanceSquared(const b2Vec2 & a,const b2Vec2 & b)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
b2Mul(const b2Mat22 & A,const b2Mat22 & 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
b2MulT(const b2Mat22 & A,const b2Mat22 & 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
b2Mul(const b2XForm & T,const b2Vec2 & v)526 inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
527 {
528 return T.position + b2Mul(T.R, v);
529 }
530
b2MulT(const b2XForm & T,const b2Vec2 & v)531 inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& v)
532 {
533 return b2MulT(T.R, v - T.position);
534 }
535
b2Abs(const b2Vec2 & a)536 inline b2Vec2 b2Abs(const b2Vec2& a)
537 {
538 b2Vec2 b; b.Set(b2Abs(a.x), b2Abs(a.y));
539 return b;
540 }
541
b2Abs(const b2Mat22 & A)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>
b2Min(T a,T b)550 inline T b2Min(T a, T b)
551 {
552 return a < b ? a : b;
553 }
554
b2Min(const b2Vec2 & a,const b2Vec2 & b)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>
b2Max(T a,T b)564 inline T b2Max(T a, T b)
565 {
566 return a > b ? a : b;
567 }
568
b2Max(const b2Vec2 & a,const b2Vec2 & b)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>
b2Clamp(T a,T low,T high)578 inline T b2Clamp(T a, T low, T high)
579 {
580 return b2Max(low, b2Min(a, high));
581 }
582
b2Clamp(const b2Vec2 & a,const b2Vec2 & low,const b2Vec2 & high)583 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
584 {
585 return b2Max(low, b2Min(a, high));
586 }
587
b2Swap(T & a,T & b)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]
b2Random()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]
b2Random(float32 lo,float32 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:"
b2NextPowerOfTwo(uint32 x)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
b2IsPowerOfTwo(uint32 x)630 inline bool b2IsPowerOfTwo(uint32 x)
631 {
632 bool result = x > 0 && (x & (x - 1)) == 0;
633 return result;
634 }
635
636 #endif
637