root/trunk/mathlib.h

/*      mathlib
 *
 *                      written by Alexander Zaprjagaev
 *                      frustum@frustum.tomsk.ru
 *                      http://frustum.tomsk.ru
 */

#ifndef __MATHLIB_H__
#define __MATHLIB_H__

#include <math.h>

#define EPSILON 1e-6
#define PI 3.14159265358979323846f
#define DEG2RAD (PI / 180.0f)
#define RAD2DEG (180.0 / PI)

struct vec2;
struct vec3;
struct vec4;
struct mat3;
struct quat;

struct vec2 {
        
        vec2() : x(0), y(0) { }
        vec2(float x,float y) : x(x), y(y) { }
        vec2(const float *v) : x(v[0]), y(v[1]) { }
        vec2(const vec2 &v) : x(v.x), y(v.y) { }
        
        int operator==(const vec2 &v) { return (x == v.x && y == v.y); }
        int operator!=(const vec2 &v) { return (x != v.x || y != v.y); }
        
        const vec2 operator*(float f) const { return vec2(x * f,y * f); }
        const vec2 operator/(float f) const { return vec2(x / f,y / f); }
        const vec2 operator+(const vec2 &v) const { return vec2(x + v.x,y + v.y); }
        const vec2 operator-() const { return vec2(-x,-y); }
        const vec2 operator-(const vec2 &v) const { return vec2(x - v.x,y - v.y); }
        
        vec2 &operator*=(float f) { return *this = *this * f; }
        vec2 &operator/=(float f) { return *this = *this / f; }
        vec2 &operator+=(const vec2 &v) { return *this = *this + v; }
        vec2 &operator-=(const vec2 &v) { return *this = *this - v; }
        
        float &operator[](int i) {
                return ((float*)&x)[i];
        }
        
        const float operator[](int i) const {
                return ((float*)&x)[i];
        }
        
        float length() const { return (float)sqrt(x * x + y * y); }
        float normalize() {
                float inv,length = (float)sqrt(x * x + y * y);
                inv = 1.0f / length;
                x *= inv;
                y *= inv;
                return length;
        }
        float dot(const vec2 &v) { return (x * v.x + y * v.y); }
        
        union {
                struct {
                        float x,y;
                };
                float v[2];
        };
};

struct vec3 {
        
        vec3() : x(0), y(0), z(0) { }
        vec3(float x,float y,float z) : x(x), y(y), z(z) { }
        vec3(const float *v) : x(v[0]), y(v[1]), z(v[2]) { }
        vec3(const vec3 &v) : x(v.x), y(v.y), z(v.z) { }
        vec3(const vec4 &v);
        
        int operator==(const vec3 &v) { return (x == v.x && y == v.y && z == v.z); }
        int operator!=(const vec3 &v) { return (x != v.x || y != v.y || z != v.z); }
        
        const vec3 operator*(float f) const { return vec3(x * f,y * f,z * f); }
        const vec3 operator/(float f) const { return vec3(x / f,y / f,z / f); }
        const vec3 operator+(const vec3 &v) const { return vec3(x + v.x,y + v.y,z + v.z); }
        const vec3 operator-() const { return vec3(-x,-y,-z); }
        const vec3 operator-(const vec3 &v) const { return vec3(x - v.x,y - v.y,z - v.z); }
        
        vec3 &operator*=(float f) { return *this = *this * f; }
        vec3 &operator/=(float f) { return *this = *this / f; }
        vec3 &operator+=(const vec3 &v) { return *this = *this + v; }
        vec3 &operator-=(const vec3 &v) { return *this = *this - v; }
        
        float &operator[](int i) {
                return ((float*)&x)[i];
        }
        
        const float operator[](int i) const {
                return ((float*)&x)[i];
        }
        
        float length() const { return (float)sqrt(x * x + y * y + z * z); }
        float normalize() {
                float inv,length = (float)sqrt(x * x + y * y + z * z);
                inv = 1.0f / length;
                x *= inv;
                y *= inv;
                z *= inv;
                return length;
        }
        float dot(const vec3 &v) { return (x * v.x + y * v.y + z * v.z); }
        vec3 cross(const vec3 &v1,const vec3 &v2) {
                x = v1.y * v2.z - v1.z * v2.y;
                y = v1.z * v2.x - v1.x * v2.z;
                z = v1.x * v2.y - v1.y * v2.x;
                return *this;
        }
        
