Math.hpp

Idź do dokumentacji tego pliku.

#if defined(_MSC_VER) && (_MSC_VER >= 1200)
#pragma once
#endif
#ifndef COMMON_MATH_H_
#define COMMON_MATH_H_

#ifdef WIN32
        #include <windows.h> // bo bez tego w d3d9types.h są błędy bo nie zna DWORD itp., poza tym potrzebny jest Point itd.
#endif

#ifdef USE_DIRECTX
        #include <d3d9types.h>
        #include <d3dx9math.h>
#endif

namespace common
{



template <typename DestT, typename SrcT>
DestT & math_cast(const SrcT &x)
{
        assert(0 && "Unsupported type in math_cast.");
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


struct POINT_
{
        union
        {
                struct
                {
                        int x, y;
                };
                int v[2];
        };

        static const POINT_ ZERO;

        POINT_() { }
        POINT_(int x, int y) : x(x), y(y) { }

        bool operator == (const POINT_ &v) const { return x == v.x && y == v.y; }
        bool operator != (const POINT_ &v) const { return x != v.x || y != v.y; }

        POINT_ operator - () const { return POINT_(-x, -y); }
        POINT_ operator + (const POINT_ &v) const { return POINT_(x+v.x, y+v.y); }
        POINT_ operator - (const POINT_ &v) const { return POINT_(x-v.x, y-v.y); }
        POINT_ & operator += (const POINT_ &v) { x += v.x; y += v.y; return *this; }
        POINT_ & operator -= (const POINT_ &v) { x -= v.x; y -= v.y; return *this; }
        POINT_ & operator *= (int v) { x *= v; y *= v; return *this; }
        POINT_ & operator /= (int v) { x /= v; y /= v; return *this; }

        int & operator [] (uint4 Index) { return v[Index]; }
        const int & operator [] (uint4 Index) const { return v[Index]; }

        int * GetArray() { return &x; }
        const int * GetArray() const { return &x; }
};

inline POINT_ operator * (const POINT_ pt, int v) { return POINT_(pt.x*v, pt.y*v); }
inline POINT_ operator / (const POINT_ pt, int v) { return POINT_(pt.x/v, pt.y/v); }
inline POINT_ operator * (int v, const POINT_ pt) { return POINT_(pt.x*v, pt.y*v); }
inline POINT_ operator / (int v, const POINT_ pt) { return POINT_(pt.x/v, pt.y/v); }

#ifdef WIN32
        template <> inline ::POINT & math_cast<::POINT, POINT_>(const POINT_ &x) { return (::POINT&)(x); }
        template <> inline POINT_ & math_cast<POINT_, ::POINT>(const ::POINT &x) { return (POINT_&)(x); }
#endif

inline void swap(POINT_ &v1, POINT_ &v2)
{
        std::swap(v1.x, v2.x);
        std::swap(v1.y, v2.y);
}

// math_point

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXVECTOR2 VEC2;
#else
        struct VEC2
        {
                float x, y;

                VEC2() { }
                VEC2(float x, float y) : x(x), y(y) { }
                VEC2(const float *Array) : x(Array[0]), y(Array[1]) { }

                operator float * () { return &x; }
                operator const float * () const { return &x; }

                bool operator == (const VEC2 &v) const { return x == v.x && y == v.y; }
                bool operator != (const VEC2 &v) const { return x != v.x || y != v.y; }

                VEC2 operator + () const { return *this; }
                VEC2 operator - () const { return VEC2(-x, -y); }
                VEC2 operator + (const VEC2 &v) const { return VEC2(x+v.x, y+v.y); }
                VEC2 operator - (const VEC2 &v) const { return VEC2(x-v.x, y-v.y); }
                VEC2 & operator += (const VEC2 &v) { x += v.x; y += v.y; return *this; }
                VEC2 & operator -= (const VEC2 &v) { x -= v.x; y -= v.y; return *this; }
                VEC2 & operator *= (float v) { x *= v; y *= v; return *this; }
                VEC2 & operator /= (float v) { x /= v; y /= v; return *this; }
        };
        inline VEC2 operator * (const VEC2 pt, float v) { return VEC2(pt.x*v, pt.y*v); }
        inline VEC2 operator / (const VEC2 pt, float v) { return VEC2(pt.x/v, pt.y/v); }
        inline VEC2 operator * (float v, const VEC2 pt) { return VEC2(pt.x*v, pt.y*v); }
        inline VEC2 operator / (float v, const VEC2 pt) { return VEC2(pt.x/v, pt.y/v); }
#endif

inline void swap(VEC2 &v1, VEC2 &v2)
{
        std::swap(v1.x, v2.x);
        std::swap(v1.y, v2.y);
}

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXVECTOR3 VEC3;
#else
        struct VEC3
        {
                float x, y, z;

                VEC3() { }
                VEC3(float x, float y, float z) : x(x), y(y), z(z) { }
                VEC3(const float *Array) : x(Array[0]), y(Array[1]), z(Array[2]) { }

                operator float * () { return &x; }
                operator const float * () const { return &x; }

                bool operator == (const VEC3 &v) const { return x == v.x && y == v.y && z == v.z; }
                bool operator != (const VEC3 &v) const { return x != v.x || y != v.y || z != v.z; }

                VEC3 operator + () const { return *this; }
                VEC3 operator - () const { return VEC3(-x, -y, -z); }
                VEC3 operator + (const VEC3 &v) const { return VEC3(x+v.x, y+v.y, z+v.z); }
                VEC3 operator - (const VEC3 &v) const { return VEC3(x-v.x, y-v.y, z-v.z); }
                VEC3 & operator += (const VEC3 &v) { x += v.x; y += v.y; z += v.z; return *this; }
                VEC3 & operator -= (const VEC3 &v) { x -= v.x; y -= v.y; z -= v.z; return *this; }
                VEC3 & operator *= (float v) { x *= v; y *= v; z *= v; return *this; }
                VEC3 & operator /= (float v) { x /= v; y /= v; z /= v; return *this; }
        };
        inline VEC3 operator * (const VEC3 pt, float v) { return VEC3(pt.x*v, pt.y*v, pt.z*v); }
        inline VEC3 operator / (const VEC3 pt, float v) { return VEC3(pt.x/v, pt.y/v, pt.z/v); }
        inline VEC3 operator * (float v, const VEC3 pt) { return VEC3(pt.x*v, pt.y*v, pt.z*v); }
        inline VEC3 operator / (float v, const VEC3 pt) { return VEC3(pt.x/v, pt.y/v, pt.z/v); }
#endif

inline void swap(VEC3 &v1, VEC3 &v2)
{
        std::swap(v1.x, v2.x);
        std::swap(v1.y, v2.y);
        std::swap(v1.z, v2.z);
}

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXVECTOR4 VEC4;
#else
        struct VEC4
        {
                float x, y, z, w;

                VEC4() { }
                VEC4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) { }
                VEC4(const float *Array) : x(Array[0]), y(Array[1]), z(Array[2]), w(Array[3]) { }

                operator float * () { return &x; }
                operator const float * () const { return &x; }

                bool operator == (const VEC4 &v) const { return x == v.x && y == v.y && z == v.z && w == v.w; }
                bool operator != (const VEC4 &v) const { return x != v.x || y != v.y || z != v.z || w != v.w; }

                VEC4 operator + () const { return *this; }
                VEC4 operator - () const { return VEC4(-x, -y, -z, -w); }
                VEC4 operator + (const VEC4 &v) const { return VEC4(x+v.x, y+v.y, z+v.z, w+v.w); }
                VEC4 operator - (const VEC4 &v) const { return VEC4(x-v.x, y-v.y, z-v.z, w-v.w); }
                VEC4 & operator += (const VEC4 &v) { x += v.x; y += v.y; z += v.z; w += v.w; return *this; }
                VEC4 & operator -= (const VEC4 &v) { x -= v.x; y -= v.y; z -= v.z; w -= v.w; return *this; }
                VEC4 & operator *= (float v) { x *= v; y *= v; z *= v; w *= v; return *this; }
                VEC4 & operator /= (float v) { x /= v; y /= v; z /= v; w /= v; return *this; }
        };
        inline VEC4 operator * (const VEC4 pt, float v) { return VEC4(pt.x*v, pt.y*v, pt.z*v, pt.w*v); }
        inline VEC4 operator / (const VEC4 pt, float v) { return VEC4(pt.x/v, pt.y/v, pt.z/v, pt.w/v); }
        inline VEC4 operator * (float v, const VEC4 pt) { return VEC4(pt.x*v, pt.y*v, pt.z*v, pt.w*v); }
        inline VEC4 operator / (float v, const VEC4 pt) { return VEC4(pt.x/v, pt.y/v, pt.z/v, pt.w/v); }
#endif

inline void swap(VEC4 &v1, VEC4 &v2)
{
        std::swap(v1.x, v2.x);
        std::swap(v1.y, v2.y);
        std::swap(v1.z, v2.z);
        std::swap(v1.w, v2.w);
}

extern const VEC2 VEC2_ZERO;
extern const VEC2 VEC2_ONE;
extern const VEC2 VEC2_POSITIVE_X;
extern const VEC2 VEC2_POSITIVE_Y;
extern const VEC2 VEC2_NEGATIVE_X;
extern const VEC2 VEC2_NEGATIVE_Y;

extern const VEC3 VEC3_ZERO;
extern const VEC3 VEC3_ONE;
extern const VEC3 VEC3_POSITIVE_X;
extern const VEC3 VEC3_POSITIVE_Y;
extern const VEC3 VEC3_POSITIVE_Z;
extern const VEC3 VEC3_NEGATIVE_X;
extern const VEC3 VEC3_NEGATIVE_Y;
extern const VEC3 VEC3_NEGATIVE_Z;

extern const VEC4 VEC4_ZERO;
extern const VEC4 VEC4_ONE;
extern const VEC4 VEC4_POSITIVE_X;
extern const VEC4 VEC4_POSITIVE_Y;
extern const VEC4 VEC4_POSITIVE_Z;
extern const VEC4 VEC4_POSITIVE_W;
extern const VEC4 VEC4_NEGATIVE_X;
extern const VEC4 VEC4_NEGATIVE_Y;
extern const VEC4 VEC4_NEGATIVE_Z;
extern const VEC4 VEC4_NEGATIVE_W;

inline float LengthSq(const VEC2 &v)
{
        return v.x*v.x + v.y*v.y;
}
inline float LengthSq(const VEC3 &v)
{
        return v.x*v.x + v.y*v.y + v.z*v.z;
}
inline float LengthSq(const VEC4 &v)
{
        return v.x*v.x + v.y*v.y + v.z*v.z + v.w*v.w;
}

inline float Length(const VEC2 &v)
{
        return sqrtf(LengthSq(v));
}
inline float Length(const VEC3 &v)
{
        return sqrtf(LengthSq(v));
}
inline float Length(const VEC4 &v)
{
        return sqrtf(LengthSq(v));
}

inline void Min(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = std::min(v1.x, v2.x);
        Out->y = std::min(v1.y, v2.y);
}
inline void Min(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = std::min(v1.x, v2.x);
        Out->y = std::min(v1.y, v2.y);
        Out->z = std::min(v1.z, v2.z);
}
inline void Min(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = std::min(v1.x, v2.x);
        Out->y = std::min(v1.y, v2.y);
        Out->z = std::min(v1.z, v2.z);
        Out->w = std::min(v1.w, v2.w);
}

inline void Max(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = std::max(v1.x, v2.x);
        Out->y = std::max(v1.y, v2.y);
}
inline void Max(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = std::max(v1.x, v2.x);
        Out->y = std::max(v1.y, v2.y);
        Out->z = std::max(v1.z, v2.z);
}
inline void Max(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = std::max(v1.x, v2.x);
        Out->y = std::max(v1.y, v2.y);
        Out->z = std::max(v1.z, v2.z);
        Out->w = std::max(v1.w, v2.w);
}


inline void Mul(VEC2 *Out, const VEC2 &v, float f)
{
        Out->x = v.x * f;
        Out->y = v.y * f;
}
inline void Mul(VEC3 *Out, const VEC3 &v, float f)
{
        Out->x = v.x * f;
        Out->y = v.y * f;
        Out->z = v.z * f;
}
inline void Mul(VEC4 *Out, const VEC4 &v, float f)
{
        Out->x = v.x * f;
        Out->y = v.y * f;
        Out->z = v.z * f;
        Out->w = v.w * f;
}

inline void Add(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = v1.x + v2.x;
        Out->y = v1.y + v2.y;
}
inline void Add(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = v1.x + v2.x;
        Out->y = v1.y + v2.y;
        Out->z = v1.z + v2.z;
}
inline void Add(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = v1.x + v2.x;
        Out->y = v1.y + v2.y;
        Out->z = v1.z + v2.z;
        Out->w = v1.w + v2.w;
}

inline void Sub(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = v1.x - v2.x;
        Out->y = v1.y - v2.y;
}
inline void Sub(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = v1.x - v2.x;
        Out->y = v1.y - v2.y;
        Out->z = v1.z - v2.z;
}
inline void Sub(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = v1.x - v2.x;
        Out->y = v1.y - v2.y;
        Out->z = v1.z - v2.z;
        Out->w = v1.w - v2.w;
}

inline void Mul(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = v1.x * v2.x;
        Out->y = v1.y * v2.y;
}
inline void Mul(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = v1.x * v2.x;
        Out->y = v1.y * v2.y;
        Out->z = v1.z * v2.z;
}
inline void Mul(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = v1.x * v2.x;
        Out->y = v1.y * v2.y;
        Out->z = v1.z * v2.z;
        Out->w = v1.w * v2.w;
}

inline void Div(VEC2 *Out, const VEC2 &v1, const VEC2 &v2)
{
        Out->x = v1.x / v2.x;
        Out->y = v1.y / v2.y;
}
inline void Div(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = v1.x / v2.x;
        Out->y = v1.y / v2.y;
        Out->z = v1.z / v2.z;
}
inline void Div(VEC4 *Out, const VEC4 &v1, const VEC4 &v2)
{
        Out->x = v1.x / v2.x;
        Out->y = v1.y / v2.y;
        Out->z = v1.z / v2.z;
        Out->w = v1.w / v2.w;
}

// math_vec_componentop

inline float Dot(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x*v2.x + v1.y*v2.y;
}
inline float Dot(const VEC3 &v1, const VEC3 &v2)
{
        return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}
inline float Dot(const VEC4 &v1, const VEC4 &v2)
{
        return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z + v1.w*v2.w;
}

inline void Minus(VEC2 *Out, VEC2 &v) { Out->x = -v.x; Out->y = -v.y; }
inline void Minus(VEC3 *Out, VEC3 &v) { Out->x = -v.x; Out->y = -v.y; Out->z = -v.z; }
inline void Minus(VEC4 *Out, VEC4 &v) { Out->x = -v.x; Out->y = -v.y; Out->z = -v.z; Out->w = -v.w; }
inline void Minus(VEC2 *InOut) { InOut->x = -InOut->x; InOut->y = -InOut->y; }
inline void Minus(VEC3 *InOut) { InOut->x = -InOut->x; InOut->y = -InOut->y; InOut->z = -InOut->z; }
inline void Minus(VEC4 *InOut) { InOut->x = -InOut->x; InOut->y = -InOut->y; InOut->z = -InOut->z; InOut->w = -InOut->w; }


inline float Cross(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x*v2.y - v1.y*v2.x;
}

inline void Cross(VEC3 *Out, const VEC3 &v1, const VEC3 &v2)
{
        Out->x = v1.y * v2.z - v1.z * v2.y;
        Out->y = v1.z * v2.x - v1.x * v2.z;
        Out->z = v1.x * v2.y - v1.y * v2.x;
}

inline float DistanceSq(const VEC2 &p1, const VEC2 &p2)
{
        return LengthSq(p2-p1);
}
inline float DistanceSq(const VEC3 &p1, const VEC3 &p2)
{
        return LengthSq(p2-p1);
}
inline float DistanceSq(const VEC4 &p1, const VEC4 &p2)
{
        return LengthSq(p2-p1);
}

inline float Distance(const VEC2 &p1, const VEC2 &p2)
{
        return Length(p2-p1);
}
inline float Distance(const VEC3 &p1, const VEC3 &p2)
{
        return Length(p2-p1);
}
inline float Distance(const VEC4 &p1, const VEC4 &p2)
{
        return Length(p2-p1);
}

inline void Lerp(VEC2 *Out, const VEC2 &v1, const VEC2 &v2, float t)
{
        Out->x = v1.x + t*(v2.x-v1.x);
        Out->y = v1.y + t*(v2.y-v1.y);
}
inline void Lerp(VEC3 *Out, const VEC3 &v1, const VEC3 &v2, float t)
{
        Out->x = v1.x + t*(v2.x-v1.x);
        Out->y = v1.y + t*(v2.y-v1.y);
        Out->z = v1.z + t*(v2.z-v1.z);
}
inline void Lerp(VEC4 *Out, const VEC4 &v1, const VEC4 &v2, float t)
{
        Out->x = v1.x + t*(v2.x-v1.x);
        Out->y = v1.y + t*(v2.y-v1.y);
        Out->z = v1.z + t*(v2.z-v1.z);
        Out->w = v1.w + t*(v2.w-v1.w);
}

inline void Normalize(VEC2 *Out, const VEC2 &v)
{
        float l = 1.0f / Length(v);
        Out->x = v.x * l;
        Out->y = v.y * l;
}
inline void Normalize(VEC3 *Out, const VEC3 &v)
{
        float l = 1.0f / Length(v);
        Out->x = v.x * l;
        Out->y = v.y * l;
        Out->z = v.z * l;
}
inline void Normalize(VEC4 *Out, const VEC4 &v)
{
        float l = 1.0f / Length(v);
        Out->x = v.x * l;
        Out->y = v.y * l;
        Out->z = v.z * l;
        Out->w = v.w * l;
}

inline void Normalize(VEC2 *v)
{
        float rcl = 1.0f / Length(*v);
        v->x *= rcl;
        v->y *= rcl;
}
inline void Normalize(VEC3 *v)
{
        float rcl = 1.0f / Length(*v);
        v->x *= rcl;
        v->y *= rcl;
        v->z *= rcl;
}
inline void Normalize(VEC4 *v)
{
        float rcl = 1.0f / Length(*v);
        v->x *= rcl;
        v->y *= rcl;
        v->z *= rcl;
        v->w *= rcl;
}

inline void Vec3ToVec4(VEC4 *Out, const VEC3 &v)
{
        Out->x = v.x;
        Out->y = v.y;
        Out->z = v.z;
        Out->w = 1.0f;
}

inline void Vec4ToVec3_Ignore(VEC3 *Out, const VEC4 &v)
{
        Out->x = v.x;
        Out->y = v.y;
        Out->z = v.z;
}
inline void Vec4ToVec3_Div(VEC3 *Out, const VEC4 &v)
{
        float rcw = 1.0f / v.w;
        Out->x = v.x * rcw;
        Out->y = v.y * rcw;
        Out->z = v.z * rcw;
}

inline void MidPoint(VEC2 *Out, const VEC2 &p1, const VEC2 &p2)
{
        Out->x = (p1.x+p2.x) * 0.5f;
        Out->y = (p1.y+p2.y) * 0.5f;
}
inline void MidPoint(VEC3 *Out, const VEC3 &p1, const VEC3 &p2)
{
        Out->x = (p1.x+p2.x) * 0.5f;
        Out->y = (p1.y+p2.y) * 0.5f;
        Out->z = (p1.z+p2.z) * 0.5f;
}
inline void MidPoint(VEC4 *Out, const VEC4 &p1, const VEC4 &p2)
{
        Out->x = (p1.x+p2.x) * 0.5f;
        Out->y = (p1.y+p2.y) * 0.5f;
        Out->z = (p1.z+p2.z) * 0.5f;
        Out->w = (p1.w+p2.w) * 0.5f;
}

inline bool VecEqual(const VEC2 &v1, const VEC2 &v2) {
        return float_equal(v1.x, v2.x) && float_equal(v1.y, v2.y);
}
inline bool VecEqual(const VEC3 &v1, const VEC3 &v2) {
        return float_equal(v1.x, v2.x) && float_equal(v1.y, v2.y) && float_equal(v1.z, v2.z);
}
inline bool VecEqual(const VEC4 &v1, const VEC4 &v2) {
        return float_equal(v1.x, v2.x) && float_equal(v1.y, v2.y) && float_equal(v1.z, v2.z) && float_equal(v1.w, v2.w);
}


inline bool Less(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x < v2.x && v1.y < v2.y;
}
inline bool Less(const VEC3 &v1, const VEC3 &v2)
{
        return v1.x < v2.x && v1.y < v2.y && v1.z < v2.z;
}
inline bool Less(const VEC4 &v1, const VEC4 &v2)
{
        return v1.x < v2.x && v1.y < v2.y && v1.z < v2.z && v1.w < v2.w;
}

inline bool LessOrEqual(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x <= v2.x && v1.y <= v2.y;
}
inline bool LessOrEqual(const VEC3 &v1, const VEC3 &v2)
{
        return v1.x <= v2.x && v1.y <= v2.y && v1.z <= v2.z;
}
inline bool LessOrEqual(const VEC4 &v1, const VEC4 &v2)
{
        return v1.x <= v2.x && v1.y <= v2.y && v1.z <= v2.z && v1.w <= v2.w;
}

inline bool Greater(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x > v2.x && v1.y > v2.y;
}
inline bool Greater(const VEC3 &v1, const VEC3 &v2)
{
        return v1.x > v2.x && v1.y > v2.y && v1.z > v2.z;
}
inline bool Greater(const VEC4 &v1, const VEC4 &v2)
{
        return v1.x > v2.x && v1.y > v2.y && v1.z > v2.z && v1.w > v2.w;
}

inline bool GreaterOrEqual(const VEC2 &v1, const VEC2 &v2)
{
        return v1.x >= v2.x && v1.y >= v2.y;
}
inline bool GreaterOrEqual(const VEC3 &v1, const VEC3 &v2)
{
        return v1.x >= v2.x && v1.y >= v2.y && v1.z >= v2.z;
}
inline bool GreaterOrEqual(const VEC4 &v1, const VEC4 &v2)
{
        return v1.x >= v2.x && v1.y >= v2.y && v1.z >= v2.z && v1.w >= v2.w;
}

// math_vec_comp

inline bool around(const VEC2 &v1, const VEC2 &v2, float Epsilon) {
        return around(v1.x, v2.x, Epsilon) && around(v1.y, v2.y, Epsilon);
}
inline bool around(const VEC3 &v1, const VEC3 &v2, float Epsilon) {
        return around(v1.x, v2.x, Epsilon) && around(v1.y, v2.y, Epsilon) && around(v1.z, v2.z, Epsilon);
}
inline bool around(const VEC4 &v1, const VEC4 &v2, float Epsilon) {
        return around(v1.x, v2.x, Epsilon) && around(v1.y, v2.y, Epsilon) && around(v1.z, v2.z, Epsilon) && around(v1.w, v2.w, Epsilon);
}


inline void Perpedicular(VEC2 *Out, const VEC2 &v)
{
        Out->x = -v.y;
        Out->y = v.x;
}


inline void Reflect(VEC2 *Out, const VEC2 &v, const VEC2 &PlaneNormal)
{
        // Wzór: *Out = v - ( 2.0f * Dot(v, PlaneNormal) * PlaneNormal );
        float f = 2.0f * Dot(v, PlaneNormal);
        Out->x = v.x - f * PlaneNormal.x;
        Out->y = v.y - f * PlaneNormal.y;
}
inline void Reflect(VEC3 *Out, const VEC3 &v, const VEC3 &PlaneNormal)
{
        // Wzór: *Out = v - ( 2.0f * Dot(v, PlaneNormal) * PlaneNormal );
        float f = 2.0f * Dot(v, PlaneNormal);
        Out->x = v.x - f * PlaneNormal.x;
        Out->y = v.y - f * PlaneNormal.y;
        Out->z = v.z - f * PlaneNormal.z;
}


inline void Project(VEC2 *Out, const VEC2 &V, const VEC2 &N)
{
        // Oryginalnie:
        // *Out = N * ( Dot(V, N) / LengthSq(N) );
        // Wersja szybsza:
        float T = (V.x*N.x + V.y*N.y) / (N.x*N.x + N.y*N.y);
        Out->x = N.x * T;
        Out->y = N.y * T;
}
inline void Project(VEC3 *Out, const VEC3 &V, const VEC3 &N)
{
        float T = (V.x*N.x + V.y*N.y + V.z*N.z) / (N.x*N.x + N.y*N.y + N.z*N.z);
        Out->x = N.x * T;
        Out->y = N.y * T;
        Out->z = N.z * T;
}
inline void Project_N(VEC2 *Out, const VEC2 &V, const VEC2 &N)
{
        // Oryginalnie:
        // *Out = N * Dot(V, N);
        // Wersja szybsza:
        float T = V.x*N.x + V.y*N.y;
        Out->x = N.x * T;
        Out->y = N.y * T;
}
inline void Project_N(VEC3 *Out, const VEC3 &V, const VEC3 &N)
{
        float T = V.x*N.x + V.y*N.y + V.z*N.z;
        Out->x = N.x * T;
        Out->y = N.y * T;
        Out->z = N.z * T;
}


void Orthogonalize_Fast(VEC3 *OutR1, VEC3 *OutR2, VEC3 *OutR3, const VEC3 &R1, const VEC3 &R2, const VEC3 &R3);
void Orthogonalize(VEC3 *OutR1, VEC3 *OutR2, VEC3 *OutR3, const VEC3 &R1, const VEC3 &R2, const VEC3 &R3);


void PerpedicularVectors(VEC3 *Out1, VEC3 *Out2, const VEC3 &v);

// math_vec


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


float TriangleArea(const VEC2 &p1, const VEC2 &p2, const VEC2 &p3);
float TriangleArea(const VEC3 &p1, const VEC3 &p2, const VEC3 &p3);

float TrianglePerimeter(const VEC2 &p1, const VEC2 &p2, const VEC2 &p3);
float TrianglePerimeter(const VEC3 &p1, const VEC3 &p2, const VEC3 &p3);


inline void Barycentric(VEC2 *Out, const VEC2 &p1, const VEC2 &p2, const VEC2 &p3, float f, float g)
{
        float e = 1.0f-f-g;
        Out->x = e*p1.x + f*p2.x + g*p3.x;
        Out->y = e*p1.y + f*p2.y + g*p3.y;
}
inline void Barycentric(VEC3 *Out, const VEC3 &p1, const VEC3 &p2, const VEC3 &p3, float f, float g)
{
        // Stara wersja, na podstawie DX SDK, dużo wolniejsza:
        //VEC3 Diff2 = p2-p1;
        //VEC3 Diff3 = p3-p1;
        //Out->x = p1.x + f*Diff2.x + g*Diff3.x;
        //Out->y = p1.y + f*Diff2.y + g*Diff3.y;
        //Out->z = p1.z + f*Diff2.z + g*Diff3.z;

        float e = 1.0f-f-g;
        Out->x = e*p1.x + f*p2.x + g*p3.x;
        Out->y = e*p1.y + f*p2.y + g*p3.y;
        Out->z = e*p1.z + f*p2.z + g*p3.z;
}
inline void Barycentric(VEC4 *Out, const VEC4 &p1, const VEC4 &p2, const VEC4 &p3, float f, float g)
{
        float e = 1.0f-f-g;
        Out->x = e*p1.x + f*p2.x + g*p3.x;
        Out->y = e*p1.y + f*p2.y + g*p3.y;
        Out->z = e*p1.z + f*p2.z + g*p3.z;
        Out->w = e*p1.w + f*p2.w + g*p3.w;
}


void CalculateBarycentric(const VEC3 &p, const VEC3 &p1, const VEC3 &p2, const VEC3 &p3, const VEC3 *Normal, float *e, float *f, float *g);

void TriangleInscribedCircle(VEC2 *OutCenter, float *OutRadius, const VEC2 &v1, const VEC2 &v2, const VEC2 &v3);
void TriangleInscribedCircle(VEC3 *OutCenter, float *OutRadius, const VEC3 &v1, const VEC3 &v2, const VEC3 &v3);
void TriangleCircumscribedCircle(VEC2 *OutCenter, float *OutRadius, const VEC2 &v1, const VEC2 &v2, const VEC2 &v3);
void TriangleCircumscribedCircle(VEC3 *OutCenter, float *OutRadius, const VEC3 &v1, const VEC3 &v2, const VEC3 &v3);

// math_triangle


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

inline void PointOnRay(VEC2 *Out, const VEC2 &Origin, const VEC2 &Direction, float t)
{
        Out->x = Origin.x + t * Direction.x;
        Out->y = Origin.y + t * Direction.y;
}
inline void PointOnRay(VEC3 *Out, const VEC3 &Origin, const VEC3 &Direction, float t)
{
        Out->x = Origin.x + t * Direction.x;
        Out->y = Origin.y + t * Direction.y;
        Out->z = Origin.z + t * Direction.z;
}

// math_ray


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


struct RECTI
{
        int left, top, right, bottom;

