b2Math.h
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:15k
- /*
- * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
- *
- * This software is provided 'as-is', without any express or implied
- * warranty. In no event will the authors be held liable for any damages
- * arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it
- * freely, subject to the following restrictions:
- * 1. The origin of this software must not be misrepresented; you must not
- * claim that you wrote the original software. If you use this software
- * in a product, an acknowledgment in the product documentation would be
- * appreciated but is not required.
- * 2. Altered source versions must be plainly marked as such, and must not be
- * misrepresented as being the original software.
- * 3. This notice may not be removed or altered from any source distribution.
- */
- #ifndef B2_MATH_H
- #define B2_MATH_H
- #include <Box2D/Common/b2Settings.h>
- #include <cmath>
- #include <cfloat>
- #include <cstddef>
- #include <limits>
- /// This function is used to ensure that a floating point number is
- /// not a NaN or infinity.
- inline bool b2IsValid(float32 x)
- {
- if (x != x)
- {
- // NaN.
- return false;
- }
- float32 infinity = std::numeric_limits<float32>::infinity();
- return -infinity < x && x < infinity;
- }
- /// This is a approximate yet fast inverse square-root.
- inline float32 b2InvSqrt(float32 x)
- {
- union
- {
- float32 x;
- int32 i;
- } convert;
- convert.x = x;
- float32 xhalf = 0.5f * x;
- convert.i = 0x5f3759df - (convert.i >> 1);
- x = convert.x;
- x = x * (1.5f - xhalf * x * x);
- return x;
- }
- #define b2Sqrt(x) sqrtf(x)
- #define b2Atan2(y, x) atan2f(y, x)
- inline float32 b2Abs(float32 a)
- {
- return a > 0.0f ? a : -a;
- }
- /// A 2D column vector.
- struct b2Vec2
- {
- /// Default constructor does nothing (for performance).
- b2Vec2() {}
- /// Construct using coordinates.
- b2Vec2(float32 x, float32 y) : x(x), y(y) {}
- /// Set this vector to all zeros.
- void SetZero() { x = 0.0f; y = 0.0f; }
- /// Set this vector to some specified coordinates.
- void Set(float32 x_, float32 y_) { x = x_; y = y_; }
- /// Negate this vector.
- b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
-
- /// Read from and indexed element.
- float32 operator () (int32 i) const
- {
- return (&x)[i];
- }
- /// Write to an indexed element.
- float32& operator () (int32 i)
- {
- return (&x)[i];
- }
- /// Add a vector to this vector.
- void operator += (const b2Vec2& v)
- {
- x += v.x; y += v.y;
- }
-
- /// Subtract a vector from this vector.
- void operator -= (const b2Vec2& v)
- {
- x -= v.x; y -= v.y;
- }
- /// Multiply this vector by a scalar.
- void operator *= (float32 a)
- {
- x *= a; y *= a;
- }
- /// Get the length of this vector (the norm).
- float32 Length() const
- {
- return b2Sqrt(x * x + y * y);
- }
- /// Get the length squared. For performance, use this instead of
- /// b2Vec2::Length (if possible).
- float32 LengthSquared() const
- {
- return x * x + y * y;
- }
- /// Convert this vector into a unit vector. Returns the length.
- float32 Normalize()
- {
- float32 length = Length();
- if (length < b2_epsilon)
- {
- return 0.0f;
- }
- float32 invLength = 1.0f / length;
- x *= invLength;
- y *= invLength;
- return length;
- }
- /// Does this vector contain finite coordinates?
- bool IsValid() const
- {
- return b2IsValid(x) && b2IsValid(y);
- }
- float32 x, y;
- };
- /// A 2D column vector with 3 elements.
- struct b2Vec3
- {
- /// Default constructor does nothing (for performance).
- b2Vec3() {}
- /// Construct using coordinates.
- b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
- /// Set this vector to all zeros.
- void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
- /// Set this vector to some specified coordinates.
- void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
- /// Negate this vector.
- b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
- /// Add a vector to this vector.
- void operator += (const b2Vec3& v)
- {
- x += v.x; y += v.y; z += v.z;
- }
- /// Subtract a vector from this vector.
- void operator -= (const b2Vec3& v)
- {
- x -= v.x; y -= v.y; z -= v.z;
- }
- /// Multiply this vector by a scalar.
- void operator *= (float32 s)
- {
- x *= s; y *= s; z *= s;
- }
- float32 x, y, z;
- };
- /// A 2-by-2 matrix. Stored in column-major order.
- struct b2Mat22
- {
- /// The default constructor does nothing (for performance).
- b2Mat22() {}
- /// Construct this matrix using columns.
- b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
- {
- col1 = c1;
- col2 = c2;
- }
- /// Construct this matrix using scalars.
- b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
- {
- col1.x = a11; col1.y = a21;
- col2.x = a12; col2.y = a22;
- }
- /// Construct this matrix using an angle. This matrix becomes
- /// an orthonormal rotation matrix.
- explicit b2Mat22(float32 angle)
- {
- // TODO_ERIN compute sin+cos together.
- float32 c = cosf(angle), s = sinf(angle);
- col1.x = c; col2.x = -s;
- col1.y = s; col2.y = c;
- }
- /// Initialize this matrix using columns.
- void Set(const b2Vec2& c1, const b2Vec2& c2)
- {
- col1 = c1;
- col2 = c2;
- }
- /// Initialize this matrix using an angle. This matrix becomes
- /// an orthonormal rotation matrix.
- void Set(float32 angle)
- {
- float32 c = cosf(angle), s = sinf(angle);
- col1.x = c; col2.x = -s;
- col1.y = s; col2.y = c;
- }
- /// Set this to the identity matrix.
- void SetIdentity()
- {
- col1.x = 1.0f; col2.x = 0.0f;
- col1.y = 0.0f; col2.y = 1.0f;
- }
- /// Set this matrix to all zeros.
- void SetZero()
- {
- col1.x = 0.0f; col2.x = 0.0f;
- col1.y = 0.0f; col2.y = 0.0f;
- }
- /// Extract the angle from this matrix (assumed to be
- /// a rotation matrix).
- float32 GetAngle() const
- {
- return b2Atan2(col1.y, col1.x);
- }
- b2Mat22 GetInverse() const
- {
- float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
- b2Mat22 B;
- float32 det = a * d - b * c;
- if (det != 0.0f)
- {
- det = 1.0f / det;
- }
- B.col1.x = det * d; B.col2.x = -det * b;
- B.col1.y = -det * c; B.col2.y = det * a;
- return B;
- }
- /// Solve A * x = b, where b is a column vector. This is more efficient
- /// than computing the inverse in one-shot cases.
- b2Vec2 Solve(const b2Vec2& b) const
- {
- float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
- float32 det = a11 * a22 - a12 * a21;
- if (det != 0.0f)
- {
- det = 1.0f / det;
- }
- b2Vec2 x;
- x.x = det * (a22 * b.x - a12 * b.y);
- x.y = det * (a11 * b.y - a21 * b.x);
- return x;
- }
- b2Vec2 col1, col2;
- };
- /// A 3-by-3 matrix. Stored in column-major order.
- struct b2Mat33
- {
- /// The default constructor does nothing (for performance).
- b2Mat33() {}
- /// Construct this matrix using columns.
- b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
- {
- col1 = c1;
- col2 = c2;
- col3 = c3;
- }
- /// Set this matrix to all zeros.
- void SetZero()
- {
- col1.SetZero();
- col2.SetZero();
- col3.SetZero();
- }
- /// Solve A * x = b, where b is a column vector. This is more efficient
- /// than computing the inverse in one-shot cases.
- b2Vec3 Solve33(const b2Vec3& b) const;
- /// Solve A * x = b, where b is a column vector. This is more efficient
- /// than computing the inverse in one-shot cases. Solve only the upper
- /// 2-by-2 matrix equation.
- b2Vec2 Solve22(const b2Vec2& b) const;
- b2Vec3 col1, col2, col3;
- };
- /// A transform contains translation and rotation. It is used to represent
- /// the position and orientation of rigid frames.
- struct b2Transform
- {
- /// The default constructor does nothing (for performance).
- b2Transform() {}
- /// Initialize using a position vector and a rotation matrix.
- b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
- /// Set this to the identity transform.
- void SetIdentity()
- {
- position.SetZero();
- R.SetIdentity();
- }
- /// Set this based on the position and angle.
- void Set(const b2Vec2& p, float32 angle)
- {
- position = p;
- R.Set(angle);
- }
- /// Calculate the angle that the rotation matrix represents.
- float32 GetAngle() const
- {
- return b2Atan2(R.col1.y, R.col1.x);
- }
- b2Vec2 position;
- b2Mat22 R;
- };
- /// This describes the motion of a body/shape for TOI computation.
- /// Shapes are defined with respect to the body origin, which may
- /// no coincide with the center of mass. However, to support dynamics
- /// we must interpolate the center of mass position.
- struct b2Sweep
- {
- /// Get the interpolated transform at a specific time.
- /// @param alpha is a factor in [0,1], where 0 indicates t0.
- void GetTransform(b2Transform* xf, float32 alpha) const;
- /// Advance the sweep forward, yielding a new initial state.
- /// @param t the new initial time.
- void Advance(float32 t);
- /// Normalize the angles.
- void Normalize();
- b2Vec2 localCenter; ///< local center of mass position
- b2Vec2 c0, c; ///< center world positions
- float32 a0, a; ///< world angles
- };
- extern const b2Vec2 b2Vec2_zero;
- extern const b2Mat22 b2Mat22_identity;
- extern const b2Transform b2Transform_identity;
- /// Perform the dot product on two vectors.
- inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
- {
- return a.x * b.x + a.y * b.y;
- }
- /// Perform the cross product on two vectors. In 2D this produces a scalar.
- inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
- {
- return a.x * b.y - a.y * b.x;
- }
- /// Perform the cross product on a vector and a scalar. In 2D this produces
- /// a vector.
- inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
- {
- return b2Vec2(s * a.y, -s * a.x);
- }
- /// Perform the cross product on a scalar and a vector. In 2D this produces
- /// a vector.
- inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
- {
- return b2Vec2(-s * a.y, s * a.x);
- }
- /// Multiply a matrix times a vector. If a rotation matrix is provided,
- /// then this transforms the vector from one frame to another.
- inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
- {
- return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
- }
- /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
- /// then this transforms the vector from one frame to another (inverse transform).
- inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
- {
- return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
- }
- /// Add two vectors component-wise.
- inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
- {
- return b2Vec2(a.x + b.x, a.y + b.y);
- }
- /// Subtract two vectors component-wise.
- inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
- {
- return b2Vec2(a.x - b.x, a.y - b.y);
- }
- inline b2Vec2 operator * (float32 s, const b2Vec2& a)
- {
- return b2Vec2(s * a.x, s * a.y);
- }
- inline bool operator == (const b2Vec2& a, const b2Vec2& b)
- {
- return a.x == b.x && a.y == b.y;
- }
- inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
- {
- b2Vec2 c = a - b;
- return c.Length();
- }
- inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
- {
- b2Vec2 c = a - b;
- return b2Dot(c, c);
- }
- inline b2Vec3 operator * (float32 s, const b2Vec3& a)
- {
- return b2Vec3(s * a.x, s * a.y, s * a.z);
- }
- /// Add two vectors component-wise.
- inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
- {
- return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
- }
- /// Subtract two vectors component-wise.
- inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
- {
- return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
- }
- /// Perform the dot product on two vectors.
- inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
- {
- return a.x * b.x + a.y * b.y + a.z * b.z;
- }
- /// Perform the cross product on two vectors.
- inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
- {
- 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);
- }
- inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
- {
- return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
- }
- // A * B
- inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
- {
- return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
- }
- // A^T * B
- inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
- {
- b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
- b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
- return b2Mat22(c1, c2);
- }
- /// Multiply a matrix times a vector.
- inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
- {
- return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
- }
- inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
- {
- float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
- float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
- return b2Vec2(x, y);
- }
- inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
- {
- return b2MulT(T.R, v - T.position);
- }
- inline b2Vec2 b2Abs(const b2Vec2& a)
- {
- return b2Vec2(b2Abs(a.x), b2Abs(a.y));
- }
- inline b2Mat22 b2Abs(const b2Mat22& A)
- {
- return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
- }
- template <typename T>
- inline T b2Min(T a, T b)
- {
- return a < b ? a : b;
- }
- inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
- {
- return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
- }
- template <typename T>
- inline T b2Max(T a, T b)
- {
- return a > b ? a : b;
- }
- inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
- {
- return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
- }
- template <typename T>
- inline T b2Clamp(T a, T low, T high)
- {
- return b2Max(low, b2Min(a, high));
- }
- inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
- {
- return b2Max(low, b2Min(a, high));
- }
- template<typename T> inline void b2Swap(T& a, T& b)
- {
- T tmp = a;
- a = b;
- b = tmp;
- }
- /// "Next Largest Power of 2
- /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
- /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
- /// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
- /// largest power of 2. For a 32-bit value:"
- inline uint32 b2NextPowerOfTwo(uint32 x)
- {
- x |= (x >> 1);
- x |= (x >> 2);
- x |= (x >> 4);
- x |= (x >> 8);
- x |= (x >> 16);
- return x + 1;
- }
- inline bool b2IsPowerOfTwo(uint32 x)
- {
- bool result = x > 0 && (x & (x - 1)) == 0;
- return result;
- }
- inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
- {
- xf->position = (1.0f - alpha) * c0 + alpha * c;
- float32 angle = (1.0f - alpha) * a0 + alpha * a;
- xf->R.Set(angle);
- // Shift to origin
- xf->position -= b2Mul(xf->R, localCenter);
- }
- inline void b2Sweep::Advance(float32 t)
- {
- c0 = (1.0f - t) * c0 + t * c;
- a0 = (1.0f - t) * a0 + t * a;
- }
- /// Normalize an angle in radians to be between -pi and pi
- inline void b2Sweep::Normalize()
- {
- float32 twoPi = 2.0f * b2_pi;
- float32 d = twoPi * floorf(a0 / twoPi);
- a0 -= d;
- a -= d;
- }
- #endif