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