        RECTI() { }
        RECTI(int left, int top, int right, int bottom) : left(left), top(top), right(right), bottom(bottom) { }
        RECTI(const int *Arr) : left(Arr[0]), top(Arr[1]), right(Arr[2]), bottom(Arr[3]) { }

        operator int * () { return &left; }
        operator const int * () const { return &left; }

        bool operator == (const RECTI &r) const { return left == r.left && top == r.top && right == r.right && bottom == r.bottom; }
        bool operator != (const RECTI &r) const { return left != r.left || top != r.top || right != r.right || bottom != r.bottom; }

        RECTI operator + () const { return *this; }
        RECTI operator - () const { return RECTI(-right, -bottom, -left, -top); }
        RECTI operator + (const POINT_ &p) const { return RECTI(left+p.x, top+p.y, right+p.x, bottom+p.y); }
        RECTI operator - (const POINT_ &p) const { return RECTI(left-p.x, top-p.y, right-p.x, bottom-p.y); }
        RECTI operator * (int s) const { return RECTI(left*s, top*s, right*s, bottom*s); }
        RECTI operator * (const POINT_ &s) const { return RECTI(left*s.x, top*s.y, right*s.x, bottom*s.y); }
        RECTI & operator += (const POINT_ &p) { left += p.x; top += p.y; right += p.x; bottom += p.y; return *this; }
        RECTI & operator -= (const POINT_ &p) { left -= p.x; top -= p.y; right -= p.x; bottom -= p.y; return *this; }
        RECTI & operator *= (int s) { left *= s; top *= s; right *= s; bottom *= s; return *this; }
        RECTI & operator *= (const POINT_ &s) { left *= s.x; top *= s.y; right *= s.x; bottom *= s.y; return *this; }

