math.h

#pragma once

#include "PhysicsConfig.h"
#include <iostream>
#include <stdexcept>
#include <vector>
#include <assert.h>

namespace Physics
{

const float MAX_FLOAT = 1e30f;
const float MIN_FLOAT = -MAX_FLOAT;
const float MIN_DIST = 0.001f;
const float MIN_DIST_SQ = MIN_DIST * MIN_DIST;

typedef D3DXMATRIX Matrix4x4;

enum SIDE
{
    SIDE_ON     = 0,
    SIDE_FRONT  = 1,
    SIDE_BACK   = 2,
    SIDE_BOTH   = 3,
    SIDE_FORCE_DWORD = 0x7fffffff
};

enum AXIS_INDEX
{
    AXIS_X,
    AXIS_Y,
    AXIS_Z
};



inline float min2(float a, float b)
{
    return a < b ? a : b;
}

inline float max2(float a, float b)
{
    return a > b ? a : b;
}

inline bool amostZero(float d, float epsilon=0.0004f)
{
    return d>-epsilon && d<epsilon;
}


//
//2维向量
//
class Vector2 : public D3DXVECTOR2
{
public:
    Vector2() : D3DXVECTOR2(0.0f, 0.0f) {}

    Vector2(float x_, float y_) : D3DXVECTOR2(x_, y_) {}

    void init(float x_, float y_)
    {
        x = x_;
        y = y_;
    }

    Vector2 operator+(const Vector2 & r)
    {
        return Vector2(x + r.x, y + r.y);
    }

    Vector2 operator-(const Vector2 & r)
    {
        return Vector2(x - r.x, y - r.y);
    }
};

//
// 3D vector
//
class Vector3 : public D3DXVECTOR3
{
public:
    Vector3()
    {}

    Vector3(float fx, float fy, float fz)
        :D3DXVECTOR3(fx, fy, fz)
    {}

    Vector3(const D3DXVECTOR3 & v)
        : D3DXVECTOR3(v.x, v.y, v.z)
    {}

    Vector3(const Vector3 & v)
        : D3DXVECTOR3(v.x, v.y, v.z)
    {}

    inline bool isZero() const
    {
        return x==0.0f && y==0.0f && z==0.0f;
    }

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

    //求负
    inline void negative()
    {
        x = -x; y = -y; z=-z;
    }

    inline void set(float fx, float fy, float fz) 
    { 
        x = fx; y = fy; z = fz;
    }

    float lengthSq() const
    {
        return D3DXVec3LengthSq(this);
    }

    float length() const
    {
        return D3DXVec3Length(this);
    }

    float distToSq(const Vector3 & p) const
    {
        return D3DXVec3LengthSq(&(*this - p));
    }

    float distTo(const Vector3 & p) const
    {
        return D3DXVec3Length(&(*this - p));
    }

    void normalize()
    {
        D3DXVec3Normalize(this, this);
    }

    void normalize(Vector3 & out) const
    {
        D3DXVec3Normalize(&out, this);
    }
    
    //点乘
    float dot(const Vector3& v) const
    {
        return (*this) * v;
    }

    //差乘
    Vector3 cross(const Vector3& v) const
    {
        Vector3 temp;
        D3DXVec3Cross(&temp, this, &v);
        return temp;
    }

    //差乘
    void cross(Vector3 & out, const Vector3& v) const
    {
        D3DXVec3Cross(&out, this, &v);
    }

    void toD3DXVec3(D3DXVECTOR3 & left) const
    {
        left.x = x;
        left.y = y;
        left.z = z;
    }

    void applyMatrix(const Matrix4x4 & mat)
    {
        D3DXVec3TransformCoord(this, this, &mat);
    }

    void applyNormalMatrix(const Matrix4x4 & mat)
    {
        D3DXVec3TransformNormal(this, this, &mat);
    }

