File indexing completed on 2024-05-19 14:56:23
0001 /* 0002 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #ifndef B2_MATH_H 0020 #define B2_MATH_H 0021 0022 #include <Box2D/Common/b2Settings.h> 0023 #include <math.h> 0024 0025 /// This function is used to ensure that a floating point number is not a NaN or infinity. 0026 inline bool b2IsValid(float32 x) 0027 { 0028 int32 ix = *reinterpret_cast<int32*>(&x); 0029 return (ix & 0x7f800000) != 0x7f800000; 0030 } 0031 0032 /// This is a approximate yet fast inverse square-root. 0033 inline float32 b2InvSqrt(float32 x) 0034 { 0035 union 0036 { 0037 float32 x; 0038 int32 i; 0039 } convert; 0040 0041 convert.x = x; 0042 float32 xhalf = 0.5f * x; 0043 convert.i = 0x5f3759df - (convert.i >> 1); 0044 x = convert.x; 0045 x = x * (1.5f - xhalf * x * x); 0046 return x; 0047 } 0048 0049 #define b2Sqrt(x) sqrtf(x) 0050 #define b2Atan2(y, x) atan2f(y, x) 0051 0052 /// A 2D column vector. 0053 struct b2Vec2 0054 { 0055 /// Default constructor does nothing (for performance). 0056 b2Vec2() {} 0057 0058 /// Construct using coordinates. 0059 b2Vec2(float32 x, float32 y) : x(x), y(y) {} 0060 0061 /// Set this vector to all zeros. 0062 void SetZero() { x = 0.0f; y = 0.0f; } 0063 0064 /// Set this vector to some specified coordinates. 0065 void Set(float32 x_, float32 y_) { x = x_; y = y_; } 0066 0067 /// Negate this vector. 0068 b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } 0069 0070 /// Read from and indexed element. 0071 float32 operator () (int32 i) const 0072 { 0073 return (&x)[i]; 0074 } 0075 0076 /// Write to an indexed element. 0077 float32& operator () (int32 i) 0078 { 0079 return (&x)[i]; 0080 } 0081 0082 /// Add a vector to this vector. 0083 void operator += (const b2Vec2& v) 0084 { 0085 x += v.x; y += v.y; 0086 } 0087 0088 /// Subtract a vector from this vector. 0089 void operator -= (const b2Vec2& v) 0090 { 0091 x -= v.x; y -= v.y; 0092 } 0093 0094 /// Multiply this vector by a scalar. 0095 void operator *= (float32 a) 0096 { 0097 x *= a; y *= a; 0098 } 0099 0100 /// Get the length of this vector (the norm). 0101 float32 Length() const 0102 { 0103 return b2Sqrt(x * x + y * y); 0104 } 0105 0106 /// Get the length squared. For performance, use this instead of 0107 /// b2Vec2::Length (if possible). 0108 float32 LengthSquared() const 0109 { 0110 return x * x + y * y; 0111 } 0112 0113 /// Convert this vector into a unit vector. Returns the length. 0114 float32 Normalize() 0115 { 0116 float32 length = Length(); 0117 if (length < b2_epsilon) 0118 { 0119 return 0.0f; 0120 } 0121 float32 invLength = 1.0f / length; 0122 x *= invLength; 0123 y *= invLength; 0124 0125 return length; 0126 } 0127 0128 /// Does this vector contain finite coordinates? 0129 bool IsValid() const 0130 { 0131 return b2IsValid(x) && b2IsValid(y); 0132 } 0133 0134 /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) 0135 b2Vec2 Skew() const 0136 { 0137 return b2Vec2(-y, x); 0138 } 0139 0140 float32 x, y; 0141 }; 0142 0143 /// A 2D column vector with 3 elements. 0144 struct b2Vec3 0145 { 0146 /// Default constructor does nothing (for performance). 0147 b2Vec3() {} 0148 0149 /// Construct using coordinates. 0150 b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {} 0151 0152 /// Set this vector to all zeros. 0153 void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } 0154 0155 /// Set this vector to some specified coordinates. 0156 void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } 0157 0158 /// Negate this vector. 0159 b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } 0160 0161 /// Add a vector to this vector. 0162 void operator += (const b2Vec3& v) 0163 { 0164 x += v.x; y += v.y; z += v.z; 0165 } 0166 0167 /// Subtract a vector from this vector. 0168 void operator -= (const b2Vec3& v) 0169 { 0170 x -= v.x; y -= v.y; z -= v.z; 0171 } 0172 0173 /// Multiply this vector by a scalar. 0174 void operator *= (float32 s) 0175 { 0176 x *= s; y *= s; z *= s; 0177 } 0178 0179 float32 x, y, z; 0180 }; 0181 0182 /// A 2-by-2 matrix. Stored in column-major order. 0183 struct b2Mat22 0184 { 0185 /// The default constructor does nothing (for performance). 0186 b2Mat22() {} 0187 0188 /// Construct this matrix using columns. 0189 b2Mat22(const b2Vec2& c1, const b2Vec2& c2) 0190 { 0191 ex = c1; 0192 ey = c2; 0193 } 0194 0195 /// Construct this matrix using scalars. 0196 b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) 0197 { 0198 ex.x = a11; ex.y = a21; 0199 ey.x = a12; ey.y = a22; 0200 } 0201 0202 /// Initialize this matrix using columns. 0203 void Set(const b2Vec2& c1, const b2Vec2& c2) 0204 { 0205 ex = c1; 0206 ey = c2; 0207 } 0208 0209 /// Set this to the identity matrix. 0210 void SetIdentity() 0211 { 0212 ex.x = 1.0f; ey.x = 0.0f; 0213 ex.y = 0.0f; ey.y = 1.0f; 0214 } 0215 0216 /// Set this matrix to all zeros. 0217 void SetZero() 0218 { 0219 ex.x = 0.0f; ey.x = 0.0f; 0220 ex.y = 0.0f; ey.y = 0.0f; 0221 } 0222 0223 b2Mat22 GetInverse() const 0224 { 0225 float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; 0226 b2Mat22 B; 0227 float32 det = a * d - b * c; 0228 if (det != 0.0f) 0229 { 0230 det = 1.0f / det; 0231 } 0232 B.ex.x = det * d; B.ey.x = -det * b; 0233 B.ex.y = -det * c; B.ey.y = det * a; 0234 return B; 0235 } 0236 0237 /// Solve A * x = b, where b is a column vector. This is more efficient 0238 /// than computing the inverse in one-shot cases. 0239 b2Vec2 Solve(const b2Vec2& b) const 0240 { 0241 float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; 0242 float32 det = a11 * a22 - a12 * a21; 0243 if (det != 0.0f) 0244 { 0245 det = 1.0f / det; 0246 } 0247 b2Vec2 x; 0248 x.x = det * (a22 * b.x - a12 * b.y); 0249 x.y = det * (a11 * b.y - a21 * b.x); 0250 return x; 0251 } 0252 0253 b2Vec2 ex, ey; 0254 }; 0255 0256 /// A 3-by-3 matrix. Stored in column-major order. 0257 struct b2Mat33 0258 { 0259 /// The default constructor does nothing (for performance). 0260 b2Mat33() {} 0261 0262 /// Construct this matrix using columns. 0263 b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) 0264 { 0265 ex = c1; 0266 ey = c2; 0267 ez = c3; 0268 } 0269 0270 /// Set this matrix to all zeros. 0271 void SetZero() 0272 { 0273 ex.SetZero(); 0274 ey.SetZero(); 0275 ez.SetZero(); 0276 } 0277 0278 /// Solve A * x = b, where b is a column vector. This is more efficient 0279 /// than computing the inverse in one-shot cases. 0280 b2Vec3 Solve33(const b2Vec3& b) const; 0281 0282 /// Solve A * x = b, where b is a column vector. This is more efficient 0283 /// than computing the inverse in one-shot cases. Solve only the upper 0284 /// 2-by-2 matrix equation. 0285 b2Vec2 Solve22(const b2Vec2& b) const; 0286 0287 /// Get the inverse of this matrix as a 2-by-2. 0288 /// Returns the zero matrix if singular. 0289 void GetInverse22(b2Mat33* M) const; 0290 0291 /// Get the symmetric inverse of this matrix as a 3-by-3. 0292 /// Returns the zero matrix if singular. 0293 void GetSymInverse33(b2Mat33* M) const; 0294 0295 b2Vec3 ex, ey, ez; 0296 }; 0297 0298 /// Rotation 0299 struct b2Rot 0300 { 0301 b2Rot() {} 0302 0303 /// Initialize from an angle in radians 0304 explicit b2Rot(float32 angle) 0305 { 0306 /// TODO_ERIN optimize 0307 s = sinf(angle); 0308 c = cosf(angle); 0309 } 0310 0311 /// Set using an angle in radians. 0312 void Set(float32 angle) 0313 { 0314 /// TODO_ERIN optimize 0315 s = sinf(angle); 0316 c = cosf(angle); 0317 } 0318 0319 /// Set to the identity rotation 0320 void SetIdentity() 0321 { 0322 s = 0.0f; 0323 c = 1.0f; 0324 } 0325 0326 /// Get the angle in radians 0327 float32 GetAngle() const 0328 { 0329 return b2Atan2(s, c); 0330 } 0331 0332 /// Get the x-axis 0333 b2Vec2 GetXAxis() const 0334 { 0335 return b2Vec2(c, s); 0336 } 0337 0338 /// Get the u-axis 0339 b2Vec2 GetYAxis() const 0340 { 0341 return b2Vec2(-s, c); 0342 } 0343 0344 /// Sine and cosine 0345 float32 s, c; 0346 }; 0347 0348 /// A transform contains translation and rotation. It is used to represent 0349 /// the position and orientation of rigid frames. 0350 struct b2Transform 0351 { 0352 /// The default constructor does nothing. 0353 b2Transform() {} 0354 0355 /// Initialize using a position vector and a rotation. 0356 b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} 0357 0358 /// Set this to the identity transform. 0359 void SetIdentity() 0360 { 0361 p.SetZero(); 0362 q.SetIdentity(); 0363 } 0364 0365 /// Set this based on the position and angle. 0366 void Set(const b2Vec2& position, float32 angle) 0367 { 0368 p = position; 0369 q.Set(angle); 0370 } 0371 0372 b2Vec2 p; 0373 b2Rot q; 0374 }; 0375 0376 /// This describes the motion of a body/shape for TOI computation. 0377 /// Shapes are defined with respect to the body origin, which may 0378 /// no coincide with the center of mass. However, to support dynamics 0379 /// we must interpolate the center of mass position. 0380 struct b2Sweep 0381 { 0382 /// Get the interpolated transform at a specific time. 0383 /// @param beta is a factor in [0,1], where 0 indicates alpha0. 0384 void GetTransform(b2Transform* xfb, float32 beta) const; 0385 0386 /// Advance the sweep forward, yielding a new initial state. 0387 /// @param alpha the new initial time. 0388 void Advance(float32 alpha); 0389 0390 /// Normalize the angles. 0391 void Normalize(); 0392 0393 b2Vec2 localCenter; ///< local center of mass position 0394 b2Vec2 c0, c; ///< center world positions 0395 float32 a0, a; ///< world angles 0396 0397 /// Fraction of the current time step in the range [0,1] 0398 /// c0 and a0 are the positions at alpha0. 0399 float32 alpha0; 0400 }; 0401 0402 /// Useful constant 0403 extern const b2Vec2 b2Vec2_zero; 0404 0405 /// Perform the dot product on two vectors. 0406 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) 0407 { 0408 return a.x * b.x + a.y * b.y; 0409 } 0410 0411 /// Perform the cross product on two vectors. In 2D this produces a scalar. 0412 inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b) 0413 { 0414 return a.x * b.y - a.y * b.x; 0415 } 0416 0417 /// Perform the cross product on a vector and a scalar. In 2D this produces 0418 /// a vector. 0419 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s) 0420 { 0421 return b2Vec2(s * a.y, -s * a.x); 0422 } 0423 0424 /// Perform the cross product on a scalar and a vector. In 2D this produces 0425 /// a vector. 0426 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) 0427 { 0428 return b2Vec2(-s * a.y, s * a.x); 0429 } 0430 0431 /// Multiply a matrix times a vector. If a rotation matrix is provided, 0432 /// then this transforms the vector from one frame to another. 0433 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) 0434 { 0435 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); 0436 } 0437 0438 /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, 0439 /// then this transforms the vector from one frame to another (inverse transform). 0440 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) 0441 { 0442 return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); 0443 } 0444 0445 /// Add two vectors component-wise. 0446 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) 0447 { 0448 return b2Vec2(a.x + b.x, a.y + b.y); 0449 } 0450 0451 /// Subtract two vectors component-wise. 0452 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) 0453 { 0454 return b2Vec2(a.x - b.x, a.y - b.y); 0455 } 0456 0457 inline b2Vec2 operator * (float32 s, const b2Vec2& a) 0458 { 0459 return b2Vec2(s * a.x, s * a.y); 0460 } 0461 0462 inline bool operator == (const b2Vec2& a, const b2Vec2& b) 0463 { 0464 return a.x == b.x && a.y == b.y; 0465 } 0466 0467 inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b) 0468 { 0469 b2Vec2 c = a - b; 0470 return c.Length(); 0471 } 0472 0473 inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) 0474 { 0475 b2Vec2 c = a - b; 0476 return b2Dot(c, c); 0477 } 0478 0479 inline b2Vec3 operator * (float32 s, const b2Vec3& a) 0480 { 0481 return b2Vec3(s * a.x, s * a.y, s * a.z); 0482 } 0483 0484 /// Add two vectors component-wise. 0485 inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) 0486 { 0487 return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); 0488 } 0489 0490 /// Subtract two vectors component-wise. 0491 inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) 0492 { 0493 return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); 0494 } 0495 0496 /// Perform the dot product on two vectors. 0497 inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) 0498 { 0499 return a.x * b.x + a.y * b.y + a.z * b.z; 0500 } 0501 0502 /// Perform the cross product on two vectors. 0503 inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) 0504 { 0505 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); 0506 } 0507 0508 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) 0509 { 0510 return b2Mat22(A.ex + B.ex, A.ey + B.ey); 0511 } 0512 0513 // A * B 0514 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) 0515 { 0516 return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); 0517 } 0518 0519 // A^T * B 0520 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) 0521 { 0522 b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); 0523 b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); 0524 return b2Mat22(c1, c2); 0525 } 0526 0527 /// Multiply a matrix times a vector. 0528 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) 0529 { 0530 return v.x * A.ex + v.y * A.ey + v.z * A.ez; 0531 } 0532 0533 /// Multiply a matrix times a vector. 0534 inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) 0535 { 0536 return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); 0537 } 0538 0539 /// Multiply two rotations: q * r 0540 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) 0541 { 0542 // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] 0543 // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] 0544 // s = qs * rc + qc * rs 0545 // c = qc * rc - qs * rs 0546 b2Rot qr; 0547 qr.s = q.s * r.c + q.c * r.s; 0548 qr.c = q.c * r.c - q.s * r.s; 0549 return qr; 0550 } 0551 0552 /// Transpose multiply two rotations: qT * r 0553 inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) 0554 { 0555 // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] 0556 // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] 0557 // s = qc * rs - qs * rc 0558 // c = qc * rc + qs * rs 0559 b2Rot qr; 0560 qr.s = q.c * r.s - q.s * r.c; 0561 qr.c = q.c * r.c + q.s * r.s; 0562 return qr; 0563 } 0564 0565 /// Rotate a vector 0566 inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) 0567 { 0568 return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); 0569 } 0570 0571 /// Inverse rotate a vector 0572 inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) 0573 { 0574 return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); 0575 } 0576 0577 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) 0578 { 0579 float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; 0580 float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; 0581 0582 return b2Vec2(x, y); 0583 } 0584 0585 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) 0586 { 0587 float32 px = v.x - T.p.x; 0588 float32 py = v.y - T.p.y; 0589 float32 x = (T.q.c * px + T.q.s * py); 0590 float32 y = (-T.q.s * px + T.q.c * py); 0591 0592 return b2Vec2(x, y); 0593 } 0594 0595 // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p 0596 // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p 0597 inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) 0598 { 0599 b2Transform C; 0600 C.q = b2Mul(A.q, B.q); 0601 C.p = b2Mul(A.q, B.p) + A.p; 0602 return C; 0603 } 0604 0605 // v2 = A.q' * (B.q * v1 + B.p - A.p) 0606 // = A.q' * B.q * v1 + A.q' * (B.p - A.p) 0607 inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) 0608 { 0609 b2Transform C; 0610 C.q = b2MulT(A.q, B.q); 0611 C.p = b2MulT(A.q, B.p - A.p); 0612 return C; 0613 } 0614 0615 template <typename T> 0616 inline T b2Abs(T a) 0617 { 0618 return a > T(0) ? a : -a; 0619 } 0620 0621 inline b2Vec2 b2Abs(const b2Vec2& a) 0622 { 0623 return b2Vec2(b2Abs(a.x), b2Abs(a.y)); 0624 } 0625 0626 inline b2Mat22 b2Abs(const b2Mat22& A) 0627 { 0628 return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); 0629 } 0630 0631 template <typename T> 0632 inline T b2Min(T a, T b) 0633 { 0634 return a < b ? a : b; 0635 } 0636 0637 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) 0638 { 0639 return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); 0640 } 0641 0642 template <typename T> 0643 inline T b2Max(T a, T b) 0644 { 0645 return a > b ? a : b; 0646 } 0647 0648 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) 0649 { 0650 return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); 0651 } 0652 0653 template <typename T> 0654 inline T b2Clamp(T a, T low, T high) 0655 { 0656 return b2Max(low, b2Min(a, high)); 0657 } 0658 0659 inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) 0660 { 0661 return b2Max(low, b2Min(a, high)); 0662 } 0663 0664 template<typename T> inline void b2Swap(T& a, T& b) 0665 { 0666 T tmp = a; 0667 a = b; 0668 b = tmp; 0669 } 0670 0671 /// "Next Largest Power of 2 0672 /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm 0673 /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with 0674 /// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next 0675 /// largest power of 2. For a 32-bit value:" 0676 inline uint32 b2NextPowerOfTwo(uint32 x) 0677 { 0678 x |= (x >> 1); 0679 x |= (x >> 2); 0680 x |= (x >> 4); 0681 x |= (x >> 8); 0682 x |= (x >> 16); 0683 return x + 1; 0684 } 0685 0686 inline bool b2IsPowerOfTwo(uint32 x) 0687 { 0688 bool result = x > 0 && (x & (x - 1)) == 0; 0689 return result; 0690 } 0691 0692 inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const 0693 { 0694 xf->p = (1.0f - beta) * c0 + beta * c; 0695 float32 angle = (1.0f - beta) * a0 + beta * a; 0696 xf->q.Set(angle); 0697 0698 // Shift to origin 0699 xf->p -= b2Mul(xf->q, localCenter); 0700 } 0701 0702 inline void b2Sweep::Advance(float32 alpha) 0703 { 0704 b2Assert(alpha0 < 1.0f); 0705 float32 beta = (alpha - alpha0) / (1.0f - alpha0); 0706 c0 += beta * (c - c0); 0707 a0 += beta * (a - a0); 0708 alpha0 = alpha; 0709 } 0710 0711 /// Normalize an angle in radians to be between -pi and pi 0712 inline void b2Sweep::Normalize() 0713 { 0714 float32 twoPi = 2.0f * b2_pi; 0715 float32 d = twoPi * floorf(a0 / twoPi); 0716 a0 -= d; 0717 a -= d; 0718 } 0719 0720 #endif