        bool IsValid() const { return left < right && top < bottom; }
        void Repair() { if (right < left) std::swap(left, right); if (bottom < top) std::swap(top, bottom); }

        void LeftTop(POINT_ *Out) const { Out->x = left; Out->y = top; }
        void RightBottom(POINT_ *Out) const { Out->x = right; Out->y = bottom; }

        void Extend(int d) { left -= d; top -= d; right += d; bottom += d; }
        int Width() const { return right - left; }
        int Height() const { return bottom - top; }
        float Diagonal() const { return sqrtf( (float)((right-left)*(right-left)) + (float)((bottom-top)*(bottom-top)) ); }
        int Field() const { return (right-left)*(bottom-top); }
        void SetWidth(int NewWidth) { right = left + NewWidth; }
        void SetHeight(int NewHeight) { bottom = top + NewHeight; }
        void SetSize(int NewWidth, int NewHeight) { right = left + NewWidth; bottom = top + NewHeight; }
        // Oblicza i zwraca pozycję środka
        void GetCenter(POINT_ *Out) const { Out->x = (left+right)/2; Out->y = (top+bottom)/2; }
        int GetCenterX() const { return (left+right)/2; }
        int GetCenterY() const { return (top+bottom)/2; }
        bool Empty() const { return left == 0 && top == 0 && right == 0 && bottom == 0; }
        void Clear() { left = top = right = bottom = 0; }
};

extern const RECTI RECTI_ZERO;

struct RECTF
{
        float left, top, right, bottom;

        operator float * () { return &left; }
        operator const float * () const { return &left; }

        RECTF() { }
        RECTF(float left, float top, float right, float bottom) : left(left), top(top), right(right), bottom(bottom) { }
        RECTF(const float *Arr) : left(Arr[0]), top(Arr[1]), right(Arr[2]), bottom(Arr[3]) { }

        bool operator == (const RECTF &r) const { return left == r.left && top == r.top && right == r.right && bottom == r.bottom; }
        bool operator != (const RECTF &r) const { return left != r.left || top != r.top || right != r.right || bottom != r.bottom; }

        RECTF operator + () const { return *this; }
        RECTF operator - () const { return RECTF(-right, -bottom, -left, -top); }
        RECTF operator + (const VEC2 &p) const { return RECTF(left+p.x, top+p.y, right+p.x, bottom+p.y); }
        RECTF operator - (const VEC2 &p) const { return RECTF(left-p.x, top-p.y, right-p.x, bottom-p.y); }
        RECTF operator * (float s) const { return RECTF(left*s, top*s, right*s, bottom*s); }
        RECTF operator * (const VEC2 &s) const { return RECTF(left*s.x, top*s.y, right*s.x, bottom*s.y); }
        RECTF & operator += (const VEC2 &p) { left += p.x; top += p.y; right += p.x; bottom += p.y; return *this; }
        RECTF & operator -= (const VEC2 &p) { left -= p.x; top -= p.y; right -= p.x; bottom -= p.y; return *this; }
        RECTF & operator *= (float s) { left *= s; top *= s; right *= s; bottom *= s; return *this; }
        RECTF & operator *= (const VEC2 &s) { left *= s.x; top *= s.y; right *= s.x; bottom *= s.y; return *this; }

        bool IsValid() const { return left < right && top < bottom; }
        void Repair() { if (right < left) std::swap(left, right); if (bottom < top) std::swap(top, bottom); }

        void LeftTop(VEC2 *Out) const { Out->x = left; Out->y = top; }
        void RightBottom(VEC2 *Out) const { Out->x = right; Out->y = bottom; }

        void Extend(float d) { left -= d; top -= d; right += d; bottom += d; }
        float Width() const { return right - left; }
        float Height() const { return bottom - top; }
        float Diagonal() const { return sqrtf( (right-left)*(right-left) + (bottom-top)*(bottom-top) ); }
        float Field() const { return (right-left)*(bottom-top); }
        void SetWidth(float NewWidth) { right = left + NewWidth; }
        void SetHeight(float NewHeight) { bottom = top + NewHeight; }
        void SetSize(float NewWidth, float NewHeight) { right = left + NewWidth; bottom = top + NewHeight; }
        void GetCenter(VEC2 *Out) const { Out->x = (left+right)*0.5f; Out->y = (top+bottom)*0.5f; }
        float GetCenterX() const { return (left+right)*0.5f; }
        float GetCenterY() const { return (top+bottom)*0.5f; }
        bool Empty() const { return left == 0.0f && top == 0.0f && right == 0.0f && bottom == 0.0f; }
        void Clear() { left = top = right = bottom = 0.0f; }
};

extern const RECTF RECTF_ZERO;

#ifdef WIN32
        template <> inline ::RECT & math_cast<::RECT, RECTI>(const RECTI &x) { return (::RECT&)(x); }
        template <> inline RECTI & math_cast<RECTI, ::RECT>(const ::RECT &x) { return (RECTI&)(x); }
#endif

inline void swap(RECTI &r1, RECTI &r2)
{
        std::swap(r1.left, r2.left); std::swap(r1.top, r2.top);
        std::swap(r1.right, r2.right); std::swap(r1.bottom, r2.bottom);
}

inline void swap(RECTF &r1, RECTF &r2)
{
        std::swap(r1.left, r2.left); std::swap(r1.top, r2.top);
        std::swap(r1.right, r2.right); std::swap(r1.bottom, r2.bottom);
}

inline bool PointInRect(const POINT_ &p, const RECTI &r)
{
        return
                (p.x >= r.left) &&
                (p.x <= r.right) &&
                (p.y >= r.top) &&
                (p.y <= r.bottom);
}
inline bool PointInRect(const VEC2 &p, const RECTF &r)
{
        return
                (p.x >= r.left) &&
                (p.x <= r.right) &&
                (p.y >= r.top) &&
                (p.y <= r.bottom);
}

inline bool RectInRect(const RECTI &sr, const RECTI &r)
{
        return
                (sr.left >= r.left) &&
                (sr.right <= r.right) &&
                (sr.top >= r.top) &&
                (sr.bottom <= r.bottom);
}
inline bool RectInRect(const RECTF &sr, const RECTF &r)
{
        return
                (sr.left >= r.left) &&
                (sr.right <= r.right) &&
                (sr.top >= r.top) &&
                (sr.bottom <= r.bottom);
}

inline bool OverlapRect(const RECTI &r1, const RECTI &r2)
{
        return
                (r1.left <= r2.right) &&
                (r1.right >= r2.left) &&
                (r1.top <= r2.bottom) &&
                (r1.bottom >= r2.top);
}
inline bool OverlapRect(const RECTF &r1, const RECTF &r2)
{
        return
                (r1.left <= r2.right) &&
                (r1.right >= r2.left) &&
                (r1.top <= r2.bottom) &&
                (r1.bottom >= r2.top);
}

inline bool Intersection(RECTI *Out, const RECTI &r1, const RECTI &r2)
{
        *Out = RECTI(
                std::max(r1.left, r2.left),
                std::max(r1.top, r2.top),
                std::min(r1.right, r2.right),
                std::min(r1.bottom, r2.bottom));
        return Out->IsValid();
}
inline bool Intersection(RECTF *Out, const RECTF &r1, const RECTF &r2)
{
        *Out = RECTF(
                std::max(r1.left, r2.left),
                std::max(r1.top, r2.top),
                std::min(r1.right, r2.right),
                std::min(r1.bottom, r2.bottom));
        return Out->IsValid();
}

inline void Union(RECTI *Out, const RECTI &r1, const RECTI &r2)
{
        Out->left = std::min(r1.left, r2.left);
        Out->top = std::min(r1.top, r2.top);
        Out->right = std::max(r1.right, r2.right);
        Out->bottom = std::max(r1.bottom, r2.bottom);
}
inline void Union(RECTF *Out, const RECTF &r1, const RECTF &r2)
{
        Out->left = std::min(r1.left, r2.left);
        Out->top = std::min(r1.top, r2.top);
        Out->right = std::max(r1.right, r2.right);
        Out->bottom = std::max(r1.bottom, r2.bottom);
}

// math_rect


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


struct BOX
{
        VEC3 p1, p2;

        BOX() { }
        BOX(const VEC3 &p1, const VEC3 &p2) : p1(p1), p2(p2) { }
        BOX(float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) : p1(p1x, p1y, p1z), p2(p2x, p2y, p2z) { }
        BOX(const float *Arr) : p1(&Arr[0]), p2(&Arr[3]) { }

        operator float * () { return &p1.x; }
        operator const float * () const { return &p1.x; }

        bool operator == (const BOX &p) const { return p1 == p.p1 && p2 == p.p2; }
        bool operator != (const BOX &p) const { return p1 != p.p1 || p2 != p.p2; }

        BOX operator + () const { return *this; }
        BOX operator - () const { return BOX(-p2, -p1); }
        BOX operator + (const VEC3 &v) const { return BOX(p1+v, p2+v); }
        BOX operator - (const VEC3 &v) const { return BOX(p1-v, p2-v); }
        BOX operator * (float f) const { return BOX(p1*f, p2*f); }
        BOX operator * (const VEC3 &s) const { return BOX(VEC3(p1.x*s.x, p1.y*s.y, p1.z*s.z), VEC3(p2.x*s.x, p2.y*s.y, p2.z*s.z)); }
        BOX & operator += (const VEC3 &v) { p1 += v; p2 += v; return *this; }
        BOX & operator -= (const VEC3 &v) { p1 -= v; p2 -= v; return *this; }
        BOX & operator *= (float f) { p1 *= f; p2 *= f; return *this; }
        BOX & operator *= (const VEC3 &s) { p1.x *= s.x; p1.y *= s.y; p1.z *= s.z; p2.x *= s.x; p2.y *= s.y; p2.z *= s.z; return *this; }

        bool IsValid() { return Less(p1, p2); }
        void Repair() { if (p2.x < p1.x) std::swap(p1.x, p2.x); if (p2.y < p1.y) std::swap(p1.y, p2.y); if (p2.z < p1.z) std::swap(p1.z, p2.z); }

        void Extend(float d) { p1.x -= d; p1.y -= d; p1.z -= d; p2.x += d; p2.y += d; p2.z += d; }
        void GetSize(VEC3 *Out) const { Out->x = p2.x-p1.x; Out->y = p2.y-p1.y; Out->z = p2.z-p1.z; }
        float GetCX() const { return p2.x - p1.x; }
        float GetCY() const { return p2.y - p1.y; }
        float GetCZ() const { return p2.z - p1.z; }
        void SetSize(const VEC3 &NewSize) { p2.x = p1.x + NewSize.x; p2.y = p1.y + NewSize.y; p2.z = p1.z + NewSize.z; }
        float Diagonal() const { return sqrtf( (p2.x-p1.x)*(p2.x-p1.x) + (p2.y-p1.y)*(p2.y-p1.y) + (p2.z-p1.z)*(p2.z-p1.z) ); }
        float Volume() const { return (p2.x-p1.x)*(p2.y-p1.y)*(p2.z-p1.z); }
        void CalcCenter(VEC3 *Out) const { Out->x = (p1.x+p2.x)*0.5f; Out->y = (p1.y+p2.y)*0.5f; Out->z = (p1.z+p2.z)*0.5f; }
        bool Empty() const { return *this == BOX(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); }
        void Clear() { *this = BOX(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); }
        void GetCorner(VEC3 *Out, int i) const
        {
                Out->x = (i & 1) ? p2.x : p1.x;
                Out->y = (i & 2) ? p2.y : p1.y;
                Out->z = (i & 4) ? p2.z : p1.z;
        }

