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.
b2IsValid(float32 x)26 inline 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.
b2InvSqrt(float32 x)33 inline 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.
53 struct b2Vec2
54 {
55 /// Default constructor does nothing (for performance).
b2Vec2b2Vec256 b2Vec2() {}
57
58 /// Construct using coordinates.
b2Vec2b2Vec259 b2Vec2(float32 x, float32 y) : x(x), y(y) {}
60
61 /// Set this vector to all zeros.
SetZerob2Vec262 void SetZero() { x = 0.0f; y = 0.0f; }
63
64 /// Set this vector to some specified coordinates.
Setb2Vec265 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.
operatorb2Vec271 float32 operator () (int32 i) const
72 {
73 return (&x)[i];
74 }
75
76 /// Write to an indexed element.
operatorb2Vec277 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).
Lengthb2Vec2101 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).
LengthSquaredb2Vec2108 float32 LengthSquared() const
109 {
110 return x * x + y * y;
111 }
112
113 /// Convert this vector into a unit vector. Returns the length.
Normalizeb2Vec2114 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?
IsValidb2Vec2129 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)
Skewb2Vec2135 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.
144 struct b2Vec3
145 {
146 /// Default constructor does nothing (for performance).
b2Vec3b2Vec3147 b2Vec3() {}
148
149 /// Construct using coordinates.
b2Vec3b2Vec3150 b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
151
152 /// Set this vector to all zeros.
SetZerob2Vec3153 void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
154
155 /// Set this vector to some specified coordinates.
Setb2Vec3156 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.
183 struct b2Mat22
184 {
185 /// The default constructor does nothing (for performance).
b2Mat22b2Mat22186 b2Mat22() {}
187
188 /// Construct this matrix using columns.
b2Mat22b2Mat22189 b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
190 {
191 ex = c1;
192 ey = c2;
193 }
194
195 /// Construct this matrix using scalars.
b2Mat22b2Mat22196 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.
Setb2Mat22203 void Set(const b2Vec2& c1, const b2Vec2& c2)
204 {
205 ex = c1;
206 ey = c2;
207 }
208
209 /// Set this to the identity matrix.
SetIdentityb2Mat22210 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.
SetZerob2Mat22217 void SetZero()
218 {
219 ex.x = 0.0f; ey.x = 0.0f;
220 ex.y = 0.0f; ey.y = 0.0f;
221 }
222
GetInverseb2Mat22223 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.
Solveb2Mat22239 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.
257 struct b2Mat33
258 {
259 /// The default constructor does nothing (for performance).
b2Mat33b2Mat33260 b2Mat33() {}
261
262 /// Construct this matrix using columns.
b2Mat33b2Mat33263 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.
SetZerob2Mat33271 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
299 struct b2Rot
300 {
b2Rotb2Rot301 b2Rot() {}
302
303 /// Initialize from an angle in radians
b2Rotb2Rot304 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.
Setb2Rot312 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
SetIdentityb2Rot320 void SetIdentity()
321 {
322 s = 0.0f;
323 c = 1.0f;
324 }
325
326 /// Get the angle in radians
GetAngleb2Rot327 float32 GetAngle() const
328 {
329 return b2Atan2(s, c);
330 }
331
332 /// Get the x-axis
GetXAxisb2Rot333 b2Vec2 GetXAxis() const
334 {
335 return b2Vec2(c, s);
336 }
337
338 /// Get the u-axis
GetYAxisb2Rot339 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.
350 struct b2Transform
351 {
352 /// The default constructor does nothing.
b2Transformb2Transform353 b2Transform() {}
354
355 /// Initialize using a position vector and a rotation.
b2Transformb2Transform356 b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
357
358 /// Set this to the identity transform.
SetIdentityb2Transform359 void SetIdentity()
360 {
361 p.SetZero();
362 q.SetIdentity();
363 }
364
365 /// Set this based on the position and angle.
Setb2Transform366 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.
380 struct 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
403 extern const b2Vec2 b2Vec2_zero;
404
405 /// Perform the dot product on two vectors.
b2Dot(const b2Vec2 & a,const b2Vec2 & b)406 inline 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.
b2Cross(const b2Vec2 & a,const b2Vec2 & b)412 inline 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.
b2Cross(const b2Vec2 & a,float32 s)419 inline 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.
b2Cross(float32 s,const b2Vec2 & a)426 inline 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.
b2Mul(const b2Mat22 & A,const b2Vec2 & v)433 inline 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).
b2MulT(const b2Mat22 & A,const b2Vec2 & v)440 inline 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.
446 inline 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.
452 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
453 {
454 return b2Vec2(a.x - b.x, a.y - b.y);
455 }
456
457 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
458 {
459 return b2Vec2(s * a.x, s * a.y);
460 }
461
462 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
463 {
464 return a.x == b.x && a.y == b.y;
465 }
466
b2Distance(const b2Vec2 & a,const b2Vec2 & b)467 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
468 {
469 b2Vec2 c = a - b;
470 return c.Length();
471 }
472
b2DistanceSquared(const b2Vec2 & a,const b2Vec2 & b)473 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
474 {
475 b2Vec2 c = a - b;
476 return b2Dot(c, c);
477 }
478
479 inline 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.
485 inline 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.
491 inline 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.
b2Dot(const b2Vec3 & a,const b2Vec3 & b)497 inline 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.
b2Cross(const b2Vec3 & a,const b2Vec3 & b)503 inline 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
508 inline 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
b2Mul(const b2Mat22 & A,const b2Mat22 & B)514 inline 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
b2MulT(const b2Mat22 & A,const b2Mat22 & B)520 inline 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.
b2Mul(const b2Mat33 & A,const b2Vec3 & v)528 inline 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.
b2Mul22(const b2Mat33 & A,const b2Vec2 & v)534 inline 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
b2Mul(const b2Rot & q,const b2Rot & r)540 inline 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
b2MulT(const b2Rot & q,const b2Rot & r)553 inline 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
b2Mul(const b2Rot & q,const b2Vec2 & v)566 inline 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
b2MulT(const b2Rot & q,const b2Vec2 & v)572 inline 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
b2Mul(const b2Transform & T,const b2Vec2 & v)577 inline 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
b2MulT(const b2Transform & T,const b2Vec2 & v)585 inline 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
b2Mul(const b2Transform & A,const b2Transform & B)597 inline 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)
b2MulT(const b2Transform & A,const b2Transform & B)607 inline 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
615 template <typename T>
b2Abs(T a)616 inline T b2Abs(T a)
617 {
618 return a > T(0) ? a : -a;
619 }
620
b2Abs(const b2Vec2 & a)621 inline b2Vec2 b2Abs(const b2Vec2& a)
622 {
623 return b2Vec2(b2Abs(a.x), b2Abs(a.y));
624 }
625
b2Abs(const b2Mat22 & A)626 inline b2Mat22 b2Abs(const b2Mat22& A)
627 {
628 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
629 }
630
631 template <typename T>
b2Min(T a,T b)632 inline T b2Min(T a, T b)
633 {
634 return a < b ? a : b;
635 }
636
b2Min(const b2Vec2 & a,const b2Vec2 & b)637 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
638 {
639 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
640 }
641
642 template <typename T>
b2Max(T a,T b)643 inline T b2Max(T a, T b)
644 {
645 return a > b ? a : b;
646 }
647
b2Max(const b2Vec2 & a,const b2Vec2 & b)648 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
649 {
650 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
651 }
652
653 template <typename T>
b2Clamp(T a,T low,T high)654 inline T b2Clamp(T a, T low, T high)
655 {
656 return b2Max(low, b2Min(a, high));
657 }
658
b2Clamp(const b2Vec2 & a,const b2Vec2 & low,const b2Vec2 & high)659 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
660 {
661 return b2Max(low, b2Min(a, high));
662 }
663
b2Swap(T & a,T & b)664 template<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:"
b2NextPowerOfTwo(uint32 x)676 inline 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
b2IsPowerOfTwo(uint32 x)686 inline bool b2IsPowerOfTwo(uint32 x)
687 {
688 bool result = x > 0 && (x & (x - 1)) == 0;
689 return result;
690 }
691
GetTransform(b2Transform * xf,float32 beta)692 inline 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
Advance(float32 alpha)702 inline 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
Normalize()712 inline 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