    void applyMatrix(Vector3 & out, const Matrix4x4 & mat) const
    {
        D3DXVec3TransformCoord(&out, this, &mat);
    }

    void applyNormalMatrix(Vector3 & out, const Matrix4x4 & mat) const
    {
        D3DXVec3TransformNormal(&out, this, &mat);
    }

    bool tooClose(const Vector3 & v) const
    {
        return distToSq(v) <= MIN_DIST_SQ;
    }

    void projectTo2D(Vector2 & p) const;

    void projectTo2D(int & u, int & v) const;

	int maxAxis() const;

    float minValue() const
    {
        return min2(x, min2(y, z));
    }

    //
    ///运算符重载
    //
    
    Vector3 operator - (const Vector3& v) const
    {
        return Vector3(x - v.x, y - v.y, z - v.z) ;
    }

    Vector3 operator-() const
    {
        return Vector3(-x, -y, -z);
    }

    Vector3 operator + (const Vector3& v) const
    {
        return Vector3(x + v.x, y + v.y, z + v.z) ;
    }

    float operator * (const Vector3& v) const
    {
        return x*v.x + y*v.y + z*v.z;
    }

    Vector3 operator * (float f) const
    {
        return Vector3(x * f, y * f, z * f) ;
    }

    Vector3 operator / (float f) const
    {
        return Vector3(x / f, y / f, z / f) ;
    }

    const Vector3 & operator=(const Vector3 & r)
    {
        set(r.x, r.y, r.z);
        return *this;
    }

    const Vector3 & operator=(const D3DXVECTOR3 & r)
    {
        set(r.x, r.y, r.z);
        return *this;
    }

    const Vector3 & operator+=(const Vector3 & r)
    {
        x += r.x; y += r.y; z += r.z;
        return *this;
    }

    const Vector3 & operator-=(const Vector3 & r)
    {
        x -= r.x; y -= r.y; z -= r.z;
        return *this;
    }

    const Vector3 & operator*=(float f)
    {
        x *= f; y *= f; z *= f;
        return *this;
    }

    const Vector3 & operator /= (float f)
    {
        x /= f; y /= f; z /= f;
        return *this;
    }

    float & operator[](int index)
    {
        if (index == AXIS_X) return x;
        if (index == AXIS_Y) return y;
        if (index == AXIS_Z) return z;

        assert( 0 && "Vector3 index out of range");
        return x;
    }

    float operator[](int index) const
    {
        if (index == AXIS_X) return x;
        if (index == AXIS_Y) return y;
        if (index == AXIS_Z) return z;

        assert( 0 && "Vector3 index out of range");
        return x;
    }