        void GetAllCorners(VEC3 *Out) const
        {
                Out[0] = VEC3(p1.x, p1.y, p1.z); Out[1] = VEC3(p2.x, p1.y, p1.z); Out[2] = VEC3(p1.x, p2.y, p1.z); Out[3] = VEC3(p2.x, p2.y, p1.z);
                Out[4] = VEC3(p1.x, p1.y, p2.z); Out[5] = VEC3(p2.x, p1.y, p2.z); Out[6] = VEC3(p1.x, p2.y, p2.z); Out[7] = VEC3(p2.x, p2.y, p2.z);
        }
        void AddInternalPoint(const VEC3 &p)
        {
                p1.x = std::min(p1.x, p.x); p1.y = std::min(p1.y, p.y); p1.z = std::min(p1.z, p.z);
                p2.x = std::max(p2.x, p.x); p2.y = std::max(p2.y, p.y); p2.z = std::max(p2.z, p.z);
        }

        void AddInternalBox(const BOX &b)
        {
                p1.x = std::min(p1.x, b.p1.x); p1.y = std::min(p1.y, b.p1.y); p1.z = std::min(p1.z, b.p1.z);
                p2.x = std::max(p2.x, b.p2.x); p2.y = std::max(p2.y, b.p2.y); p2.z = std::max(p2.z, b.p2.z);
        }
};

static const BOX ZERO;

inline void swap(BOX &b1, BOX &b2)
{
        std::swap(b1.p1, b2.p1);
        std::swap(b1.p2, b2.p2);
}

inline void Scale(BOX *Out, const BOX &b, float s)
{
        Out->p1.x = b.p1.x * s; Out->p1.y = b.p1.y * s; Out->p1.z = b.p1.z * s;
        Out->p2.x = b.p2.x * s; Out->p2.y = b.p2.y * s; Out->p2.z = b.p2.z * s;
}

inline void Scale(BOX *Out, const BOX &b, const VEC3 &s)
{
        Out->p1.x = b.p1.x * s.x; Out->p1.y = b.p1.y * s.y; Out->p1.z = b.p1.z * s.z;
        Out->p2.x = b.p2.x * s.x; Out->p2.y = b.p2.y * s.y; Out->p2.z = b.p2.z * s.z;
}

inline bool PointInBox(const VEC3 &p, const BOX &b)
{
        return
                (p.x >= b.p1.x) && (p.x <= b.p2.x) &&
                (p.y >= b.p1.y) && (p.y <= b.p2.y) &&
                (p.z >= b.p1.z) && (p.z <= b.p2.z);
}

inline bool BoxInBox(const BOX &sb, const BOX &b)
{
        return
                (sb.p1.x >= b.p1.x) && (sb.p2.x <= b.p2.x) &&
                (sb.p1.y >= b.p1.y) && (sb.p2.y <= b.p2.y) &&
                (sb.p1.z >= b.p1.z) && (sb.p2.z <= b.p2.z);
}

inline bool OverlapBox(const BOX &b1, const BOX &b2)
{
        return
                (b1.p1.x <= b2.p2.x) && (b1.p2.x >= b2.p1.x) &&
                (b1.p1.y <= b2.p2.y) && (b1.p2.y >= b2.p1.y) &&
                (b1.p1.z <= b2.p2.z) && (b1.p2.z >= b2.p1.z);
}

inline bool Intersection(BOX *Out, const BOX &b1, const BOX &b2)
{
        *Out = BOX(
                VEC3(
                        std::max(b1.p1.x, b2.p1.x),
                        std::max(b1.p1.y, b2.p1.y),
                        std::max(b1.p1.z, b2.p1.z)),
                VEC3(
                        std::min(b1.p2.x, b2.p2.x),
                        std::min(b1.p2.y, b2.p2.y),
                        std::min(b1.p2.z, b2.p2.z)));
        return Out->IsValid();
}

inline void Union(BOX *Out, const BOX &b1, const BOX &b2)
{
        *Out = BOX(
                VEC3(
                        std::min(b1.p1.x, b2.p1.x),
                        std::min(b1.p1.y, b2.p1.y),
                        std::min(b1.p1.z, b2.p1.z)),
                VEC3(
                        std::max(b1.p2.x, b2.p2.x),
                        std::max(b1.p2.y, b2.p2.y),
                        std::max(b1.p2.z, b2.p2.z)));
}

// math_aabb


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


struct COLOR
{
        union
        {
                uint4 ARGB;
                PACKED struct
                {
                        uint1 B, G, R, A;
                };
        };

        COLOR() { }
        COLOR(uint4 c) : ARGB(c) { }
        COLOR(unsigned long c) : ARGB((uint4)c) { }
        COLOR(uint1 R, uint1 G, uint1 B) : B(B), G(G), R(R), A(0xFF) { }
        COLOR(uint1 A, uint1 R, uint1 G, uint1 B) : B(B), G(G), R(R), A(A) { }

        bool operator == (COLOR c) const { return ARGB == c.ARGB; }
        bool operator != (COLOR c) const { return ARGB != c.ARGB; }

        // Operatory unarne - dany kolor i negatyw koloru
        COLOR operator + () const { return *this; }
        COLOR operator - () const { return COLOR(~ARGB); }
        // Dodawanie i odejmowanie kolorów
        COLOR operator + (const COLOR &c) const { return COLOR(safe_add(A, c.A), safe_add(R, c.R), safe_add(G, c.G), safe_add(B, c.B)); }
        COLOR operator - (const COLOR &c) const { return COLOR(safe_sub(A, c.A), safe_sub(R, c.R), safe_sub(G, c.G), safe_sub(B, c.B)); }
        COLOR & operator += (const COLOR &c) { A = safe_add(A, c.A); R = safe_add(R, c.R); G = safe_add(G, c.G); B = safe_add(B, c.B); return *this; }
        COLOR & operator -= (const COLOR &c) { A = safe_sub(A, c.A); R = safe_sub(R, c.R); G = safe_sub(G, c.G); B = safe_sub(B, c.B); return *this; }
        // Mnożenie i dzielenie koloru przez liczbę
        // Autor: Tarlandil
        COLOR operator * (float s) const { return COLOR( (uint1)(minmax(0, (int)(A*s), 255)), (uint1)(minmax(0, (int)(R*s), 255)), (uint1)(minmax(0, (int)(G*s), 255)), (uint1)(minmax(0, (int)(B*s), 255)) ); }
        COLOR operator / (float s) const { return COLOR( (uint1)(minmax(0, (int)(A/s), 255)), (uint1)(minmax(0, (int)(R/s), 255)), (uint1)(minmax(0, (int)(G/s), 255)), (uint1)(minmax(0, (int)(B/s), 255)) ); }
        COLOR & operator *= (float s) { A = (uint1)(minmax(0, (int)(A*s), 255)); R = (uint1)(minmax(0, (int)(R*s), 255)); G = (uint1)(minmax(0, (int)(G*s), 255)); B = (uint1)(minmax(0, (int)(B*s), 255)); return *this; }
        COLOR & operator /= (float s) { A = (uint1)(minmax(0, (int)(A/s), 255)); R = (uint1)(minmax(0, (int)(R/s), 255)); G = (uint1)(minmax(0, (int)(G/s), 255)); B = (uint1)(minmax(0, (int)(B/s), 255));  return *this; }
        // Mnożenie dwóch kolorów, czyli modulacja
        COLOR operator * (const COLOR &c) const { return COLOR( (uint1)(((int)A*(int)c.A)/255), (uint1)(((int)R*(int)c.R)/255), (uint1)(((int)G*(int)c.G)/255), (uint1)(((int)B*(int)c.B)/255) ); }
        COLOR & operator *= (const COLOR &c) { A = (uint1)(((int)A*(int)c.A)/255); R = (uint1)(((int)R*(int)c.R)/255); G = (uint1)(((int)G*(int)c.G)/255); B = (uint1)(((int)B*(int)c.B)/255); return *this; }
};

extern const COLOR COLOR_TRANSPARENT_;
extern const COLOR COLOR_BLACK;
extern const COLOR COLOR_GRAY;
extern const COLOR COLOR_SILVER;
extern const COLOR COLOR_WHITE;
extern const COLOR COLOR_FUCHSIA;
extern const COLOR COLOR_RED;
extern const COLOR COLOR_ORANGE;
extern const COLOR COLOR_YELLOW;
extern const COLOR COLOR_LIME;
extern const COLOR COLOR_AQUA;
extern const COLOR COLOR_BLUE;
extern const COLOR COLOR_PURPLE;
extern const COLOR COLOR_MAROON;
extern const COLOR COLOR_OLIVE;
extern const COLOR COLOR_GREEN;
extern const COLOR COLOR_TEAL;


struct COLORF
{
        float R, G, B, A;

        COLORF() { }
        COLORF(float R, float G, float B) : R(R), G(G), B(B), A(1.0f) { }
        COLORF(float A, float R, float G, float B) : R(R), G(G), B(B), A(A) { }
        COLORF(const float *Arr) : R(Arr[0]), G(Arr[1]), B(Arr[2]), A(Arr[3]) { }

        operator float * () { return &R; }
        operator const float * () const { return &R; }

        bool operator == (const COLORF &c) const { return A == c.A && R == c.R && G == c.G && B == c.B; }
        bool operator != (const COLORF &c) const { return A != c.A || R != c.R || G != c.G || B != c.B; }

        // Operatory unarne - dany kolor i negatyw koloru
        COLORF operator + () const { return *this; }
        COLORF operator - () const { return COLORF(1.0f-A, 1.0f-R, 1.0f-G, 1.0f-B); }
        // Dodawanie i odejmowanie kolorów
        COLORF operator + (const COLORF &c) const { return COLORF(A+c.A, R+c.R, G+c.G, B+c.B); }
        COLORF operator - (const COLORF &c) const { return COLORF(A-c.A, R-c.R, G-c.G, B-c.B); }
        COLORF & operator += (const COLORF &c) { A += c.A; R += c.R; G += c.G; B += c.B; return *this; }
        COLORF & operator -= (const COLORF &c) { A -= c.A; R -= c.R; G -= c.G; B -= c.B; return *this; }
        // Mnożenie i dzielenie koloru przez liczbę
        COLORF operator * (float s) const { return COLORF(A*s, R*s, G*s, B*s); }
        COLORF operator / (float s) const { return COLORF(A/s, R/s, G/s, B/s); }
        COLORF & operator *= (float s) { A *= s; R *= s; G *= s; B *= s; return *this; }
        COLORF & operator /= (float s) { A /= s; R /= s; G /= s; B /= s; return *this; }
        // Mnożenie dwóch kolorów, czyli modulacja
        COLORF operator * (const COLORF &c) const { return COLORF(A*c.A, R*c.R, G*c.G, B*c.B); }
        COLORF & operator *= (const COLORF &c) { A *= c.A; R *= c.R; G *= c.G; B *= c.B; return *this; }