        union {
                struct {
                        float x,y,z;
                };
                float v[3];
        };
};

struct vec4 {
        
        vec4() : x(0), y(0), z(0), w(1) { }
        vec4(float x,float y,float z,float w) : x(x), y(y), z(z), w(w) { }
        vec4(const float *v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) { }
        vec4(const vec3 &v) : x(v.x), y(v.y), z(v.z), w(1) { }
        vec4(const vec3 &v,float w) : x(v.x), y(v.y), z(v.z), w(w) { }
        vec4(const vec4 &v) : x(v.x), y(v.y), z(v.z), w(v.w) { }
        
        int operator==(const vec4 &v) { return (x == v.x && y == v.y && z == v.z && w == v.w); }
        int operator!=(const vec4 &v) { return (x != v.x || y != v.y || z != v.z || w != v.w); }
        
        const vec4 operator*(float f) const { return vec4(x * f,y * f,z * f,w * f); }
        const vec4 operator/(float f) const { return vec4(x / f,y / f,z / f,w / f); }
        const vec4 operator+(const vec4 &v) const { return vec4(x + v.x,y + v.y,z + v.z, w + v.w); }
        const vec4 operator-() const { return vec4(-x,-y,-z,-w); }
        const vec4 operator-(const vec4 &v) const { return vec4(x - v.x,y - v.y,z - v.z, z - v.w); }
        
        vec4 &operator*=(float f) { return *this = *this * f; }
        vec4 &operator/=(float f) { return *this = *this / f; }
        vec4 &operator+=(const vec4 &v) { return *this = *this + v; }
        vec4 &operator-=(const vec4 &v) { return *this = *this - v; }
        
        float &operator[](int i) {
                return ((float*)&x)[i];
        }
        
        const float operator[](int i) const {
                return ((float*)&x)[i];
        }
        
        float dot(const vec4 &v) const { return (x * v.x + y * v.y + z * v.z + w * v.w); }
        
        union {
                struct {
                        float x,y,z,w;
                };
                float v[4];
        };
};

inline vec3::vec3(const vec4 &v) {
        x = v.x;
        y = v.y;
        z = v.z;
}

struct mat4 {
        
