aboutsummaryrefslogtreecommitdiff
path: root/src/3rdparty/Box2D/Common/b2Math.h
diff options
context:
space:
mode:
Diffstat (limited to 'src/3rdparty/Box2D/Common/b2Math.h')
-rw-r--r--src/3rdparty/Box2D/Common/b2Math.h707
1 files changed, 707 insertions, 0 deletions
diff --git a/src/3rdparty/Box2D/Common/b2Math.h b/src/3rdparty/Box2D/Common/b2Math.h
new file mode 100644
index 0000000..7a816e5
--- /dev/null
+++ b/src/3rdparty/Box2D/Common/b2Math.h
@@ -0,0 +1,707 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 <math.h>
+
+/// This function is used to ensure that a floating point number is not a NaN or infinity.
+inline bool b2IsValid(float32 x)
+{
+ return isfinite(x);
+}
+
+#define b2Sqrt(x) sqrtf(x)
+#define b2Atan2(y, x) atan2f(y, x)
+
+/// A 2D column vector.
+struct b2Vec2
+{
+ /// Default constructor does nothing (for performance).
+ b2Vec2() {}
+
+ /// Construct using coordinates.
+ b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {}
+
+ /// 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);
+ }
+
+ /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
+ b2Vec2 Skew() const
+ {
+ return b2Vec2(-y, x);
+ }
+
+ float32 x, y;
+};
+
+/// A 2D column vector with 3 elements.
+struct b2Vec3
+{
+ /// Default constructor does nothing (for performance).
+ b2Vec3() {}
+
+ /// Construct using coordinates.
+ b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {}
+
+ /// 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)
+ {
+ ex = c1;
+ ey = c2;
+ }
+
+ /// Construct this matrix using scalars.
+ b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
+ {
+ ex.x = a11; ex.y = a21;
+ ey.x = a12; ey.y = a22;
+ }
+
+ /// Initialize this matrix using columns.
+ void Set(const b2Vec2& c1, const b2Vec2& c2)
+ {
+ ex = c1;
+ ey = c2;
+ }
+
+ /// Set this to the identity matrix.
+ void SetIdentity()
+ {
+ ex.x = 1.0f; ey.x = 0.0f;
+ ex.y = 0.0f; ey.y = 1.0f;
+ }
+
+ /// Set this matrix to all zeros.
+ void SetZero()
+ {
+ ex.x = 0.0f; ey.x = 0.0f;
+ ex.y = 0.0f; ey.y = 0.0f;
+ }
+
+ b2Mat22 GetInverse() const
+ {
+ float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+ b2Mat22 B;
+ float32 det = a * d - b * c;
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+ B.ex.x = det * d; B.ey.x = -det * b;
+ B.ex.y = -det * c; B.ey.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 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.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 ex, ey;
+};
+
+/// 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)
+ {
+ ex = c1;
+ ey = c2;
+ ez = c3;
+ }
+
+ /// Set this matrix to all zeros.
+ void SetZero()
+ {
+ ex.SetZero();
+ ey.SetZero();
+ ez.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;
+
+ /// Get the inverse of this matrix as a 2-by-2.
+ /// Returns the zero matrix if singular.
+ void GetInverse22(b2Mat33* M) const;
+
+ /// Get the symmetric inverse of this matrix as a 3-by-3.
+ /// Returns the zero matrix if singular.
+ void GetSymInverse33(b2Mat33* M) const;
+
+ b2Vec3 ex, ey, ez;
+};
+
+/// Rotation
+struct b2Rot
+{
+ b2Rot() {}
+
+ /// Initialize from an angle in radians
+ explicit b2Rot(float32 angle)
+ {
+ /// TODO_ERIN optimize
+ s = sinf(angle);
+ c = cosf(angle);
+ }
+
+ /// Set using an angle in radians.
+ void Set(float32 angle)
+ {
+ /// TODO_ERIN optimize
+ s = sinf(angle);
+ c = cosf(angle);
+ }
+
+ /// Set to the identity rotation
+ void SetIdentity()
+ {
+ s = 0.0f;
+ c = 1.0f;
+ }
+
+ /// Get the angle in radians
+ float32 GetAngle() const
+ {
+ return b2Atan2(s, c);
+ }
+
+ /// Get the x-axis
+ b2Vec2 GetXAxis() const
+ {
+ return b2Vec2(c, s);
+ }
+
+ /// Get the u-axis
+ b2Vec2 GetYAxis() const
+ {
+ return b2Vec2(-s, c);
+ }
+
+ /// Sine and cosine
+ float32 s, c;
+};
+
+/// 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.
+ b2Transform() {}
+
+ /// Initialize using a position vector and a rotation.
+ b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
+
+ /// Set this to the identity transform.
+ void SetIdentity()
+ {
+ p.SetZero();
+ q.SetIdentity();
+ }
+
+ /// Set this based on the position and angle.
+ void Set(const b2Vec2& position, float32 angle)
+ {
+ p = position;
+ q.Set(angle);
+ }
+
+ b2Vec2 p;
+ b2Rot q;
+};
+
+/// 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 beta is a factor in [0,1], where 0 indicates alpha0.
+ void GetTransform(b2Transform* xfb, float32 beta) const;
+
+ /// Advance the sweep forward, yielding a new initial state.
+ /// @param alpha the new initial time.
+ void Advance(float32 alpha);
+
+ /// Normalize the angles.
+ void Normalize();
+
+ b2Vec2 localCenter; ///< local center of mass position
+ b2Vec2 c0, c; ///< center world positions
+ float32 a0, a; ///< world angles
+
+ /// Fraction of the current time step in the range [0,1]
+ /// c0 and a0 are the positions at alpha0.
+ float32 alpha0;
+};
+
+/// Useful constant
+extern const b2Vec2 b2Vec2_zero;
+
+/// 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.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.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.ex), b2Dot(v, A.ey));
+}
+
+/// 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 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.ex + B.ex, A.ey + B.ey);
+}
+
+// A * B
+inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
+{
+ return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
+}
+
+// A^T * B
+inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
+{
+ b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
+ b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
+ return b2Mat22(c1, c2);
+}
+
+/// Multiply a matrix times a vector.
+inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
+{
+ return v.x * A.ex + v.y * A.ey + v.z * A.ez;
+}
+
+/// Multiply a matrix times a vector.
+inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
+{
+ return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
+}
+
+/// Multiply two rotations: q * r
+inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
+{
+ // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+ // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
+ // s = qs * rc + qc * rs
+ // c = qc * rc - qs * rs
+ b2Rot qr;
+ qr.s = q.s * r.c + q.c * r.s;
+ qr.c = q.c * r.c - q.s * r.s;
+ return qr;
+}
+
+/// Transpose multiply two rotations: qT * r
+inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
+{
+ // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+ // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
+ // s = qc * rs - qs * rc
+ // c = qc * rc + qs * rs
+ b2Rot qr;
+ qr.s = q.c * r.s - q.s * r.c;
+ qr.c = q.c * r.c + q.s * r.s;
+ return qr;
+}
+
+/// Rotate a vector
+inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
+{
+ return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
+}
+
+/// Inverse rotate a vector
+inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
+{
+ return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
+}
+
+inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
+{
+ float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
+ float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
+
+ return b2Vec2(x, y);
+}
+
+inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
+{
+ float32 px = v.x - T.p.x;
+ float32 py = v.y - T.p.y;
+ float32 x = (T.q.c * px + T.q.s * py);
+ float32 y = (-T.q.s * px + T.q.c * py);
+
+ return b2Vec2(x, y);
+}
+
+// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
+{
+ b2Transform C;
+ C.q = b2Mul(A.q, B.q);
+ C.p = b2Mul(A.q, B.p) + A.p;
+ return C;
+}
+
+// v2 = A.q' * (B.q * v1 + B.p - A.p)
+// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
+{
+ b2Transform C;
+ C.q = b2MulT(A.q, B.q);
+ C.p = b2MulT(A.q, B.p - A.p);
+ return C;
+}
+
+template <typename T>
+inline T b2Abs(T a)
+{
+ return a > T(0) ? a : -a;
+}
+
+inline b2Vec2 b2Abs(const b2Vec2& a)
+{
+ return b2Vec2(b2Abs(a.x), b2Abs(a.y));
+}
+
+inline b2Mat22 b2Abs(const b2Mat22& A)
+{
+ return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
+}
+
+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 beta) const
+{
+ xf->p = (1.0f - beta) * c0 + beta * c;
+ float32 angle = (1.0f - beta) * a0 + beta * a;
+ xf->q.Set(angle);
+
+ // Shift to origin
+ xf->p -= b2Mul(xf->q, localCenter);
+}
+
+inline void b2Sweep::Advance(float32 alpha)
+{
+ b2Assert(alpha0 < 1.0f);
+ float32 beta = (alpha - alpha0) / (1.0f - alpha0);
+ c0 += beta * (c - c0);
+ a0 += beta * (a - a0);
+ alpha0 = alpha;
+}
+
+/// 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
generated by cgit v1.1-26-g67d0 at 2025-05-18 20:37:26 +0000