        bool IsValid() const { return (A >= 0.0f && A <= 1.0f) && (R >= 0.0f && R <= 1.0f) && (G >= 0.0f && G <= 1.0f) && (B >= 0.0f && B <= 1.0f); }
        void Repair()
        {
                A = minmax(0.0f, A, 1.0f); R = minmax(0.0f, R, 1.0f);
                G = minmax(0.0f, G, 1.0f); B = minmax(0.0f, B, 1.0f);
        }
};

inline void swap(COLORF &c1, COLORF &c2)
{
        std::swap(c1.A, c2.A); std::swap(c1.R, c2.R);
        std::swap(c1.G, c2.G); std::swap(c1.B, c2.B);
}

extern const COLORF COLORF_TRANSPARENT_;
extern const COLORF COLORF_BLACK;
extern const COLORF COLORF_GRAY;
extern const COLORF COLORF_SILVER;
extern const COLORF COLORF_WHITE;
extern const COLORF COLORF_FUCHSIA;
extern const COLORF COLORF_RED;
extern const COLORF COLORF_ORANGE;
extern const COLORF COLORF_YELLOW;
extern const COLORF COLORF_LIME;
extern const COLORF COLORF_AQUA;
extern const COLORF COLORF_BLUE;
extern const COLORF COLORF_PURPLE;
extern const COLORF COLORF_MAROON;
extern const COLORF COLORF_OLIVE;
extern const COLORF COLORF_GREEN;
extern const COLORF COLORF_TEAL;

inline COLORF operator * (float s, const COLORF &c) { return COLORF(c.A*s, c.R*s, c.G*s, c.B*s); }
inline COLORF operator / (float s, const COLORF &c) { return COLORF(c.A/s, c.R/s, c.G/s, c.B/s); }

inline COLOR operator * (float s, const COLOR &c) { return COLOR( (uint1)(minmax(0, (int)(c.A*s), 255)), (uint1)(minmax(0, (int)(c.R*s), 255)), (uint1)(minmax(0, (int)(c.G*s), 255)), (uint1)(minmax(0, (int)(c.B*s), 255)) ); }
inline COLOR operator / (float s, const COLOR &c) { return COLOR( (uint1)(minmax(0, (int)(c.A/s), 255)), (uint1)(minmax(0, (int)(c.R/s), 255)), (uint1)(minmax(0, (int)(c.G/s), 255)), (uint1)(minmax(0, (int)(c.B/s), 255)) ); }

template <> inline uint4 & math_cast<uint4, COLOR>(const COLOR &x) { return (uint4&)(x); }
template <> inline unsigned long & math_cast<unsigned long, COLOR>(const COLOR &x) { return (unsigned long&)(x); }
template <> inline COLOR & math_cast<COLOR, uint4>(const uint4 &x) { return (COLOR&)(x); }
template <> inline COLOR & math_cast<COLOR, unsigned long>(const unsigned long &x) { return (COLOR&)(x); }

#ifdef USE_DIRECTX
        template <> inline D3DXCOLOR & math_cast<D3DXCOLOR, COLORF>(const COLORF &x) { return (D3DXCOLOR&)(x); }
        template <> inline COLORF & math_cast<COLORF, D3DXCOLOR>(const D3DXCOLOR &x) { return (COLORF&)(x); }
#endif

inline void ColorToColorf(COLORF *Out, COLOR c)
{
        Out->R = c.R / 255.0f;
        Out->G = c.G / 255.0f;
        Out->B = c.B / 255.0f;
        Out->A = c.A / 255.0f;
}
inline COLOR ColorfToColor(const COLORF &c)
{
        return COLOR(
                (uint1)(minmax(0.0f, c.A, 1.0f)*255.0f+0.5f),
                (uint1)(minmax(0.0f, c.R, 1.0f)*255.0f+0.5f),
                (uint1)(minmax(0.0f, c.G, 1.0f)*255.0f+0.5f),
                (uint1)(minmax(0.0f, c.B, 1.0f)*255.0f+0.5f));
}


void ColorToStr(tstring *Out, COLOR Color, char Format = 'X');
void ColorToStr(tstring *Out, const COLORF &Color, char Format = 'F');

bool StrToColor(COLOR *Out, const tstring &Str);
bool StrToColor(COLORF *Out, const tstring &Str);

inline void Add(COLORF *Out, const COLORF &c1, const COLORF &c2)
{
        Out->A = c1.A + c2.A; Out->R = c1.R + c2.R;
        Out->G = c1.G + c2.G; Out->B = c1.B + c2.B;
}
inline void Sub(COLORF *Out, const COLORF &c1, const COLORF &c2)
{
        Out->A = c1.A - c2.A; Out->R = c1.R - c2.R;
        Out->G = c1.G - c2.G; Out->B = c1.B - c2.B;
}
inline void Mul(COLORF *Out, const COLORF &c1, const COLORF &c2)
{
        Out->A = c1.A * c2.A; Out->R = c1.R * c2.R;
        Out->G = c1.G * c2.G; Out->B = c1.B * c2.B;
}
inline void Mul(COLORF *Out, const COLORF &c, float s)
{
        Out->A = c.A * s; Out->R = c.R * s;
        Out->G = c.G * s; Out->B = c.B * s;
}
inline void Div(COLORF *Out, const COLORF &c, float s)
{
        Out->A = c.A / s; Out->R = c.R / s;
        Out->G = c.G / s; Out->B = c.B / s;
}
inline void Negative(COLORF *Out, const COLORF &c)
{
        Out->A = 1.0f - c.A; Out->R = 1.0f - c.R;
        Out->G = 1.0f - c.G; Out->B = 1.0f - c.B;
}

inline COLOR ARGB_ABGR(COLOR color)
{
        return COLOR(
                (color.ARGB & 0xFF00FF00) |
                ((color.ARGB >> 16) & 0x000000FF) |
                ((color.ARGB & 0x000000FF) << 16)
                );
}

inline void Lerp(COLORF *Out, const COLORF &c1, const COLORF &c2, float t)
{
        Out->A = Lerp(c1.A, c2.A, t);
        Out->R = Lerp(c1.R, c2.R, t);
        Out->G = Lerp(c1.G, c2.G, t);
        Out->B = Lerp(c1.B, c2.B, t);
}
inline COLOR Lerp(COLOR c1, COLOR c2, float t)
{
        return COLOR(
                (uint1)(minmax(0, (int)(c1.A + t*((float)c2.A-(float)c1.A)), 255)),
                (uint1)(minmax(0, (int)(c1.R + t*((float)c2.R-(float)c1.R)), 255)),
                (uint1)(minmax(0, (int)(c1.G + t*((float)c2.G-(float)c1.G)), 255)),
                (uint1)(minmax(0, (int)(c1.B + t*((float)c2.B-(float)c1.B)), 255)) );
}


void HsbToRgb(COLORF *Out, float hue, float saturation, float brightness);
void RgbToHsb(float *OutHue, float *OutSaturation, float *OutBrightness, const COLORF &Color);


inline void ColorRainbow(COLORF *Out, float t)
{
        HsbToRgb(Out, t*0.75f, 1.0f, 1.0f);
}

inline float ColorToGrayscale(const COLORF &c)
{
        return 0.299f * c.R + 0.587f * c.G + 0.114f * c.B;
}
inline float ColorToGrayscale(COLOR &c)
{
        return 0.299f * (c.R/255.0f) + 0.587f * (c.G/255.0f) + 0.114f * (c.B/255.0f);
}

inline void GrayscaleToColor(COLORF *Out, float t)
{
        Out->A = 1.0f;
        Out->R = Out->G = Out->B = t;
}
inline void GrayscaleToColor(COLOR *Out, float t)
{
        Out->A = 255;
        Out->R = Out->G = Out->B = (uint1)(t*255.0f);
}


inline void AdjustContrast(COLORF *Color, float t)
{
        Color->R = 0.5f + t * (Color->R - 0.5f);
        Color->G = 0.5f + t * (Color->G - 0.5f);
        Color->B = 0.5f + t * (Color->B - 0.5f);
}


inline void AdjustSaturation(COLORF *Color, float t)
{
        float gray = ColorToGrayscale(*Color);
        Color->R = gray + t * (Color->R - gray);
        Color->G = gray + t * (Color->G - gray);
        Color->B = gray + t * (Color->B - gray);
}

inline COLOR VectorToColor(const VEC3 &v)
{
        int R = minmax(0, (int)(127.0f * v.x + 128.0f), 255);
        int G = minmax(0, (int)(127.0f * v.y + 128.0f), 255);
        int B = minmax(0, (int)(127.0f * v.z + 128.0f), 255);
        return COLOR((uint4)((R << 16) | (G << 8) | B));
}

inline void ColorToVector(VEC3 *Out, COLOR color)
{
        const float o = 1.0f / 127.0f;
        Out->x = (((color.ARGB >> 16) & 0xFF) - 128.0f) * o;
        Out->y = (((color.ARGB >>  8) & 0xFF) - 128.0f) * o;
        Out->z = (((color.ARGB      ) & 0xFF) - 128.0f) * o;
}

inline uint ColorDistance(COLOR Color1, COLOR Color2)
{
        int rd = (int)Color1.R - (int)Color2.R;
        int gd = (int)Color1.G - (int)Color2.G;
        int bd = (int)Color1.B - (int)Color2.B;
        return (uint)(rd*rd + gd*gd + bd*bd);
}

// math_colors


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXPLANE PLANE;
#else

        struct PLANE
        {
                float a, b, c, d;

                PLANE() { }
                PLANE(float a, float b, float c, float d) : a(a), b(b), c(c), d(d) { }
                PLANE(const float *Arr) : a(Arr[0]), b(Arr[1]), c(Arr[2]), d(Arr[3]) { }

                operator float * () { return &a; }
                operator const float * () const { return &a; }

                bool operator == (const PLANE &p) const { return a == p.a && b == p.b && c == p.c && d == p.d; }
                bool operator != (const PLANE &p) const { return a != p.a || b != p.b || c != p.c || d != p.d; }

                PLANE operator + () const { return *this; }
                PLANE operator - () const { return PLANE(-a, -b, -c, -d); }

                PLANE operator * (float s) const { return PLANE(a*s, b*s, c*s, d*s); }
                PLANE operator / (float s) const { return PLANE(a/s, b/s, c/s, d/s); }
                PLANE & operator *= (float s) { a *= s; b *= s; c *= s; d *= s; return *this; }
                PLANE & operator /= (float s) { a /= s; b /= s; c /= s; d /= s; return *this; }
        };
        inline PLANE operator * (float s, const PLANE &p) { return PLANE(p.a*s, p.b*s, p.c*s, p.d*s); }
#endif

extern const PLANE PLANE_ZERO;
extern const PLANE PLANE_POSITIVE_X; 
extern const PLANE PLANE_POSITIVE_Y; 
extern const PLANE PLANE_POSITIVE_Z; 
extern const PLANE PLANE_NEGATIVE_X; 
extern const PLANE PLANE_NEGATIVE_Y; 
extern const PLANE PLANE_NEGATIVE_Z; 

inline void swap(PLANE &p1, PLANE &p2)
{
        std::swap(p1.a, p2.a); std::swap(p1.b, p2.b);
        std::swap(p1.c, p2.c); std::swap(p1.d, p2.d);
}

inline VEC3 & GetPlaneNormal(PLANE &Plane) { return (VEC3&)Plane.a; }
inline const VEC3 & GetPlaneNormal(const PLANE &Plane) { return (const VEC3&)Plane.a; }


inline void GetPlaneMemberPoint(VEC3 *Out, const PLANE &Plane)
{
        *Out = GetPlaneNormal(Plane) * -Plane.d;
}


inline void RecalculatePlaneD(PLANE *Plane, const VEC3 &p)
{
        Plane->d = -Dot(GetPlaneNormal(*Plane), p);
}

inline bool PlaneEqual(const PLANE &p1, const PLANE &p2) {
        return float_equal(p1.a, p2.a) && float_equal(p1.b, p2.b) && float_equal(p1.c, p2.c) && float_equal(p1.d, p2.d);
}


inline void PointsToPlane(PLANE *Out, const VEC3 &p1, const VEC3 &p2, const VEC3 &p3) {
        Cross(&GetPlaneNormal(*Out), p2-p1, p3-p1);
        Normalize(&GetPlaneNormal(*Out));
        Out->d = -Dot(p1, GetPlaneNormal(*Out));
}

inline void PointNormalToPlane(PLANE *Out, const VEC3 &Point, const VEC3 &Normal) {
        Out->a = Normal.x; Out->b = Normal.y; Out->c = Normal.z;
        Out->d = -Dot(Normal, Point);
}

inline void Mul(PLANE *Out, const PLANE &p, float s)
{
        Out->a = p.a * s; Out->b = p.b * s;
        Out->c = p.c * s; Out->d = p.d * s;
}
inline void Div(PLANE *Out, const PLANE &p, float s)
{
        Out->a = p.a / s; Out->b = p.b / s;
        Out->c = p.c / s; Out->d = p.d / s;
}


inline float Dot(const PLANE &p, const VEC4 &pt)
{
        return p.a*pt.x + p.b*pt.y + p.c*pt.z + p.d*pt.w;
}


inline float DotCoord(const PLANE &p, const VEC3 &pt)
{
        return p.a*pt.x + p.b*pt.y + p.c*pt.z + p.d;
}


inline float DotNormal(const PLANE &p, const VEC3 &v)
{
        return p.a*v.x + p.b*v.y + p.c*v.z;
}


inline void Normalize(PLANE *Out, const PLANE &p)
{
        float nlsq = 1.0f / Length(GetPlaneNormal(p));
        Out->a = p.a * nlsq; Out->b = p.b * nlsq;
        Out->c = p.c * nlsq; Out->d = p.d * nlsq;
}
inline void Normalize(PLANE *p)
{
        float nlsq = 1.0f / Length(GetPlaneNormal(*p));
        p->a *= nlsq; p->b *= nlsq;
        p->c *= nlsq; p->d *= nlsq;
}

// math_plane


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


struct LINE2D
{
        float a, b, c;

        LINE2D() { }
        LINE2D(float a, float b, float c) : a(a), b(b), c(c) { }
        LINE2D(const VEC2 &Normal, float c) : a(Normal.x), b(Normal.y), c(c) { }
        LINE2D(const float *Arr) : a(Arr[0]), b(Arr[1]), c(Arr[2]) { }

        operator float * () { return &a; }
        operator const float * () const { return &a; }

        VEC2 & GetNormal() { return (VEC2&)a; }
        const VEC2 & GetNormal() const { return (VEC2&)a; }

        bool operator == (const LINE2D &L) const { return a == L.a && b == L.b && c == L.c; }
        bool operator != (const LINE2D &L) const { return a != L.a || b != L.b || c != L.c; }

        LINE2D operator + () const { return *this; }
        LINE2D operator - () const { return LINE2D(-a, -b, -c); }

        // Skalowanie
        LINE2D operator * (float s) const { return LINE2D(a*s, b*s, c*s); }
        LINE2D operator / (float s) const { return LINE2D(a/s, b/s, c/s); }
        LINE2D & operator *= (float s) { a *= s; b *= s; c *= s; return *this; }
        LINE2D & operator /= (float s) { a /= s; b /= s; c /= s; return *this; }

        void GetTangent(VEC2 *Out) { Out->x = b; Out->y = -a; }


        void GetMemberPoint(VEC2 *Out)
        {
                *Out = GetNormal() * -c;
        }


        void RecalculateD(const VEC2 &p)
        {
                c = -Dot(GetNormal(), p);
        }
};
inline LINE2D operator * (float s, const LINE2D &p) { return LINE2D(p.a*s, p.b*s, p.c*s); }
inline LINE2D operator / (float s, const LINE2D &p) { return LINE2D(p.a/s, p.b/s, p.c/s); }

extern const LINE2D LINE2D_ZERO;
extern const LINE2D LINE2D_POSITIVE_X;
extern const LINE2D LINE2D_NEGATIVE_X;
extern const LINE2D LINE2D_POSITIVE_Y;
extern const LINE2D LINE2D_NEGATIVE_Y;

inline bool LineEqual(const LINE2D &p1, const LINE2D &p2) {
        return float_equal(p1.a, p2.a) && float_equal(p1.b, p2.b) && float_equal(p1.c, p2.c);
}

inline void swap(LINE2D &p1, LINE2D &p2)
{
        std::swap(p1.a, p2.a); std::swap(p1.b, p2.b); std::swap(p1.c, p2.c);
}


inline void PointsToLine(LINE2D *Out, const VEC2 &p1, const VEC2 &p2) {
        Out->a = p1.y - p2.y;
        Out->b = p2.x - p1.x;
        Out->c = - Out->a * p1.x - Out->b * p1.y;
}

inline void PointNormalToLine(LINE2D *Out, const VEC2 &Point, const VEC2 &Normal) {
        Out->a = Normal.x; Out->b = Normal.y;
        Out->c = -Dot(Normal, Point);
}

inline bool LinesParallel(const LINE2D &L1, const LINE2D &L2) {
        return FLOAT_ALMOST_ZERO(L1.a*L2.b - L2.a*L1.b);
}

inline bool LinesPerpedicular(const LINE2D &L1, const LINE2D &L2) {
        return FLOAT_ALMOST_ZERO(L1.a*L2.a + L1.b*L2.b);
}


inline float Dot(const LINE2D &L1, const LINE2D &L2) {
        return L1.a*L2.a + L1.b*L2.b;
}


inline float LineDistance(const LINE2D &L1, const LINE2D &L2) {
        return L1.c - L2.c;
}


bool LinesIntersection(VEC2 *Out, const LINE2D &L1, const LINE2D &L2);

bool PointsColinear(const VEC2 &p1, const VEC2 &p2, const VEC2 &p3);

inline void Mul(LINE2D *Out, const LINE2D &p, float s) {
        Out->a = p.a * s; Out->b = p.b * s; Out->c = p.c * s;
}
inline void Div(LINE2D *Out, const LINE2D &p, float s) {
        Out->a = p.a / s; Out->b = p.b / s; Out->c = p.c / s;
}


inline float Dot(const LINE2D &p, const VEC3 &pt) {
        return p.a*pt.x + p.b*pt.y + p.c*pt.z;
}


inline float DotCoord(const LINE2D &p, const VEC2 &pt) {
        return p.a*pt.x + p.b*pt.y + p.c;
}


inline float DotNormal(const LINE2D &p, const VEC2 &v) {
        return p.a*v.x + p.b*v.y;
}


inline void Normalize(LINE2D *Out, const LINE2D &p) {
        float nlsq = 1.f / Length(p.GetNormal());
        Out->a = p.a * nlsq; Out->b = p.b * nlsq; Out->c = p.c * nlsq;
}
inline void Normalize(LINE2D *p) {
        float nlsq = 1.f / Length(p->GetNormal());
        p->a *= nlsq; p->b *= nlsq; p->c *= nlsq;
}

// math_line3d


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXMATRIX MATRIX;
#else

