Box2D/Box2D/Common/b2Math.h

Go to the documentation of this file.

/*
* 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>

inline bool b2IsValid(float32 x)
{
        if (x != x)
        {
                // NaN.
                return false;
        }

        float32 infinity = std::numeric_limits<float32>::infinity();
        return -infinity < x && x < infinity;
}

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;
}

struct b2Vec2
{
        b2Vec2() {}

        b2Vec2(float32 x, float32 y) : x(x), y(y) {}

        void SetZero() { x = 0.0f; y = 0.0f; }

        void Set(float32 x_, float32 y_) { x = x_; y = y_; }

        b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
        
        float32 operator () (int32 i) const
        {
                return (&x)[i];
        }

        float32& operator () (int32 i)
        {
                return (&x)[i];
        }

        void operator += (const b2Vec2& v)
        {
                x += v.x; y += v.y;
        }
        
        void operator -= (const b2Vec2& v)
        {
                x -= v.x; y -= v.y;
        }

        void operator *= (float32 a)
        {
                x *= a; y *= a;
        }

        float32 Length() const
        {
                return b2Sqrt(x * x + y * y);
        }

        float32 LengthSquared() const
        {
                return x * x + y * y;
        }

        float32 Normalize()
        {
                float32 length = Length();
                if (length < b2_epsilon)
                {
                        return 0.0f;
                }
                float32 invLength = 1.0f / length;
                x *= invLength;
                y *= invLength;

                return length;
        }

        bool IsValid() const
        {
                return b2IsValid(x) && b2IsValid(y);
        }

        float32 x, y;
};

struct b2Vec3
{
        b2Vec3() {}

        b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}

        void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }

        void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }

        b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }

        void operator += (const b2Vec3& v)
        {
                x += v.x; y += v.y; z += v.z;
        }

        void operator -= (const b2Vec3& v)
        {
                x -= v.x; y -= v.y; z -= v.z;
        }

        void operator *= (float32 s)
        {
                x *= s; y *= s; z *= s;
        }

        float32 x, y, z;
};

struct b2Mat22
{
        b2Mat22() {}

        b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
        {
                col1 = c1;
                col2 = c2;
        }

        b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
        {
                col1.x = a11; col1.y = a21;
                col2.x = a12; col2.y = a22;
        }

        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;
        }

        void Set(const b2Vec2& c1, const b2Vec2& c2)
        {
                col1 = c1;
                col2 = c2;
        }

        void Set(float32 angle)
        {
                float32 c = cosf(angle), s = sinf(angle);
                col1.x = c; col2.x = -s;
                col1.y = s; col2.y = c;
        }

        void SetIdentity()
        {
                col1.x = 1.0f; col2.x = 0.0f;
                col1.y = 0.0f; col2.y = 1.0f;
        }

        void SetZero()
        {
                col1.x = 0.0f; col2.x = 0.0f;
                col1.y = 0.0f; col2.y = 0.0f;
        }

        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;
        }

        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;
};

struct b2Mat33
{
        b2Mat33() {}

        b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
        {
                col1 = c1;
                col2 = c2;
                col3 = c3;
        }

        void SetZero()
        {
                col1.SetZero();
                col2.SetZero();
                col3.SetZero();
        }

        b2Vec3 Solve33(const b2Vec3& b) const;

        b2Vec2 Solve22(const b2Vec2& b) const;

        b2Vec3 col1, col2, col3;
};

struct b2Transform
{
        b2Transform() {}

        b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}

        void SetIdentity()
        {
                position.SetZero();
                R.SetIdentity();
        }

        void Set(const b2Vec2& p, float32 angle)
        {
                position = p;
                R.Set(angle);
        }

        float32 GetAngle() const
        {
                return b2Atan2(R.col1.y, R.col1.x);
        }

        b2Vec2 position;
        b2Mat22 R;
};

struct b2Sweep
{
        void GetTransform(b2Transform* xf, float32 alpha) const;

        void Advance(float32 t);

        b2Vec2 localCenter;     
        b2Vec2 c0, c;           
        float32 a0, a;          
};


extern const b2Vec2 b2Vec2_zero;
extern const b2Mat22 b2Mat22_identity;
extern const b2Transform b2Transform_identity;

inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
{
        return a.x * b.x + a.y * b.y;
}

inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
{
        return a.x * b.y - a.y * b.x;
}

inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
{
        return b2Vec2(s * a.y, -s * a.x);
}

inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
{
        return b2Vec2(-s * a.y, s * a.x);
}

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);
}

inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
{
        return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
}

inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
{
        return b2Vec2(a.x + b.x, a.y + b.y);
}

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);
}

inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
{
        return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}

inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
{
        return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}

inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
{
        return a.x * b.x + a.y * b.y + a.z * b.z;
}

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);
}

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;
}

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;
}

#endif

---