        mat4() {
                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
                mat[1] = 1.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        mat4(const float *m) {
                mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
                mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
                mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
                mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
        }
        mat4(const mat4 &m) {
                mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
                mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
                mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
                mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
        }
        
        vec3 operator*(const vec3 &v) {
                vec3 ret;
                ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12];
                ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13];
                ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14];
                return ret;
        }
        mat4 operator*(const float f) {
                mat4 ret;
                ret[0] = mat[0] * f; ret[4] = mat[4] * f; ret[8] = mat[8] * f; ret[12] = mat[12] * f;
                ret[1] = mat[1] * f; ret[5] = mat[5] * f; ret[9] = mat[9] * f; ret[13] = mat[13] * f;
                ret[2] = mat[2] * f; ret[6] = mat[6] * f; ret[10] = mat[10] * f; ret[14] = mat[14] * f;
                ret[3] = mat[3] * f; ret[7] = mat[7] * f; ret[11] = mat[11] * f; ret[15] = mat[15] * f;
                return ret;
        }
        mat4 operator*(const mat4 &m) {
                mat4 ret;
                ret[0] = mat[0] * m[0] + mat[4] * m[1] + mat[8] * m[2] + mat[12] * m[3];
                ret[1] = mat[1] * m[0] + mat[5] * m[1] + mat[9] * m[2] + mat[13] * m[3];
                ret[2] = mat[2] * m[0] + mat[6] * m[1] + mat[10] * m[2] + mat[14] * m[3];
                ret[3] = mat[3] * m[0] + mat[7] * m[1] + mat[11] * m[2] + mat[15] * m[3];
                ret[4] = mat[0] * m[4] + mat[4] * m[5] + mat[8] * m[6] + mat[12] * m[7];
                ret[5] = mat[1] * m[4] + mat[5] * m[5] + mat[9] * m[6] + mat[13] * m[7];
                ret[6] = mat[2] * m[4] + mat[6] * m[5] + mat[10] * m[6] + mat[14] * m[7];
                ret[7] = mat[3] * m[4] + mat[7] * m[5] + mat[11] * m[6] + mat[15] * m[7];
                ret[8] = mat[0] * m[8] + mat[4] * m[9] + mat[8] * m[10] + mat[12] * m[11];
                ret[9] = mat[1] * m[8] + mat[5] * m[9] + mat[9] * m[10] + mat[13] * m[11];
                ret[10] = mat[2] * m[8] + mat[6] * m[9] + mat[10] * m[10] + mat[14] * m[11];
                ret[11] = mat[3] * m[8] + mat[7] * m[9] + mat[11] * m[10] + mat[15] * m[11];
                ret[12] = mat[0] * m[12] + mat[4] * m[13] + mat[8] * m[14] + mat[12] * m[15];
                ret[13] = mat[1] * m[12] + mat[5] * m[13] + mat[9] * m[14] + mat[13] * m[15];
                ret[14] = mat[2] * m[12] + mat[6] * m[13] + mat[10] * m[14] + mat[14] * m[15];
                ret[15] = mat[3] * m[12] + mat[7] * m[13] + mat[11] * m[14] + mat[15] * m[15];
                return ret;
        }
        mat4 operator+(const mat4 &m) {
                mat4 ret;
                ret[0] = mat[0] + m[0]; ret[4] = mat[4] + m[4]; ret[8] = mat[8] + m[8]; ret[12] = mat[12] + m[12];
                ret[1] = mat[1] + m[1]; ret[5] = mat[5] + m[5]; ret[9] = mat[9] + m[9]; ret[13] = mat[13] + m[13];
                ret[2] = mat[2] + m[2]; ret[6] = mat[6] + m[6]; ret[10] = mat[10] + m[10]; ret[14] = mat[14] + m[14];
                ret[3] = mat[3] + m[3]; ret[7] = mat[7] + m[7]; ret[11] = mat[11] + m[11]; ret[15] = mat[15] + m[15];
                return ret;
        }
        mat4 operator-(const mat4 &m) {
                mat4 ret;
                ret[0] = mat[0] - m[0]; ret[4] = mat[4] - m[4]; ret[8] = mat[8] - m[8]; ret[12] = mat[12] - m[12];
                ret[1] = mat[1] - m[1]; ret[5] = mat[5] - m[5]; ret[9] = mat[9] - m[9]; ret[13] = mat[13] - m[13];
                ret[2] = mat[2] - m[2]; ret[6] = mat[6] - m[6]; ret[10] = mat[10] - m[10]; ret[14] = mat[14] - m[14];
                ret[3] = mat[3] - m[3]; ret[7] = mat[7] - m[7]; ret[11] = mat[11] - m[11]; ret[15] = mat[15] - m[15];
                return ret;
        }
        
        mat4 &operator*=(const float f) { return *this = *this * f; }
        mat4 &operator*=(const mat4 &m) { return *this = *this * m; }
        mat4 &operator+=(const mat4 &m) { return *this = *this + m; }
        mat4 &operator-=(const mat4 &m) { return *this = *this - m; }
        
        float &operator[](int i) {
                return mat[i];
        }
        const float operator[](int i) const {
                return mat[i];
        }
        
        mat4 zero() {
                mat4 ret;
                ret[0] = 0.0; ret[4] = 0.0; ret[8] = 0.0; ret[12] = 0.0;
                ret[1] = 0.0; ret[5] = 0.0; ret[9] = 0.0; ret[13] = 0.0;
                ret[2] = 0.0; ret[6] = 0.0; ret[10] = 0.0; ret[14] = 0.0;
                ret[3] = 0.0; ret[7] = 0.0; ret[11] = 0.0; ret[15] = 0.0;
                return ret;
        }
        mat4 identity() {
                mat4 ret;
                ret[0] = 1.0; ret[4] = 0.0; ret[8] = 0.0; ret[12] = 0.0;
                ret[1] = 0.0; ret[5] = 1.0; ret[9] = 0.0; ret[13] = 0.0;
                ret[2] = 0.0; ret[6] = 0.0; ret[10] = 1.0; ret[14] = 0.0;
                ret[3] = 0.0; ret[7] = 0.0; ret[11] = 0.0; ret[15] = 1.0;
                return ret;
        }
        mat4 rotation() {
                mat4 ret;
                ret[0] = mat[0]; ret[4] = mat[4]; ret[8] = mat[8]; ret[12] = 0;
                ret[1] = mat[1]; ret[5] = mat[5]; ret[9] = mat[9]; ret[13] = 0;
                ret[2] = mat[2]; ret[6] = mat[6]; ret[10] = mat[10]; ret[14] = 0;
                ret[3] = 0; ret[7] = 0; ret[11] = 0; ret[15] = 1;
                return ret;
        }
        mat4 transpose() {
                mat4 ret;
                ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[3];
                ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[7];
                ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[11];
                ret[3] = mat[12]; ret[7] = mat[13]; ret[11] = mat[14]; ret[15] = mat[15];
                return ret;
        }
        mat4 inverse() {
                mat4 ret;
                float det;
                det = mat[0] * mat[5] * mat[10];
                det += mat[4] * mat[9] * mat[2];
                det += mat[8] * mat[1] * mat[6];
                det -= mat[8] * mat[5] * mat[2];
                det -= mat[4] * mat[1] * mat[10];
                det -= mat[0] * mat[9] * mat[6];
                det = 1.0f / det;
                ret[0] =  (mat[5] * mat[10] - mat[9] * mat[6]) * det;
                ret[1] = -(mat[1] * mat[10] - mat[9] * mat[2]) * det;
                ret[2] =  (mat[1] * mat[6] - mat[5] * mat[2]) * det;
                ret[3] = 0.0;
                ret[4] = -(mat[4] * mat[10] - mat[8] * mat[6]) * det;
                ret[5] =  (mat[0] * mat[10] - mat[8] * mat[2]) * det;
                ret[6] = -(mat[0] * mat[6] - mat[4] * mat[2]) * det;
                ret[7] = 0.0;
                ret[8] =  (mat[4] * mat[9] - mat[8] * mat[5]) * det;
                ret[9] = -(mat[0] * mat[9] - mat[8] * mat[1]) * det;
                ret[10] =  (mat[0] * mat[5] - mat[4] * mat[1]) * det;
                ret[11] = 0.0;
                ret[12] = -(mat[12] * ret[0] + mat[13] * ret[4] + mat[14] * ret[8]);
                ret[13] = -(mat[12] * ret[1] + mat[13] * ret[5] + mat[14] * ret[9]);
                ret[14] = -(mat[12] * ret[2] + mat[13] * ret[6] + mat[14] * ret[10]);
                ret[15] = 1.0;
                return ret;
        }
        
        void rotation_x(const float angle) {
                float rad = angle * DEG2RAD;
                float c = (float)cos(rad);
                float s = (float)sin(rad);
                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
                mat[1] = 0.0; mat[5] = c; mat[9] = -s; mat[13] = 0.0;
                mat[2] = 0.0; mat[6] = s; mat[10] = c; mat[14] = 0.0;
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        void rotation_y(const float angle) {
                float rad = angle * DEG2RAD;
                float c = (float)cos(rad);
                float s = (float)sin(rad);
                mat[0] = c; mat[4] = 0.0; mat[8] = s; mat[12] = 0.0;
                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
                mat[2] = -s; mat[6] = 0.0; mat[10] = c; mat[14] = 0.0;
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        void rotation_z(const float angle) {
                float rad = angle * DEG2RAD;
                float c = (float)cos(rad);
                float s = (float)sin(rad);
                mat[0] = c; mat[4] = -s; mat[8] = 0.0; mat[12] = 0.0;
                mat[1] = s; mat[5] = c; mat[9] = 0.0; mat[13] = 0.0;
                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        void translate(const vec3 &v) {
                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = v[0];
                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = v[1];
                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = v[2];
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        void translate(const float x,const float y,const float z) {
                translate(vec3(x,y,z));
        }
        void scale(const vec3 &v) {
                mat[0] = v[0]; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
                mat[1] = 0.0; mat[5] = v[1]; mat[9] = 0.0; mat[13] = 0.0;
                mat[2] = 0.0; mat[6] = 0.0; mat[10] = v[2]; mat[14] = 0.0;
                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
        }
        void scale(const float x,const float y,const float z) {
                scale(vec3(x,y,z));
        }
        
        void lock_at(const vec3 &eye,const vec3 &dir,const vec3 &up) {
                vec3 x,y,z;
                mat4 m0,m1;
                z = eye - dir;
                z.normalize();
                x.cross(up,z);
                x.normalize();
                y.cross(z,x);
                y.normalize();
                m0[0] = x[0]; m0[4] = x[1]; m0[8] = x[2]; m0[12] = 0.0;
                m0[1] = y[0]; m0[5] = y[1]; m0[9] = y[2]; m0[13] = 0.0;
                m0[2] = z[0]; m0[6] = z[1]; m0[10] = z[2]; m0[14] = 0.0;
                m0[3] = 0.0; m0[7] = 0.0; m0[11] = 0.0; m0[15] = 1.0;
                m1.translate(-eye);
                *this = m0 * m1;
        }
        void look_at(const float *eye,const float *dir,const float *up) {
                lock_at(vec3(eye),vec3(dir),vec3(up));
        }
        
        float mat[16];
};

struct quat {
        
        quat() : x(0), y(0), z(0), w(1) { }
        quat(const vec3 &dir,const float angle) {
                set(dir,angle);
        }
        
        float &operator[](int i) {
                return ((float*)&x)[i];
        }
        const float operator[](int i) const {
                return ((float*)&x)[i];
        }
        
        quat operator*(const quat &q) {
                quat ret;
                ret.x = w * q.x + x * q.x + y * q.z - z * q.y;
                ret.y = w * q.y + y * q.w + z * q.x - x * q.z;
                ret.z = w * q.z + z * q.w + x * q.y - y * q.x;
                ret.w = w * q.w - x * q.x - y * q.y - z * q.z;
                return ret;
        }
        
        void set(const vec3 &dir,const float angle) {
                float length = dir.length();
                if(length != 0.0) {
                        length = 1.0f / length;
                        float sinangle = (float)sin(angle * DEG2RAD / 2.0f);
                        x = dir[0] * length * sinangle;
                        y = dir[1] * length * sinangle;
                        z = dir[2] * length * sinangle;
                        w = (float)cos(angle * DEG2RAD / 2.0);
                } else {
                        x = y = z = 0.0;
                        w = 1.0;
                }
        }
        void set(const float x,const float y,const float z,const float angle) {
                set(vec3(x,y,z),angle);
        }
        
        quat slerp(const quat &q0,const quat &q1,const float t) {
                float k0,k1,cosomega = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
                quat q,ret;
                if(cosomega < 0.0) {
                        cosomega = -cosomega;
                        q.x = -q1.x;
                        q.y = -q1.y;
                        q.z = -q1.z;
                        q.w = -q1.w;
                } else {
                        q.x = q1.x;
                        q.y = q1.y;
                        q.z = q1.z;
                        q.w = q1.w;
                }
                if(1.0 - cosomega > 1e-6) {
                        float omega = (float)acos(cosomega);
                        float sinomega = (float)sin(omega);
                        k0 = (float)sin((1.0 - t) * omega) / sinomega;
                        k1 = (float)sin(t * omega) / sinomega;
                } else {
                        k0 = 1.0f - t;
                        k1 = t;
                }
                ret.x = q0.x * k0 + q.x * k1;
                ret.y = q0.y * k0 + q.y * k1;
                ret.z = q0.z * k0 + q.z * k1;
                ret.w = q0.w * k0 + q.w * k1;
                return ret;
        }
        
        mat4 to_matrix() {
                mat4 ret;
                float x2,y2,z2,xx,yy,zz,xy,yz,xz,wx,wy,wz;
                x2 = x + x;
                y2 = y + y;
                z2 = z + z;
                xx = x * x2;
                yy = y * y2;
                zz = z * z2;
                xy = x * y2;
                yz = y * z2;
                xz = z * x2;
                wx = w * x2;
                wy = w * y2;
                wz = w * z2;
                ret[0] = 1.0f - (yy + zz); ret[4] = xy - wz; ret[8] = xz + wy; ret[12] = 0.0f;
                ret[1] = xy + wz; ret[5] = 1.0f - (xx + zz); ret[9] = yz - wx; ret[13] = 0.0f;
                ret[2] = xz - wy; ret[6] = yz + wx; ret[10] = 1.0f - (xx + yy); ret[14] = 0.0f;
                ret[3] = 0.0f; ret[7] = 0.0f; ret[11] = 0.0f; ret[15] = 1.0f;
                return ret;
        }
        
        union {
                struct {
                        float x,y,z,w;
                };
                float q[4];
        };
};

#endif /* __MATHLIB_H__ */