        struct MATRIX
        {
                union
                {
                        struct
                        {
                                float _11, _12, _13, _14;
                                float _21, _22, _23, _24;
                                float _31, _32, _33, _34;
                                float _41, _42, _43, _44;
                        };
                        float m[4][4]; // [wiersz][kolumna]
                };

                MATRIX() { }
                MATRIX(float _11, float _12, float _13, float _14, float _21, float _22, float _23, float _24, float _31, float _32, float _33, float _34, float _41, float _42, float _43, float _44) : _11(_11), _12(_12), _13(_13), _14(_14), _21(_21), _22(_22), _23(_23), _24(_24), _31(_31), _32(_32), _33(_33), _34(_34), _41(_41), _42(_42), _43(_43), _44(_44) { }
                MATRIX(const float *Array) : _11(Array[0]), _12(Array[1]), _13(Array[2]), _14(Array[3]), _21(Array[4]), _22(Array[5]), _23(Array[6]), _24(Array[7]), _31(Array[8]), _32(Array[9]), _33(Array[10]), _34(Array[11]), _41(Array[12]), _42(Array[13]), _43(Array[14]), _44(Array[15]) { }

                operator float * () { return &_11; }
                operator const float * () const { return &_11; }

                bool operator == (const MATRIX &m) const { return _11 == m._11 && _12 == m._12 && _13 == m._13 && _14 == m._14 && _21 == m._21 && _22 == m._22 && _23 == m._23 && _24 == m._24 && _31 == m._31 && _32 == m._32 && _33 == m._33 && _34 == m._34 && _41 == m._41 && _42 == m._42 && _43 == m._43 && _44 == m._44; }
                bool operator != (const MATRIX &m) const { return _11 != m._11 || _12 != m._12 || _13 != m._13 || _14 != m._14 || _21 != m._21 || _22 != m._22 || _23 != m._23 || _24 != m._24 || _31 != m._31 || _32 != m._32 || _33 != m._33 || _34 != m._34 || _41 != m._41 || _42 != m._42 || _43 != m._43 || _44 != m._44; }

                float & operator () (unsigned Row, unsigned Col) { return m[Row][Col]; }
                float operator () (unsigned Row, unsigned Col) const { return m[Row][Col]; }

                MATRIX operator + () const { return *this; }
                MATRIX operator - () const;
                MATRIX operator + (const MATRIX &m) const;
                MATRIX operator - (const MATRIX &m) const;
                MATRIX operator * (const MATRIX &m) const
                {
                        return MATRIX(
                                _11 * m._11 + _12 * m._21 + _13 * m._31 + _14 * m._41,
                                _11 * m._12 + _12 * m._22 + _13 * m._32 + _14 * m._42,
                                _11 * m._13 + _12 * m._23 + _13 * m._33 + _14 * m._43,
                                _11 * m._14 + _12 * m._24 + _13 * m._34 + _14 * m._44,

                                _21 * m._11 + _22 * m._21 + _23 * m._31 + _24 * m._41,
                                _21 * m._12 + _22 * m._22 + _23 * m._32 + _24 * m._42,
                                _21 * m._13 + _22 * m._23 + _23 * m._33 + _24 * m._43,
                                _21 * m._14 + _22 * m._24 + _23 * m._34 + _24 * m._44,

                                _31 * m._11 + _32 * m._21 + _33 * m._31 + _34 * m._41,
                                _31 * m._12 + _32 * m._22 + _33 * m._32 + _34 * m._42,
                                _31 * m._13 + _32 * m._23 + _33 * m._33 + _34 * m._43,
                                _31 * m._14 + _32 * m._24 + _33 * m._34 + _34 * m._44,

                                _41 * m._11 + _42 * m._21 + _43 * m._31 + _44 * m._41,
                                _41 * m._12 + _42 * m._22 + _43 * m._32 + _44 * m._42,
                                _41 * m._13 + _42 * m._23 + _43 * m._33 + _44 * m._43,
                                _41 * m._14 + _42 * m._24 + _43 * m._34 + _44 * m._44);
                }
                MATRIX & operator += (const MATRIX &m);
                MATRIX & operator -= (const MATRIX &m);
                MATRIX & operator *= (const MATRIX &m);

                MATRIX operator * (float s) const;
                MATRIX operator / (float s) const;
                MATRIX & operator *= (float s);
                MATRIX & operator /= (float s);
        };
#endif

extern const MATRIX MATRIX_ZERO;
extern const MATRIX MATRIX_IDENTITY;

void MatrixToStr(tstring *Out, const MATRIX &m);
bool StrToMatrix(MATRIX *Out, const tstring &Str);

inline void swap(MATRIX &m1, MATRIX &m2)
{
        std::swap(m1._11, m2._11); std::swap(m1._12, m2._12); std::swap(m1._13, m2._13); std::swap(m1._14, m2._14);
        std::swap(m1._21, m2._21); std::swap(m1._22, m2._22); std::swap(m1._23, m2._23); std::swap(m1._24, m2._24);
        std::swap(m1._31, m2._31); std::swap(m1._32, m2._32); std::swap(m1._33, m2._33); std::swap(m1._34, m2._34);
        std::swap(m1._41, m2._41); std::swap(m1._42, m2._42); std::swap(m1._43, m2._43); std::swap(m1._44, m2._44);
}

bool MatrixEqual(const MATRIX &m1, const MATRIX &m2);
void Minus(MATRIX *m);
void Add(MATRIX *Out, const MATRIX &m1, const MATRIX &m2);
void Sub(MATRIX *Out, const MATRIX &m1, const MATRIX &m2);
void Mul(MATRIX *Out, const MATRIX &m1, const MATRIX &m2);
void Mul(MATRIX *m, float s);
void Div(MATRIX *m, float s);

void Transform(VEC4 *Out, const VEC2 &v, const MATRIX &m);
void Transform(VEC2 *Out, const VEC2 &v, const MATRIX &m);
void TransformCoord(VEC2 *Out, const VEC2 &v, const MATRIX &m);
void TransformNormal(VEC2 *Out, const VEC2 &v, const MATRIX &m);

void Transform(VEC4 *Out, const VEC3 &v, const MATRIX &m);

void Transform(VEC3 *Out, const VEC3 &v, const MATRIX &m);

void TransformCoord(VEC3 *Out, const VEC3 &v, const MATRIX &m);

void TransformNormal(VEC3 *Out, const VEC3 &v, const MATRIX &m);

void TransformArray(VEC3 OutPoints[], const VEC3 InPoints[], size_t PointCount, const MATRIX &M);
void TransformArray(VEC3 InOutPoints[], size_t PointCount, const MATRIX &M);
void TransformNormalArray(VEC3 OutPoints[], const VEC3 InPoints[], size_t PointCount, const MATRIX &M);
void TransformNormalArray(VEC3 InOutPoints[], size_t PointCount, const MATRIX &M);
void TransformCoordArray(VEC3 OutPoints[], const VEC3 InPoints[], size_t PointCount, const MATRIX &M);
void TransformCoordArray(VEC3 InOutPoints[], size_t PointCount, const MATRIX &M);


void Transform(VEC4 *Out, const VEC4 &v, const MATRIX &m);


void Transform(PLANE *Out, const PLANE &p, const MATRIX &m);
void TransformRay(VEC3 *OutOrigin, VEC3 *OutDir, const VEC3 &RayOrigin, const VEC3 &RayDir, const MATRIX &m);

void TransformBox(BOX *Out, const BOX &In, const MATRIX &M);

void TransformBoxCoord(BOX *Out, const BOX &In, const MATRIX &M);


void AxesToMatrix(MATRIX *Out, const VEC3 &AxisX, const VEC3 &AxisY, const VEC3 &AxisZ);

void AxesToMatrixTranslation(MATRIX *Out, const VEC3 &Origin, const VEC3 &AxisX, const VEC3 &AxisY, const VEC3 &AxisZ);


void LookAtLH(MATRIX *Out, const VEC3 &Eye, const VEC3 &Forward, const VEC3 &Up);
void LookAtRH(MATRIX *Out, const VEC3 &Eye, const VEC3 &Forward, const VEC3 &Up);

void Identity(MATRIX *Out);
void Translation(MATRIX *Out, float x, float y, float z);
void Translation(MATRIX *Out, const VEC3 &v);
void Scaling(MATRIX *Out, float sx, float sy, float sz);
void Scaling(MATRIX *Out, const VEC3 &sv);
void Scaling(MATRIX *Out, float s);

void ScalingAxis(MATRIX *Out, const VEC3 &n, float k);

void Rotation2D(MATRIX *Out, float Angle);
void RotationX(MATRIX *Out, float Angle);
void RotationY(MATRIX *Out, float Angle);
void RotationZ(MATRIX *Out, float Angle);
void RotationAxisLH(MATRIX *Out, const VEC3 &Axis, float Angle);
inline void RotationAxisRH(MATRIX *Out, const VEC3 &Axis, float Angle) { RotationAxisLH(Out, Axis, -Angle); }


void RotationYawPitchRoll(MATRIX *Out, float Yaw, float Pitch, float Roll);
void RotationYawPitchRoll(MATRIX *Out, const VEC3 &EulerAngles);

void RotationYawPitchRollInv(MATRIX *Out, float Yaw, float Pitch, float Roll);
void RotationYawPitchRollInv(MATRIX *Out, const VEC3 &EulerAngles);

/*/// Przetwarza macierz obrotu (to musi być czysta macierz obrotu) na kąty Eulera
void RotationMatrixToEulerAngles(const MATRIX &m, float *Yaw, float *Pitch, float *Roll);*/


void OrthoLH(MATRIX *Out, float Width, float Height, float ZNear, float ZFar);
void OrthoRH(MATRIX *Out, float Width, float Height, float ZNear, float ZFar);
void OrthoOffCenterLH(MATRIX *Out, float Left, float Right, float Bottom, float Top, float ZNear, float ZFar);
void OrthoOffCenterRH(MATRIX *Out, float Left, float Right, float Bottom, float Top, float ZNear, float ZFar);

void PerspectiveLH(MATRIX *Out, float Width, float Height, float ZNear, float ZFar);
void PerspectiveRH(MATRIX *Out, float Width, float Height, float ZNear, float ZFar);
void PerspectiveFovLH(MATRIX *Out, float FovY, float Aspect, float ZNear, float ZFar);
void PerspectiveFovRH(MATRIX *Out, float FovY, float Aspect, float ZNear, float ZFar);
void PerspectiveOffCenterLH(MATRIX *Out, float Left, float Right, float Bottom, float Top, float ZNear, float ZFar);
void PerspectiveOffCenterRH(MATRIX *Out, float Left, float Right, float Bottom, float Top, float ZNear, float ZFar);
void PerspectiveFovLH_Inf(MATRIX *Out, float FovY, float Aspect, float ZNear);

void MatrixShadow(MATRIX *Out, const VEC4 &Light, const PLANE &Plane);

void MatrixReflect(MATRIX *Out, const PLANE &p);

void Transpose(MATRIX *m);
void Lerp(MATRIX *Out, const MATRIX &m1, const MATRIX &m2, float t);
float Det(const MATRIX &m);
bool Inverse(MATRIX *Out, const MATRIX &m);

// math_matrix


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

#if defined(USE_DIRECTX) && !defined(DOXYGEN)
        typedef D3DXQUATERNION QUATERNION;
#else

        struct QUATERNION
        {
                float x, y, z, w;

                QUATERNION() { }
                QUATERNION(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) { }
                QUATERNION(const float *Array) : x(Array[0]), y(Array[1]), z(Array[2]), w(Array[3]) { }

                operator float * () { return &x; }
                operator const float * () const { return &x; }

                bool operator == (const QUATERNION &v) const { return x == v.x && y == v.y && z == v.z && w == v.w; }
                bool operator != (const QUATERNION &v) const { return x != v.x || y != v.y || z != v.z || w != v.w; }