    friend std::ostream & operator<<(std::ostream & out, const Vector3 & v);
    friend std::istream & operator>>(std::istream & in, Vector3 & v);
};

namespace MathConst
{
    const Vector3 vec3Max = Vector3(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
    const Vector3 vec3Min = Vector3(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
    const Vector3 vec3Zero = Vector3(0.0f, 0.0f, 0.0f);
    const Vector3 vec3x = Vector3(1.0f, 0.0f, 0.0f);
    const Vector3 vec3y = Vector3(0.0f, 1.0f, 0.0f);
    const Vector3 vec3z = Vector3(0.0f, 0.0f, 1.0f);
}

//
//直线
//
class Line
{
public:
    Line(){}

    Line(const Vector3 & s, const Vector3 & e)
        : start(s), end(e)
    {}

    float length() const
    {
        return start.distTo(end);
    }

    Vector3 direction() const
    {
        Vector3 dir = end - start;
        dir.normalize();
        return dir;
    }

    //线性插值
    Vector3 interpolation(float ratio) const
    {
        return end*ratio + start*(1.0f-ratio);
    }
    
    //线性插值
    Vector3 interpolation(float rs, float re) const
    {
        float down = rs + re;
        if(down == 0.0f)
        {
            return start;
        }
        down = 1.0f/down;
        //return interpolation(rs*down);
        return (end*rs + start*re)*down;
    }

    //点在直线上的投影
    Vector3 project(const Vector3 & point);

    bool intersect(Vector3 & cross, const Line & line);

public:
    Vector3 start;
    Vector3 end;
};

//
//三角形
//
class Triangle
{
public:
    Triangle(){}
    Triangle(const Vector3 & A, const Vector3 & B, const Vector3 & C)
        :a(A), b(B), c(C)
    {}

    Vector3 normal() const
    {
        Vector3 ab = b - a;
        Vector3 bc = c - b;
        Vector3 n = ab.cross(bc);
        n.normalize();
        return n;
    }

    Vector3 & operator[](int i)
    {
        if(i == 0) return a;
        if(i == 1) return b;
        if(i == 2) return c;
        throw std::out_of_range("Triangle index out of range");
    }
    const Vector3 & operator[](int i) const
    {
        if(i == 0) return a;
        if(i == 1) return b;
        if(i == 2) return c;
        throw std::out_of_range("Triangle index out of range");
    }

    bool isPointIn(const Vector3 & p) const;

    bool isLineIntersect(const Line & line, Vector3 * cross=NULL) const;

    bool isSphereIntersect(const Vector3 & center, float radius, Vector3 * cross=NULL) const;
    
    void applyMatrix(const Matrix4x4 & mat)
    {
        a.applyMatrix(mat);
        b.applyMatrix(mat);
        c.applyMatrix(mat);
    }

    bool intersect(const Triangle & triangle, const Vector3 & offset) const;

    bool intersectRay(const Vector3 & start, const Vector3 & dir, float *pDist=NULL) const;

public :
    Vector3 a, b, c;
};

//
//平面
//
class Plane
{
public:
    Plane()  : distance(0.0f)  {}

    Plane(const Vector3 & n, const Vector3 & p)
        : normal(n)
    {
        distance = normal.dot(p);
    }

    Plane(const Vector3 & n, float d) 
        : normal(n), distance(d) 
    {}

    Plane(const Triangle & tri) 
        : normal(tri.normal())
    { 
        distance = normal.dot(tri.a);
    }

    void set(const Vector3 & n, const Vector3 & p)   
    {
        normal = n;
        distance = normal.dot(p);
    }

    float distToPoint(const Vector3 & p) const
    {
        return normal.dot(p) - distance;
    }

    bool isIntersect(const Line & line, Vector3 * cross=NULL) const;

    bool isIntersect(const Vector3 & start, const Vector3 & end, float * pPos=NULL) const;

    bool isSphereIntersect(const Vector3 & center, float radius, Vector3 * cross=NULL);

    int witchSide(const Vector3 & pt) const
    {
        float dt = distToPoint(pt);
        if (amostZero(dt)) return SIDE_ON;
        if (dt > 0) return SIDE_FRONT;
        return SIDE_BACK;
    }

    int witchSide(const Triangle & tri)
    {
        int side = 0;
        side |= witchSide(tri.a);
        side |= witchSide(tri.b);
        side |= witchSide(tri.c);
        return side;
    }

    bool operator==(const Plane & p)
    {
        return normal == p.normal && distance == p.distance;
    }

public:
    Vector3 normal;
    float distance;
};

//
//多边形
//
typedef std::vector<Vector3> PolygonBase;

class Polygon : public PolygonBase
{
public:

    Polygon(): PolygonBase() {}

    Polygon(int size) : PolygonBase(size) {}

    Polygon(const Polygon& p) : PolygonBase(p) {}

    Polygon(const Triangle & tri)
    {
        *this = tri;
    }

    const Polygon & operator=(const Triangle & tri)
    {
        addTriangle(tri);
        return *this;
    }

    void genPanel(Plane & panel) const
    {
        assert(size() >= 3);

        const_iterator it = begin();
        const Vector3 &a = *it++;
        const Vector3 &b = *it++;
        const Vector3 &c = *it;
        Vector3 ab = b - a;
        Vector3 bc = c - b;
        Vector3 n = ab.cross(bc);
        n.normalize();
        panel.set(n, a);
    }

    void addTriangle(const Triangle & tri)
    {
        addPoint(tri.a);
        addPoint(tri.b);
        addPoint(tri.c);
    }

    void split(const Plane & panel, Polygon & frontPoly, Polygon & backPoly) const;

    void splitFront(const Plane & panel, Polygon & frontPoly) const;

    bool choiceFront(const Plane & panel);

    void addPoint(const Vector3 & p);

    void adjudge();

    float minDistToPanel(const Plane & panel);
};


class AABB
{
public:

    Vector3 min;
    Vector3 max;

    void zero()
    {
        min = MathConst::vec3Max;
        max = MathConst::vec3Min;
    }

    //正规化。aabb经过变换后，min可能要大于max。
    void normalization();

    void addPoint(const Vector3 & pt)
    {
        min.x = min2(min.x, pt.x);
        min.y = min2(min.y, pt.y);
        min.z = min2(min.z, pt.z);

        max.x = max2(max.x, pt.x);
        max.y = max2(max.y, pt.y);
        max.z = max2(max.z, pt.z);
    }

    void addTriangle(const Triangle & tri)
    {
        addPoint(tri.a);
        addPoint(tri.b);
        addPoint(tri.c);
    }

    void getCenter(Vector3 & center) const
    {
        center = (max + min) * 0.5f;
    }

    void getDiameter(Vector3 & diameter) const
    {
        diameter = max - min;
    }

    void sub(const AABB & aabb)
    {
        min.x = max2(min.x, aabb.min.x);
        min.y = max2(min.y, aabb.min.y);
        min.z = max2(min.z, aabb.min.z);

        max.x = min2(max.x, aabb.max.x);
        max.y = min2(max.y, aabb.max.y);
        max.z = min2(max.z, aabb.max.z);
    }

    void add(const AABB & aabb)
    {

    }

    bool intersectsRay(const Vector3 & origin, const Vector3 & dir ) const;

    bool intersect(const AABB & aabb) const;

    bool clipLine(Vector3 & start, Vector3 & end ) const;


    void applyMatrix(const Matrix4x4 & mat);
    void applyMatrix(AABB & out, const Matrix4x4 & mat) const;
};



typedef std::vector<int> TriangleIndices;
typedef std::vector<Triangle> TriangleSet;


class FRect
{
public:
    float left, top, right, bottom;

    FRect() : left(0.0f), top(0.0f), right(0.0f), bottom(0.0f)
    {
    }

    FRect(float l, float t, float r, float b)
        : left(l), top(t), right(r), bottom(b)
    {
    }

    float width() const { return right - left; }
    float height() const { return bottom - top; }

    bool isIn(float x_, float y_) const
    {
        if(x_ < left || x_ > right) return false;
        if(y_ < top || y_ > bottom) return false;
        return true;
    }

    void expand(const FRect & rc)
    {
        left = min2(left, rc.left);
        top = min2(top, rc.top);
        right = max2(right, rc.right);
        bottom = max2(bottom, rc.bottom);
    }

    void expand(float x, float y)
    {
        left = min2(left, x);
        top = min2(top, y);
        right = max2(right, x);
        bottom = max2(bottom, y);
    }

    void delta(float dx, float dy)
    {
        left -= dx;
        right += dx;
        top -= dy;
        bottom += dy;
    }

    ///正规化。aabb经过变换后，min可能要大于max。
    void normalization();

    ///两个区域是否相交
    bool isIntersect(const FRect & rc) const;

    ///三角形xz所在区域，是否与当前区域相交
    bool isTriangleXZIn(const Triangle & tri) const;
};





}//namespace Physics