                QUATERNION operator + () const { return *this; }
                QUATERNION operator - () const { return QUATERNION(-x, -y, -z, -w); }
                QUATERNION operator + (const QUATERNION &q) const { return QUATERNION(x+q.x, y+q.y, z+q.z, w+q.w); }
                QUATERNION operator - (const QUATERNION &q) const { return QUATERNION(x-q.x, y-q.y, z-q.z, w-q.w); }
                QUATERNION operator * (const QUATERNION &q) const { return QUATERNION(w*q.x + x*q.w + y*q.z - z*q.y, w*q.y + y*q.w + z*q.x - x*q.z, w*q.z + z*q.w + x*q.y - y*q.x, w*q.w - x*q.x - y*q.y - z*q.z); }
                QUATERNION & operator += (const QUATERNION &q) { x += q.x; y += q.y; z += q.z; w += q.w; return *this; }
                QUATERNION & operator -= (const QUATERNION &q) { x -= q.x; y -= q.y; z -= q.z; w -= q.w; return *this; }
                QUATERNION & operator *= (const QUATERNION &q) { QUATERNION R; R.x = w*q.x + x*q.w + y*q.z - z*q.y; R.y = w*q.y + y*q.w + z*q.x - x*q.z; R.z = w*q.z + z*q.w + x*q.y - y*q.x; R.w = w*q.w - x*q.x - y*q.y - z*q.z; *this = R; return *this; }
                QUATERNION & operator *= (float v) { x *= v; y *= v; z *= v; w *= v; return *this; }
                QUATERNION & operator /= (float v) { x /= v; y /= v; z /= v; w /= v; return *this; }
        };
        inline QUATERNION operator * (const QUATERNION pt, float v) { return QUATERNION(pt.x*v, pt.y*v, pt.z*v, pt.w*v); }
        inline QUATERNION operator / (const QUATERNION pt, float v) { return QUATERNION(pt.x/v, pt.y/v, pt.z/v, pt.w/v); }
        inline QUATERNION operator * (float v, const QUATERNION pt) { return QUATERNION(pt.x*v, pt.y*v, pt.z*v, pt.w*v); }
        inline QUATERNION operator / (float v, const QUATERNION pt) { return QUATERNION(pt.x/v, pt.y/v, pt.z/v, pt.w/v); }
#endif

extern const QUATERNION QUATERNION_ZERO;
extern const QUATERNION QUATERNION_IDENTITY;

inline void swap(QUATERNION &v1, QUATERNION &v2)
{
        std::swap(v1.x, v2.x);
        std::swap(v1.y, v2.y);
        std::swap(v1.z, v2.z);
        std::swap(v1.w, v2.w);
}

inline float CalcQuaternionAngle(const QUATERNION &Q) { return safe_acos(Q.w) * 2.0f; }
void CalcQuaternionAxis(VEC3 *Out, const QUATERNION &Q);

inline bool QuaternionEqual(const QUATERNION &q1, const QUATERNION &q2) {
        return float_equal(q1.x, q2.x) && float_equal(q1.y, q2.y) && float_equal(q1.z, q2.z) && float_equal(q1.w, q2.w);
}

void Minus(QUATERNION *q);
inline void Minus(QUATERNION *Out, const QUATERNION &q) { *Out = q; Minus(Out); }
void Add(QUATERNION *Out, const QUATERNION &q1, const QUATERNION &q2);
void Sub(QUATERNION *Out, const QUATERNION &q1, const QUATERNION &q2);
void Mul(QUATERNION *Out, const QUATERNION &q1, const QUATERNION &q2);
void Mul(QUATERNION *q, float v);
void Div(QUATERNION *q, float v);
inline void Mul(QUATERNION *Out, const QUATERNION &q, float v) { *Out = q; Mul(Out, v); }
inline void Div(QUATERNION *Out, const QUATERNION &q, float v) { *Out = q; Div(Out, v); }

void RotationMatrixToQuaternion(QUATERNION *Out, const MATRIX &RotationMatrix);
void QuaternionToRotationMatrix(MATRIX *Out, const QUATERNION &q);

void QuaternionTransform(VEC3 *Out, const VEC3 &p, const QUATERNION &q);
void EulerAnglesToQuaternionO2I(QUATERNION *Out, float Yaw, float Pitch, float Roll);
void EulerAnglesToQuaternionI2O(QUATERNION *Out, float Yaw, float Pitch, float Roll);
void QuaternionO2IToEulerAngles(float *Yaw, float *Pitch, float *Roll, const QUATERNION &q);
void QuaternionI2OToEulerAngles(float *Yaw, float *Pitch, float *Roll, const QUATERNION &q);

void QuaternionRotationX(QUATERNION *Out, float a);
void QuaternionRotationY(QUATERNION *Out, float a);
void QuaternionRotationZ(QUATERNION *Out, float a);
void AxisToQuaternion(QUATERNION *Out, const VEC3 &Axis, float Angle);


void QuaternionDiff(QUATERNION *Out, const QUATERNION &a, const QUATERNION &b);
void Conjugate(QUATERNION *q);
inline void Conjugate(QUATERNION *Out, const QUATERNION &q) { *Out = q; Conjugate(Out); }
void Inverse(QUATERNION *Out, const QUATERNION &q);
void Normalize(QUATERNION *Out, const QUATERNION &q);
void Normalize(QUATERNION *InOut);
float Dot(const QUATERNION &q1, const QUATERNION &q2);
float LengthSq(const QUATERNION &q);
float Length(const QUATERNION &q);
void Log(QUATERNION *Out, const QUATERNION &q);
void Exp(QUATERNION *Out, const QUATERNION &q);

void Pow(QUATERNION *InOut, float t);
inline void Pow(QUATERNION *Out, const QUATERNION &q, float t) { *Out = q; Pow(Out, t); }
void Lerp(QUATERNION *Out, const QUATERNION &q1, const QUATERNION &q2, float t);

void Slerp(QUATERNION *Out, const QUATERNION &q0, const QUATERNION &q1, float t);
void Squad(QUATERNION *Out, const QUATERNION &Q1, const QUATERNION &A, const QUATERNION &B, const QUATERNION &C, float t);
void SquadSetup(QUATERNION *OutA, QUATERNION *OutB, QUATERNION *OutC, const QUATERNION &Q0, const QUATERNION &Q1, const QUATERNION &Q2, const QUATERNION &Q3);

// math_quaternion


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

struct FRUSTUM_PLANES;
struct FRUSTUM_POINTS;


struct FRUSTUM_PLANES
{
        static const size_t PLANE_LEFT   = 0;
        static const size_t PLANE_RIGHT  = 1;
        static const size_t PLANE_TOP    = 2;
        static const size_t PLANE_BOTTOM = 3;
        static const size_t PLANE_NEAR   = 4;
        static const size_t PLANE_FAR    = 5;

        PLANE Planes[6];

        FRUSTUM_PLANES() { }
        FRUSTUM_PLANES(const MATRIX &WorldViewProj) { Set(WorldViewProj); }
        FRUSTUM_PLANES(const FRUSTUM_POINTS &FrustumPoints) { Set(FrustumPoints); }
        void Set(const MATRIX &WorldViewProj);
        void Set(const FRUSTUM_POINTS &FrustumPoints);
        void Normalize();

        PLANE & operator [] (size_t Index) { return Planes[Index]; }
        const PLANE & operator [] (size_t Index) const { return Planes[Index]; }
};


struct FRUSTUM_POINTS
{
        static const size_t NEAR_LEFT_BOTTOM  = 0;
        static const size_t NEAR_RIGHT_BOTTOM = 1;
        static const size_t NEAR_LEFT_TOP     = 2;
        static const size_t NEAR_RIGHT_TOP    = 3;
        static const size_t FAR_LEFT_BOTTOM   = 4;
        static const size_t FAR_RIGHT_BOTTOM  = 5;
        static const size_t FAR_LEFT_TOP      = 6;
        static const size_t FAR_RIGHT_TOP     = 7;

        VEC3 Points[8];

        FRUSTUM_POINTS() { }
        FRUSTUM_POINTS(const FRUSTUM_PLANES &FrustumPlanes) { Set(FrustumPlanes); }
        FRUSTUM_POINTS(const MATRIX &WorldViewProjInv) { Set(WorldViewProjInv); }
        void Set(const FRUSTUM_PLANES &FrustumPlanes);
        void Set(const MATRIX &WorldViewProjInv);

        VEC3 & operator [] (size_t Index) { return Points[Index]; }
        const VEC3 & operator [] (size_t Index) const { return Points[Index]; }

        void CalcCenter(VEC3 *Out) const;
        void CalcBoundingBox(BOX *Box) const;

        void CalcBoundingSphere(VEC3 *SphereCenter, float *SphereRadius) const;
};


class FRUSTUM_RADAR
{
private:
        VEC3 m_Eye;
        VEC3 m_Forward;
        VEC3 m_Up;
        VEC3 m_Right;
        float m_RFactor;
        float m_UFactor;
        float m_RSphereFactor;
        float m_USphereFactor;
        float m_ZNear;
        float m_ZFar;

public:
        FRUSTUM_RADAR() { }
        FRUSTUM_RADAR(const VEC3 &Eye, const VEC3 &Forward, const VEC3 &Up, const VEC3 &Right, float FovY, float Aspect, float ZNear, float ZFar) { Set(Eye, Forward, Up, Right, FovY, Aspect, ZNear, ZFar); }

        const VEC3 & GetEye() const     { return m_Eye; }
        const VEC3 & GetForward() const { return m_Forward; }
        const VEC3 & GetUp() const      { return m_Up; }
        const VEC3 & GetRight() const   { return m_Right; }
        float GetZNear() const   { return m_ZNear; }
        float GetZFar() const    { return m_ZFar; }

        float GetRFactor() const { return m_RFactor; }
        float GetUFactor() const { return m_UFactor; }
        float GetRSphereFactor() const { return m_RSphereFactor; }
        float GetUSphereFactor() const { return m_USphereFactor; }

        void SetEye    (const VEC3 &Eye)     { m_Eye = Eye; }
        void SetForward(const VEC3 &Forward) { m_Forward = Forward; }
        void SetUp     (const VEC3 &Up)      { m_Up = Up; }
        void SetRight  (const VEC3 &Right)   { m_Right = Right; }
        void SetZNear(float ZNear) { m_ZNear = ZNear; }
        void SetZFor (float ZFar)  { m_ZFar = ZFar; }
        void SetFovAndAspect(float FovY, float Aspect);
        
        void Set(const VEC3 &Eye, const VEC3 &Forward, const VEC3 &Up, const VEC3 &Right, float FovY, float Aspect, float ZNear, float ZFar) { SetProjection(FovY, Aspect, ZNear, ZFar); SetView(Eye, Forward, Up, Right); }
        void SetProjection(float FovY, float Aspect, float ZNear, float ZFar);
        void SetView(const VEC3 &Eye, const VEC3 &Forward, const VEC3 &Up, const VEC3 &Right);
};

// math_frustum


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

float CapsuleVolume(const VEC3 &p1, const VEC3 &p2, float R);
void RandomPointInCapsule(VEC3 *Out, const VEC3 &p1, const VEC3 &p2, float R, RandomGenerator &Rand);
void RandomPointInCapsule(VEC3 *Out, const VEC3 &p1, const VEC3 &p2, float R);



//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// INNE


template <typename T>
inline void SmoothCD_T(T *InOutPos, const T &Dest, T *InOutVel, float SmoothTime, float TimeDelta)
{
        assert(0 && "SmoothCD_T for invalid template type.");
}

template <>
inline void SmoothCD_T<float>(float *InOutPos, const float &Dest, float *InOutVel, float SmoothTime, float TimeDelta)
{
        SmoothCD(InOutPos, Dest, InOutVel, SmoothTime, TimeDelta);
}

template <>
inline void SmoothCD_T<VEC2>(VEC2 *InOutPos, const VEC2 &Dest, VEC2 *InOutVel, float SmoothTime, float TimeDelta)
{
        SmoothCD(&InOutPos->x, Dest.x, &InOutVel->x, SmoothTime, TimeDelta);
        SmoothCD(&InOutPos->y, Dest.y, &InOutVel->y, SmoothTime, TimeDelta);
}

template <>
inline void SmoothCD_T<VEC3>(VEC3 *InOutPos, const VEC3 &Dest, VEC3 *InOutVel, float SmoothTime, float TimeDelta)
{
        SmoothCD(&InOutPos->x, Dest.x, &InOutVel->x, SmoothTime, TimeDelta);
        SmoothCD(&InOutPos->y, Dest.y, &InOutVel->y, SmoothTime, TimeDelta);
        SmoothCD(&InOutPos->z, Dest.z, &InOutVel->z, SmoothTime, TimeDelta);
}

template <typename T>
class SmoothCD_obj
{
public:
        T Pos;
        T Dest; 
        T Vel;
        float SmoothTime;

        SmoothCD_obj() { }
        SmoothCD_obj(const T &Pos, float SmoothTime, const T &Vel) : Pos(Pos), Dest(Pos), Vel(Vel), SmoothTime(SmoothTime) { }
        void Set(const T &Pos, const T &Vel) { this->Pos = Pos; this->Dest = Pos; this->Vel = Vel; }

        void Update(const T &Dest, float TimeDelta) {
                this->Dest = Dest;
                SmoothCD_T<T>(&Pos, Dest, &Vel, SmoothTime, TimeDelta);
        }
};

// math_smoothcd



void SphericalToCartesian(VEC3 *Out, float Yaw, float Pitch, float R);

void CartesianToSpherical(float *OutYaw, float *OutPitch, float *OutR, const VEC3 &Pos);

// math_spherical


void ConvexHull2D(std::vector<VEC2> *OutPoints, const VEC2 InPoints[], size_t InPointCount);


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH


float ClosestPointOnLine(const VEC3 &p, const VEC3 &RayOrig, const VEC3 &RayDir);

float PointToLineDistance(const VEC3 &P, const VEC3 &LineOrig, const VEC3 &LineDir);

bool PointOnLine(const VEC3 &P, const VEC3 &LineOrig, const VEC3 &LineDir);

float PointToLine(const VEC3 &P, const VEC3 &LineOrig, const VEC3 &LineDir);

bool PointInLineSegment(const VEC3 &P, const VEC3 &SegmentBeg, const VEC3 &SegmentEnd);
void ClosestPointOnLineSegment(VEC3 *Out, const VEC3 &P, const VEC3 &P1, const VEC3 &P2);

void ClosestPointInBox(VEC3 *Out, const BOX &Box, const VEC3 &p);

float PointToBoxDistance(const VEC3 &p, const BOX &box);
bool PointInSphere(const VEC3 &p, const VEC3 &SphereCenter, float SphereRadius);

float PointToSphereDistance(const VEC3 &p, const VEC3 &SphereCenter, float SphereRadius);

void ClosestPointOnSphere(VEC3 *Out, const VEC3 &SphereCenter, float SphereRadius, const VEC3 &p);

bool PointOnPlane(const VEC3 &p, const PLANE &plane);
void ClosestPointOnPlane(VEC3 *Out, const PLANE &Plane, const VEC3 &p);
bool PointOnTriangle(const VEC3 &point, const VEC3 &pa, const VEC3 &pb, const VEC3 &pc);
bool PointInFrustum(const VEC3 &p, const FRUSTUM_PLANES &Frustum);
bool PointInFrustum(const VEC3 &p, const FRUSTUM_RADAR &Frustum);

bool ClosestPointsOnLines(float *OutT1, float *OutT2, const VEC3 &RayOrig1, const VEC3 &RayDir1, const VEC3 &RayOrig2, const VEC3 &RayDir2);

bool RayToBox(float *OutT, const VEC3 &RayOrig, const VEC3 &RayDir, const BOX &Box);

bool RayToSphere(const VEC3 &RayOrig, const VEC3 &RayDir, const VEC3 &SphereCenter, float SphereRadius, float *OutT);

bool RayToPlane(const VEC3 &RayOrig, const VEC3 &RayDir, const PLANE &Plane, float *OutT, float *OutVD);

bool RayToTriangle(
        const VEC3 &RayOrig, const VEC3 &RayDir,
        const VEC3 &p0, const VEC3 &p1, const VEC3 &p2,
        bool BackfaceCulling,
        float *OutT);
bool RayToFrustum(const VEC3 &RayOrig, const VEC3 &RayDir, const FRUSTUM_PLANES &Frustum, float *t_near, float *t_far);
bool SphereToBox(const VEC3 &SphereCenter, float SphereRadius, const BOX &Box);
bool SphereInBox(const VEC3 &SphereCenter, float SphereRadius, const BOX &Box);
bool BoxInSphere(const BOX &Box, const VEC3 &SphereCenter, float SphereRadius);
bool PlaneToBox(const PLANE &plane, const BOX &box);

int ClassifyBoxToPlane(const PLANE &plane, const BOX &box);
bool TriangleInBox(const VEC3 &p1, const VEC3 &p2, const VEC3 &p3, const BOX &Box);

bool TriangleToBox(const VEC3 &p1, const VEC3 &p2, const VEC3 &p3, const BOX &Box, const PLANE *OptTrianglePlane = NULL);

bool BoxToFrustum_Fast(const BOX &Box, const FRUSTUM_PLANES &Frustum);

bool BoxToFrustum(const BOX &Box, const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints = NULL);
bool BoxToFrustum(const BOX &Box, const FRUSTUM_RADAR &Frustum);
bool BoxInFrustum(const BOX &Box, const FRUSTUM_PLANES &Frustum);
bool SphereToSphere(const VEC3 &Center1, float Radius1, const VEC3 &Center2, float Radius2);

bool SphereToPlane(const VEC3 &SphereCenter, float SphereRadius, const PLANE &Plane);

int ClassifySphereToPlane(const VEC3 &SphereCenter, float SphereRadius, const PLANE &Plane);

bool SphereToFrustum_Fast(const VEC3 &SphereCenter, float SphereRadius, const FRUSTUM_PLANES &Frustum);

bool SphereToFrustum(
        const VEC3 &SphereCenter, float SphereRadius,
        const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints = NULL);

bool SphereToFrustum(const VEC3 &SphereCenter, float SphereRadius, const FRUSTUM_RADAR &Frustum);
bool SphereInFrustum(const VEC3 &SphereCenter, float SphereRadius, const FRUSTUM_PLANES &Frustum);

bool Intersect2Planes(const PLANE &Plane1, const PLANE &Plane2, VEC3 *LineOrig, VEC3 *LineDir);
bool Intersect3Planes(const PLANE &P1, const PLANE &P2, const PLANE &P3, VEC3 *OutP);
bool PlaneToTriangle(const PLANE &Plane, const VEC3 &p0, const VEC3 &p1, const VEC3 &p2);

int ClassifyPlaneToTriangle(const PLANE &Plane, const VEC3 &p0, const VEC3 &p1, const VEC3 &p2);
int ClassifyFrustumToPlane(const FRUSTUM_POINTS &Frustum, const PLANE &Plane);
bool IntersectLines(const VEC2 &p1, const VEC2 &p2, const VEC2 &p3, const VEC2 &p4);

bool TriangleToTriangle(
        const VEC3 &V0,const VEC3 &V1,const VEC3 &V2,
        const VEC3 &U0,const VEC3 &U1,const VEC3 &U2);
bool TriangleInFrustum(const VEC3 &t1, const VEC3 &t2, const VEC3 &t3, const FRUSTUM_PLANES &Frustum);

bool TriangleToFrustum(
        const VEC3 &t1, const VEC3 &t2, const VEC3 &t3, const PLANE *OptTrianglePlane,
        const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints);
bool FrustumToFrustum(
        const FRUSTUM_PLANES &Frustum1_Planes, const FRUSTUM_POINTS &Frustum1_Points,
        const FRUSTUM_PLANES &Frustum2_Planes, const FRUSTUM_POINTS &Frustum2_Points);

bool PointInPolygon(const VEC2 &Point, const VEC2 PolygonPoints[], uint PolygonPointCount);

bool RayToPolygon(const VEC3 &RayOrig, const VEC3 &RayDir, const VEC3 PolygonPoints[], uint PolygonPointCount, bool BackfaceCulling, float *OutT);


bool SweptSphereToPlane(const VEC3 &SphereCenter, float SphereRadius, const VEC3 &SphereSweepDir, const PLANE &Plane, float *OutT0, float *OutT1);

bool SweptSphereToFrustum(const VEC3 &SphereCenter, float SphereRadius, const VEC3 &SphereSweepDir, const FRUSTUM_PLANES &Frustum);

bool SweptSphereToSphere(
        const VEC3 &SphereCenter1, float SphereRadius1,
        const VEC3 &SphereCenter2, float SphereRadius2,
        const VEC3 &SphereSweepDir1,
        float *OutT1, float *OutT2);

bool SweptBoxToBox(const BOX &Box1, const BOX &Box2, const VEC3 &SweepDirBox1, float *OutT);

// math_collisions



void SphereBoundingPoints(VEC3 *OutSphereCenter, float *OutSphereRadius, const VEC3 Points[], size_t PointCount);
void BoxBoundingPoints(BOX *box, const VEC3 points[], size_t PointCount);
void BoxBoundingSphere(VEC3 *SphereCenter, float *SphereRadius, const BOX &Box);
void SphereBoundingBox(BOX *Out, const VEC3 &SphereCenter, float SphereRadius);

void BoxBoundingSpheres(BOX *OutBox, const VEC3 SpheresCenter[], const float SpheresRadius[], size_t SphereCount);

void RandomPointInUnitSphere(VEC3 *Out, RandomGenerator &Rand);
void RandomPointInUnitSphere(VEC3 *Out);
void RandomPointInUnitCircle(VEC2 *Out, RandomGenerator &Rand);
void RandomPointInUnitCircle(VEC2 *Out);


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH

extern const float POISSON_DISC_1D[];
const uint4 POISSON_DISC_1D_COUNT = 100;

extern const VEC2 POISSON_DISC_2D[];
const uint4 POISSON_DISC_2D_COUNT = 100;

extern const VEC3 POISSON_DISC_3D[];
const uint4 POISSON_DISC_3D_COUNT = 100;

// math_poisson_disc

// code_math

} // namespace common


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Konwersje


template <>
struct SthToStr_obj<common::POINT_>
{
        void operator () (tstring *Str, const common::POINT_ &Sth)
        {
                *Str = common::Format(_T("#,#")) % Sth.x % Sth.y;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::POINT_>
{
        bool operator () (common::POINT_ *Sth, const tstring &Str)
        {
                size_t CommaPos = Str.find(_T(','));
                if (CommaPos == tstring::npos) return false;
                if (!StrToSth<int>(&Sth->x, Str.substr(0, CommaPos))) return false;
                if (!StrToSth<int>(&Sth->y, Str.substr(CommaPos+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::VEC2>
{
        void operator () (tstring *Str, const common::VEC2 &Sth)
        {
                *Str = common::Format(_T("#,#")) % Sth.x % Sth.y;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::VEC2>
{
        bool operator () (common::VEC2 *Sth, const tstring &Str)
        {
                size_t CommaPos = Str.find(_T(','));
                if (CommaPos == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->x, Str.substr(0, CommaPos))) return false;
                if (!StrToSth<float>(&Sth->y, Str.substr(CommaPos+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::VEC3>
{
        void operator () (tstring *Str, const common::VEC3 &Sth)
        {
                *Str = common::Format(_T("#,#,#")) % Sth.x % Sth.y % Sth.z;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::VEC3>
{
        bool operator () (common::VEC3 *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->x, Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->y, Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->z, Str.substr(CommaPos2+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::VEC4>
{
        void operator () (tstring *Str, const common::VEC4 &Sth)
        {
                *Str = common::Format(_T("#,#,#,#")) % Sth.x % Sth.y % Sth.z % Sth.w;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::VEC4>
{
        bool operator () (common::VEC4 *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                size_t CommaPos3 = Str.find(_T(','), CommaPos2+1);
                if (CommaPos3 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->x, Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->y, Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->z, Str.substr(CommaPos2+1, CommaPos3-(CommaPos2+1)))) return false;
                if (!StrToSth<float>(&Sth->w, Str.substr(CommaPos3+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::RECTI>
{
        void operator () (tstring *Str, const common::RECTI &Sth)
        {
                *Str = common::Format(_T("#,#,#,#")) % Sth.left % Sth.top % Sth.right % Sth.bottom;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::RECTI>
{
        bool operator () (common::RECTI *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                size_t CommaPos3 = Str.find(_T(','), CommaPos2+1);
                if (CommaPos3 == tstring::npos) return false;
                if (!StrToSth<int>(&Sth->left,   Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<int>(&Sth->top,    Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<int>(&Sth->right,  Str.substr(CommaPos2+1, CommaPos3-(CommaPos2+1)))) return false;
                if (!StrToSth<int>(&Sth->bottom, Str.substr(CommaPos3+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::RECTF>
{
        void operator () (tstring *Str, const common::RECTF &Sth)
        {
                *Str = common::Format(_T("#,#,#,#")) % Sth.left % Sth.top % Sth.right % Sth.bottom;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::RECTF>
{
        bool operator () (common::RECTF *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                size_t CommaPos3 = Str.find(_T(','), CommaPos2+1);
                if (CommaPos3 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->left,   Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->top,    Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->right,  Str.substr(CommaPos2+1, CommaPos3-(CommaPos2+1)))) return false;
                if (!StrToSth<float>(&Sth->bottom, Str.substr(CommaPos3+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::BOX>
{
        void operator () (tstring *Str, const common::BOX &Sth)
        {
                *Str = common::Format(_T("#;#")) % Sth.p1 % Sth.p2;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::BOX>
{
        bool operator () (common::BOX *Sth, const tstring &Str)
        {
                size_t SemicolonPos = Str.find(_T(';'));
                if (SemicolonPos == tstring::npos) return false;
                if (!StrToSth<common::VEC3>(&Sth->p1, Str.substr(0, SemicolonPos))) return false;
                if (!StrToSth<common::VEC3>(&Sth->p2, Str.substr(SemicolonPos+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <> struct SthToStr_obj<common::COLOR> {
        void operator () (tstring *Str, const common::COLOR &Sth) { common::ColorToStr(Str, Sth); }
        static inline bool IsSupported() { return true; }
};
template <> struct StrToSth_obj<common::COLOR> {
        bool operator () (common::COLOR *Sth, const tstring &Str) { return common::StrToColor(Sth, Str); }
        static inline bool IsSupported() { return true; }
};

template <> struct SthToStr_obj<common::COLORF> {
        void operator () (tstring *Str, const common::COLORF &Sth) { common::ColorToStr(Str, Sth); }
        static inline bool IsSupported() { return true; }
};
template <> struct StrToSth_obj<common::COLORF> {
        bool operator () (common::COLORF *Sth, const tstring &Str) { return common::StrToColor(Sth, Str); }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::PLANE>
{
        void operator () (tstring *Str, const common::PLANE &Sth)
        {
                *Str = common::Format(_T("#,#,#,#")) % Sth.a % Sth.b % Sth.c % Sth.d;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::PLANE>
{
        bool operator () (common::PLANE *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                size_t CommaPos3 = Str.find(_T(','), CommaPos2+1);
                if (CommaPos3 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->a, Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->b, Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->c, Str.substr(CommaPos2+1, CommaPos3-(CommaPos2+1)))) return false;
                if (!StrToSth<float>(&Sth->d, Str.substr(CommaPos3+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::LINE2D>
{
        void operator () (tstring *Str, const common::LINE2D &Sth)
        {
                *Str = common::Format(_T("#,#,#")) % Sth.a % Sth.b % Sth.c;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::LINE2D>
{
        bool operator () (common::LINE2D *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->a, Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->b, Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->c, Str.substr(CommaPos2+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

template <> struct SthToStr_obj<common::MATRIX> {
        void operator () (tstring *Str, const common::MATRIX &Sth) { common::MatrixToStr(Str, Sth); }
        static inline bool IsSupported() { return true; }
};
template <> struct StrToSth_obj<common::MATRIX> {
        bool operator () (common::MATRIX *Sth, const tstring &Str) { return common::StrToMatrix(Sth, Str); }
        static inline bool IsSupported() { return true; }
};

template <>
struct SthToStr_obj<common::QUATERNION>
{
        void operator () (tstring *Str, const common::QUATERNION &Sth)
        {
                *Str = common::Format(_T("#,#,#,#")) % Sth.x % Sth.y % Sth.z % Sth.w;
        }
        static inline bool IsSupported() { return true; }
};

template <>
struct StrToSth_obj<common::QUATERNION>
{
        bool operator () (common::QUATERNION *Sth, const tstring &Str)
        {
                size_t CommaPos1 = Str.find(_T(','));
                if (CommaPos1 == tstring::npos) return false;
                size_t CommaPos2 = Str.find(_T(','), CommaPos1+1);
                if (CommaPos2 == tstring::npos) return false;
                size_t CommaPos3 = Str.find(_T(','), CommaPos2+1);
                if (CommaPos3 == tstring::npos) return false;
                if (!StrToSth<float>(&Sth->x, Str.substr(0, CommaPos1))) return false;
                if (!StrToSth<float>(&Sth->y, Str.substr(CommaPos1+1, CommaPos2-(CommaPos1+1)))) return false;
                if (!StrToSth<float>(&Sth->z, Str.substr(CommaPos2+1, CommaPos3-(CommaPos2+1)))) return false;
                if (!StrToSth<float>(&Sth->w, Str.substr(CommaPos3+1))) return false;
                return true;
        }
        static inline bool IsSupported() { return true; }
};

// code_sthtostr
#endif

---