#ifndef __UTIL3D_H__
#define __UTIL3D_H__

#include <math.h>

/* axes/coordinates */
enum Axis
{
    X = 0,
    Y,
    Z,
    W
};

/* pi/radians */
#define PI              3.14159265359f
#define TWO_PI          6.28318530718f    // 2*PI
#define HALF_PI         1.570796326795f   // PI/2
#define RAD_PER_DEG     0.01745329251994f // PI/180
#define DEG_PER_RAD     57.29577951308f   // 180/PI

/* most significant bit */
#define MSB32 0x80000000
#define MSB16 0x8000
#define MSB8  0x80

/* for random numbers */
#define RAND_MAX_PLUS_ONE_INV  1.0f/(float)(RAND_MAX+1)

/* types for different size integers */
typedef char              Int8;
typedef unsigned char    UInt8;
typedef short             Int16;
typedef unsigned short   UInt16;
typedef int               Int32;
typedef unsigned int     UInt32;

/* points and matrices */
typedef float Vector[4];
typedef float Vector3D[3];
typedef float Vector2D[2];
typedef float Matrix[4][4];

#define ABS(a)  ((a) >= 0 ? (a) : (-(a)))
#define MIN(a, b)  ((a) < (b) ? (a) : (b))
#define MAX(a, b)  ((a) > (b) ? (a) : (b))
#define BOUND(a, min, max)  ( ((a) < (min)) ? (min) : ( ((a) > (max)) ? (max) : (a) ) )

/* moves a variable var towards to, changing it by an amount delta every time */
#define MOVE_TO(var, to, delta)  (var) = (((var) < (to)) ? MIN((to), (var)+(delta)) : MAX((to), (var)-(delta)))
#define MOVE_TO_ZERO(var, delta)  MOVE_TO((var), 0, (delta))

/* returns a random number in the range [x, y)
   i.e. between x and y, including x, but not y */
#define RANDOM_INT(x, y)  (MIN((x), (y)) + (UInt32)(ABS((x) - (y))*rand()*RAND_MAX_PLUS_ONE_INV))
#define RANDOM_FLOAT(x, y)  ((float)MIN((x), (y)) + ((float)ABS((x) - (y))*(float)rand()*RAND_MAX_PLUS_ONE_INV))

/* is number even or odd? */
#define IS_ODD(x)  (((x) & 0x01) != 0x0)
#define IS_EVEN(x) (((x) & 0x01) == 0x0)

#define FLOOR(a)  ((a) > 0 ? (Int32)(a) : -(Int32)(-a))
#define CEILING(a)  ((a) == (Int32)(a) ? (a) : (a) > 0 ? 1 + (Int32)(a) : -(1 + (Int32)(-a)))

#define ROUND(a)  ((a) > 0 ? (Int32)(a + 0.5f) : -(Int32)(0.5f - a))

#define SWAP_INT(a, b)  ((a)^=(b)^=(a)^=(b))

#define DEG_TO_RAD(deg) ((deg)*RAD_PER_DEG)
#define RAD_TO_DEG(rad) ((rad)*DEG_PER_RAD)

inline Int32 SQR(Int32 i)
{
    return (i*i);
}

inline float SQR(float f)
{
    return (f*f);
}

inline void SET_VECTOR_4(Vector v, float x, float y, float z, float w)
{
    v[0] = x; v[1] = y; v[2] = z; v[3] = w;
}

inline void SET_VECTOR_3(Vector v, float x, float y, float z)
{
    v[0] = x; v[1] = y; v[2] = z; v[3] = 1.0f;
}

inline void SET_VECTOR3D(Vector3D v, float x, float y, float z)
{
    v[0] = x; v[1] = y; v[2] = z;
}

inline void SET_VECTOR2D(Vector2D v, float x, float y)
{
    v[0] = x; v[1] = y;
}

inline void COPY_VECTOR(Vector v, Vector v0)
{
    v[0] = v0[0]; v[1] = v0[1]; v[2] = v0[2]; v[3] = v0[3];
}

inline void COPY_VECTOR3D(Vector3D v, Vector3D v0)
{
    v[0] = v0[0]; v[1] = v0[1]; v[2] = v0[2];
}

inline void COPY_VECTOR2D(Vector2D v, Vector2D v0)
{
    v[0] = v0[0]; v[1] = v0[1];
}

inline void SWAP_VECTOR(Vector v0, Vector v1)
{
    Vector tmp;
    COPY_VECTOR(tmp, v0);
    COPY_VECTOR(v0, v1);
    COPY_VECTOR(v1, tmp);
}

inline void SWAP_VECTOR3D(Vector3D v0, Vector3D v1)
{
    Vector3D tmp;
    COPY_VECTOR3D(tmp, v0);
    COPY_VECTOR3D(v0, v1);
    COPY_VECTOR3D(v1, tmp);
}

inline void SWAP_VECTOR2D(Vector2D v0, Vector2D v1)
{
    Vector2D tmp;
    COPY_VECTOR2D(tmp, v0);
    COPY_VECTOR2D(v0, v1);
    COPY_VECTOR2D(v1, tmp);
}

inline void COPY_MATRIX(Matrix m, Matrix m0)
{
    m[0][0] = m0[0][0]; m[0][1] = m0[0][1]; 
    m[0][2] = m0[0][2]; m[0][3] = m0[0][3];
    m[1][0] = m0[1][0]; m[1][1] = m0[1][1]; 
    m[1][2] = m0[1][2]; m[1][3] = m0[1][3];
    m[2][0] = m0[2][0]; m[2][1] = m0[2][1]; 
    m[2][2] = m0[2][2]; m[2][3] = m0[2][3];
    m[3][0] = m0[3][0]; m[3][1] = m0[3][1]; 
    m[3][2] = m0[3][2]; m[3][3] = m0[3][3];
}

inline void VECTOR_ADD(Vector v, Vector v1, Vector v2)
{
    v[X] = v1[X] + v2[X];
    v[Y] = v1[Y] + v2[Y];
    v[Z] = v1[Z] + v2[Z];
    v[W] = v1[W];
}

inline void VECTOR_SUB(Vector v, Vector v1, Vector v2)
{
    v[X] = v1[X] - v2[X];
    v[Y] = v1[Y] - v2[Y];
    v[Z] = v1[Z] - v2[Z];
    v[W] = v1[W];
}

inline void VECTOR3D_ADD(Vector3D v, Vector3D v1, Vector3D v2)
{
    v[X] = v1[X] + v2[X];
    v[Y] = v1[Y] + v2[Y];
    v[Z] = v1[Z] + v2[Z];
}

inline void VECTOR3D_SUB(Vector3D v, Vector3D v1, Vector3D v2)
{
    v[X] = v1[X] - v2[X];
    v[Y] = v1[Y] - v2[Y];
    v[Z] = v1[Z] - v2[Z];
}

inline void VECTOR2D_ADD(Vector2D v, Vector2D v1, Vector2D v2)
{
    v[X] = v1[X] + v2[X];
    v[Y] = v1[Y] + v2[Y];
}

inline void VECTOR2D_SUB(Vector2D v, Vector2D v1, Vector2D v2)
{
    v[X] = v1[X] - v2[X];
    v[Y] = v1[Y] - v2[Y];
}

inline void VECTOR_MULT_SCALAR(Vector v, float s, Vector v1)
{
    v[X] = s*v1[X];
    v[Y] = s*v1[Y];
    v[Z] = s*v1[Z];
    v[W] = v1[W];
}

inline void VECTOR3D_MULT_SCALAR(Vector3D v, float s, Vector3D v1)
{
    v[X] = s*v1[X];
    v[Y] = s*v1[Y];
    v[Z] = s*v1[Z];
}

inline void VECTOR2D_MULT_SCALAR(Vector2D v, float s, Vector2D v1)
{
    v[X] = s*v1[X];
    v[Y] = s*v1[Y];
}

inline void VECTOR_AVG(Vector res, Vector v1, Vector v2)
{
    VECTOR_ADD(res, v1, v2);
    VECTOR_MULT_SCALAR(res, 0.5f, res);
}

inline void VECTOR3D_AVG(Vector3D res, Vector3D v1, Vector3D v2)
{
    VECTOR3D_ADD(res, v1, v2);
    VECTOR3D_MULT_SCALAR(res, 0.5f, res);
}

inline void VECTOR2D_AVG(Vector2D res, Vector2D v1, Vector2D v2)
{
    VECTOR2D_ADD(res, v1, v2);
    VECTOR2D_MULT_SCALAR(res, 0.5f, res);
}

inline float DOT_PRODUCT(Vector v1, Vector v2)
{
    return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
}

inline float DOT_PRODUCT3D(Vector3D v1, Vector3D v2)
{
    return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
}

inline float DOT_PRODUCT2D(Vector2D v1, Vector2D v2)
{
    return (v1[X]*v2[X] + v1[Y]*v2[Y]);
}

inline void CROSS_PRODUCT(Vector v, Vector v1, Vector v2)
{
    Vector tmp;
    tmp[X] = v1[Y]*v2[Z] - v1[Z]*v2[Y];
    tmp[Y] = v1[Z]*v2[X] - v1[X]*v2[Z];
    tmp[Z] = v1[X]*v2[Y] - v1[Y]*v2[X];
    tmp[W] = 0.0f;
    COPY_VECTOR(v, tmp);
}

inline void CROSS_PRODUCT_QUICK(Vector v, Vector v1, Vector v2)
{
    v[X] = v1[Y]*v2[Z] - v1[Z]*v2[Y];
    v[Y] = v1[Z]*v2[X] - v1[X]*v2[Z];
    v[Z] = v1[X]*v2[Y] - v1[Y]*v2[X];
    v[W] = 0.0f;
}

inline void CROSS_PRODUCT_TRI(Vector norm, Vector pt0, Vector pt1, Vector pt2)
{
    norm[X] = (pt0[Y]-pt1[Y])*(pt2[Z]-pt1[Z]) - (pt0[Z]-pt1[Z])*(pt2[Y]-pt1[Y]);
    norm[Y] = (pt0[Z]-pt1[Z])*(pt2[X]-pt1[X]) - (pt0[X]-pt1[X])*(pt2[Z]-pt1[Z]);
    norm[Z] = (pt0[X]-pt1[X])*(pt2[Y]-pt1[Y]) - (pt0[Y]-pt1[Y])*(pt2[X]-pt1[X]);
    norm[W] = 0.0f;
}

inline void CROSS_PRODUCT3D(Vector3D v, Vector3D v1, Vector3D v2)
{
    Vector tmp;
    tmp[X] = v1[Y]*v2[Z] - v1[Z]*v2[Y];
    tmp[Y] = v1[Z]*v2[X] - v1[X]*v2[Z];
    tmp[Z] = v1[X]*v2[Y] - v1[Y]*v2[X];
    COPY_VECTOR3D(v, tmp);
}

inline void CROSS_PRODUCT3D_QUICK(Vector3D v, Vector3D v1, Vector3D v2)
{
    v[X] = v1[Y]*v2[Z] - v1[Z]*v2[Y];
    v[Y] = v1[Z]*v2[X] - v1[X]*v2[Z];
    v[Z] = v1[X]*v2[Y] - v1[Y]*v2[X];
}

inline void CROSS_PRODUCT3D_TRI(Vector3D norm, Vector3D pt0, Vector3D pt1, 
                                Vector3D pt2)
{
    norm[X] = (pt0[Y]-pt1[Y])*(pt2[Z]-pt1[Z]) - (pt0[Z]-pt1[Z])*(pt2[Y]-pt1[Y]);
    norm[Y] = (pt0[Z]-pt1[Z])*(pt2[X]-pt1[X]) - (pt0[X]-pt1[X])*(pt2[Z]-pt1[Z]);
    norm[Z] = (pt0[X]-pt1[X])*(pt2[Y]-pt1[Y]) - (pt0[Y]-pt1[Y])*(pt2[X]-pt1[X]);
}

/* computes the Z component of the cross product
   of the vectors (v0-v1) * (v2-v1) */
inline float NORM_Z(Vector v0, Vector v1, Vector v2)
{
    return ((v2[X]-v0[X])*(v2[Y]-v1[Y]) - (v2[Y]-v0[Y])*(v2[X]-v1[X]));
}

inline void HOMOGENIZE(Vector v)
{
    float w_inv = 1.0f/v[W];
    v[X] *= w_inv;
    v[Y] *= w_inv;
    v[Z] *= w_inv;
}

inline void HOMOGENIZE2D(Vector v)
{
    float w_inv = 1.0f/v[W];
    v[X] *= w_inv;
    v[Y] *= w_inv;
}

inline float MAGNITUDE(Vector v)
{
    return (float)sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
}

inline float MAGNITUDE3D(Vector3D v)
{
    return (float)sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
}

inline float MAGNITUDE2D(Vector2D v)
{
    return (float)sqrt(v[X]*v[X] + v[Y]*v[Y]);
}

inline float DISTANCE_SQUARED(Vector v1, Vector v2)
{
    return (SQR(v1[X]-v2[X]) + SQR(v1[Y]-v2[Y]) + SQR(v1[Z]-v2[Z]));
}

inline float DISTANCE3D_SQUARED(Vector3D v1, Vector3D v2)
{
    return (SQR(v1[X]-v2[X]) + SQR(v1[Y]-v2[Y]) + SQR(v1[Z]-v2[Z]));
}

inline float DISTANCE2D_SQUARED(Vector2D v1, Vector2D v2)
{
    return (SQR(v1[X]-v2[X]) + SQR(v1[Y]-v2[Y]));
}

inline float DISTANCE(Vector v1, Vector v2)
{
    return (float)sqrt(DISTANCE_SQUARED(v1, v2));
}

inline float DISTANCE3D(Vector3D v1, Vector3D v2)
{
    return (float)sqrt(DISTANCE3D_SQUARED(v1, v2));
}

inline float DISTANCE2D(Vector2D v1, Vector2D v2)
{
    return (float)sqrt(DISTANCE2D_SQUARED(v1, v2));
}

inline void NORMALIZE(Vector v)
{
    float m = 1.0f/MAGNITUDE(v);
    v[X] *= m;
    v[Y] *= m;
    v[Z] *= m;
}

inline void NORMALIZE3D(Vector3D v)
{
    float m = 1.0f/MAGNITUDE3D(v);
    v[X] *= m;
    v[Y] *= m;
    v[Z] *= m;
}

inline void NORMALIZE2D(Vector2D v)
{
    float m = 1.0f/MAGNITUDE2D(v);
    v[X] *= m;
    v[Y] *= m;
}

/* puts the transpose of m in res */
inline void TRANSPOSE(Matrix res, Matrix m)
{
    Matrix tmp;
    tmp[0][0] = m[0][0]; tmp[0][1] = m[1][0]; 
    tmp[0][2] = m[2][0]; tmp[0][3] = m[3][0];
    tmp[1][0] = m[0][1]; tmp[1][1] = m[1][1]; 
    tmp[1][2] = m[2][1]; tmp[1][3] = m[3][1];
    tmp[2][0] = m[0][2]; tmp[2][1] = m[1][2]; 
    tmp[2][2] = m[2][2]; tmp[2][3] = m[3][2];
    tmp[3][0] = m[0][3]; tmp[3][1] = m[1][3]; 
    tmp[3][2] = m[2][3]; tmp[3][3] = m[3][3];
    COPY_MATRIX(res, tmp);
}

/* puts the transpose of m in res; res and m must be different */
inline void TRANSPOSE_QUICK(Matrix res, Matrix m)
{
    res[0][0] = m[0][0]; res[0][1] = m[1][0]; 
    res[0][2] = m[2][0]; res[0][3] = m[3][0];
    res[1][0] = m[0][1]; res[1][1] = m[1][1]; 
    res[1][2] = m[2][1]; res[1][3] = m[3][1];
    res[2][0] = m[0][2]; res[2][1] = m[1][2]; 
    res[2][2] = m[2][2]; res[2][3] = m[3][2];
    res[3][0] = m[0][3]; res[3][1] = m[1][3]; 
    res[3][2] = m[2][3]; res[3][3] = m[3][3];
}

inline void MAT_MULT_MAT_4x4(Matrix res, Matrix m1, Matrix m2)
{
    Matrix tmp;
    tmp[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0] + 
                m1[0][3]*m2[3][0];
    tmp[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1] + 
                m1[0][3]*m2[3][1];
    tmp[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2] + 
                m1[0][3]*m2[3][2];
    tmp[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + 
                m1[0][3]*m2[3][3];
    tmp[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0] + 
                m1[1][3]*m2[3][0];
    tmp[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1] + 
                m1[1][3]*m2[3][1];
    tmp[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2] + 
                m1[1][3]*m2[3][2];
    tmp[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + 
                m1[1][3]*m2[3][3];
    tmp[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0] + 
                m1[2][3]*m2[3][0];
    tmp[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1] + 
                m1[2][3]*m2[3][1];
    tmp[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2] + 
                m1[2][3]*m2[3][2];
    tmp[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + 
                m1[2][3]*m2[3][3];
    tmp[3][0] = m1[3][0]*m2[0][0] + m1[3][1]*m2[1][0] + m1[3][2]*m2[2][0] + 
                m1[3][3]*m2[3][0];
    tmp[3][1] = m1[3][0]*m2[0][1] + m1[3][1]*m2[1][1] + m1[3][2]*m2[2][1] + 
                m1[3][3]*m2[3][1];
    tmp[3][2] = m1[3][0]*m2[0][2] + m1[3][1]*m2[1][2] + m1[3][2]*m2[2][2] + 
                m1[3][3]*m2[3][2];
    tmp[3][3] = m1[3][0]*m2[0][3] + m1[3][1]*m2[1][3] + m1[3][2]*m2[2][3] + 
                m1[3][3]*m2[3][3];
    COPY_MATRIX(res, tmp);
}

/* assumes res is not the same as m1 or m2 */
inline void MAT_MULT_MAT_4x4_QUICK(Matrix res, Matrix m1, Matrix m2)
{
    res[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0] + 
                m1[0][3]*m2[3][0];
    res[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1] + 
                m1[0][3]*m2[3][1];
    res[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2] + 
                m1[0][3]*m2[3][2];
    res[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + 
                m1[0][3]*m2[3][3];
    res[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0] + 
                m1[1][3]*m2[3][0];
    res[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1] + 
                m1[1][3]*m2[3][1];
    res[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2] + 
                m1[1][3]*m2[3][2];
    res[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + 
                m1[1][3]*m2[3][3];
    res[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0] + 
                m1[2][3]*m2[3][0];
    res[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1] + 
                m1[2][3]*m2[3][1];
    res[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2] + 
                m1[2][3]*m2[3][2];
    res[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + 
                m1[2][3]*m2[3][3];
    res[3][0] = m1[3][0]*m2[0][0] + m1[3][1]*m2[1][0] + m1[3][2]*m2[2][0] + 
                m1[3][3]*m2[3][0];
    res[3][1] = m1[3][0]*m2[0][1] + m1[3][1]*m2[1][1] + m1[3][2]*m2[2][1] + 
                m1[3][3]*m2[3][1];
    res[3][2] = m1[3][0]*m2[0][2] + m1[3][1]*m2[1][2] + m1[3][2]*m2[2][2] + 
                m1[3][3]*m2[3][2];
    res[3][3] = m1[3][0]*m2[0][3] + m1[3][1]*m2[1][3] + m1[3][2]*m2[2][3] + 
                m1[3][3]*m2[3][3];
}

/* ignores the last rows of the matrices */
inline void MAT_MULT_MAT_3x4(Matrix res, Matrix m1, Matrix m2)
{
    Matrix tmp;
    tmp[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
    tmp[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
    tmp[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
    tmp[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + 
                m1[0][3];
    tmp[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
    tmp[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
    tmp[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
    tmp[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + 
                m1[1][3];
    tmp[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
    tmp[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
    tmp[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
    tmp[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + 
                m1[2][3];
    tmp[3][0] = 0.0f;
    tmp[3][1] = 0.0f;
    tmp[3][2] = 0.0f;
    tmp[3][3] = 1.0f;
    COPY_MATRIX(res, tmp);
}

/* assumes res is not the same as m1 or m2
   ignores the last rows of the matrices */
inline void MAT_MULT_MAT_3x4_QUICK(Matrix res, Matrix m1, Matrix m2)
{
    res[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
    res[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
    res[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
    res[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + 
                m1[0][3];
    res[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
    res[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
    res[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
    res[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + 
                m1[1][3];
    res[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
    res[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
    res[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
    res[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + 
                m1[2][3];
    res[3][0] = 0.0f;
    res[3][1] = 0.0f;
    res[3][2] = 0.0f;
    res[3][3] = 1.0f;
}

inline void MAT_MULT_VEC_4x4_4(Vector res, Matrix m1, Vector v2)
{
    Vector tmp;
    tmp[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3]*v2[3];
    tmp[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3]*v2[3];
    tmp[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3]*v2[3];
    tmp[3] = m1[3][0]*v2[0] + m1[3][1]*v2[1] + m1[3][2]*v2[2] + m1[3][3]*v2[3];
    COPY_VECTOR(res, tmp);
}

/* assumes res is not the same as v2 */
inline void MAT_MULT_VEC_4x4_4_QUICK(Vector res, Matrix m1, Vector v2)
{
    res[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3]*v2[3];
    res[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3]*v2[3];
    res[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3]*v2[3];
    res[3] = m1[3][0]*v2[0] + m1[3][1]*v2[1] + m1[3][2]*v2[2] + m1[3][3]*v2[3];
}

/* assumes homogeneous coordinate is 1.0f */
inline void MAT_MULT_VEC_4x4_3(Vector res, Matrix m1, Vector v2)
{
    Vector tmp;
    tmp[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3];
    tmp[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3];
    tmp[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3];
    tmp[3] = m1[3][0]*v2[0] + m1[3][1]*v2[1] + m1[3][2]*v2[2] + m1[3][3];
    COPY_VECTOR(res, tmp);
}

/* assumes res is not the same as v2
   assumes homogeneous coordinate is 1.0f */
inline void MAT_MULT_VEC_4x4_3_QUICK(Vector res, Matrix m1, Vector v2)
{
    res[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3];
    res[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3];
    res[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3];
    res[3] = m1[3][0]*v2[0] + m1[3][1]*v2[1] + m1[3][2]*v2[2] + m1[3][3];
}

/* assumes last row of matrix is [0 0 0 1] */
inline void MAT_MULT_VEC_3x4_4(Vector res, Matrix m1, Vector v2)
{
    Vector tmp;
    tmp[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3]*v2[3];
    tmp[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3]*v2[3];
    tmp[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3]*v2[3];
    tmp[3] = v2[3];
    COPY_VECTOR(res, tmp);
}

/* assumes res is not the same as v2
   assumes last row of matrix is [0 0 0 1] */
inline void MAT_MULT_VEC_3x4_4_QUICK(Vector res, Matrix m1, Vector v2)
{
    res[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3]*v2[3];
    res[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3]*v2[3];
    res[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3]*v2[3];
    res[3] = v2[3];
}

/* assumes homogeneous coordinate is 1.0f
   assumes last row of matrix is [0 0 0 1] */
inline void MAT_MULT_VEC_3x4_3(Vector res, Matrix m1, Vector v2)
{
    Vector tmp;
    tmp[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3];
    tmp[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3];
    tmp[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3];
    tmp[3] = 1.0f;
    COPY_VECTOR(res, tmp);
}

/* assumes res is not the same as v2
   assumes homogeneous coordinate is 1.0f
   assumes last row of matrix is [0 0 0 1] */
inline void MAT_MULT_VEC_3x4_3_QUICK(Vector res, Matrix m1, Vector v2)
{
    res[0] = m1[0][0]*v2[0] + m1[0][1]*v2[1] + m1[0][2]*v2[2] + m1[0][3];
    res[1] = m1[1][0]*v2[0] + m1[1][1]*v2[1] + m1[1][2]*v2[2] + m1[1][3];
    res[2] = m1[2][0]*v2[0] + m1[2][1]*v2[1] + m1[2][2]*v2[2] + m1[2][3];
    res[3] = 1.0f;
}

inline void IDENTITY(Matrix m)
{
    m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
    m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
    m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

inline void SCALE(Matrix m, float sx, float sy, float sz)
{
    m[0][0] = sx; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
    m[1][0] = 0.0f; m[1][1] = sy; m[1][2] = 0.0f; m[1][3] = 0.0f;
    m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = sz; m[2][3] = 0.0f;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

inline void UNIFORM_SCALE(Matrix m, float s)
{
    SCALE(m, s, s, s);
}

inline void TRANSLATE(Matrix m, float dx, float dy, float dz)
{
    m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = dx;
    m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = dy;
    m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = dz;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

inline void ROTATE_X(Matrix m, float rad)
{
    float cos_angle, sin_angle;
    cos_angle = (float)cos(rad);
    sin_angle = (float)sin(rad);
    m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
    m[1][0] = 0.0f; m[1][1] = cos_angle; m[1][2] = -sin_angle; m[1][3] = 0.0f;
    m[2][0] = 0.0f; m[2][1] = sin_angle; m[2][2] = cos_angle; m[2][3] = 0.0f;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

inline void ROTATE_Y(Matrix m, float rad)
{
    float cos_angle, sin_angle;
    cos_angle = (float)cos(rad);
    sin_angle = (float)sin(rad);
    m[0][0] = cos_angle; m[0][1] = 0.0f; m[0][2] = sin_angle; m[0][3] = 0.0f;
    m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
    m[2][0] = -sin_angle; m[2][1] = 0.0f; m[2][2] = cos_angle; m[2][3] = 0.0f;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

inline void ROTATE_Z(Matrix m, float rad)
{
    float cos_angle, sin_angle;
    cos_angle = (float)cos(rad);
    sin_angle = (float)sin(rad);
    m[0][0] = cos_angle; m[0][1] = -sin_angle; m[0][2] = 0.0f; m[0][3] = 0.0f;
    m[1][0] = sin_angle; m[1][1] = cos_angle; m[1][2] = 0.0f; m[1][3] = 0.0f;
    m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
    m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

/* Defines matrix. Given yaw, pitch, roll, and location, gives matrix to move
   objects from centered at the origin with front along +z to the position
   specified. */
inline void ROTATE_THEN_MOVE(Matrix matrix, float yaw, float pitch, float roll,
 			     float xloc, float yloc, float zloc)
{
    float cosyaw, cospitch, cosroll, sinyaw, sinpitch, sinroll;
    cosyaw = (float)cos(yaw);
    cospitch = (float)cos(pitch);
    cosroll = (float)cos(roll);
    sinyaw = (float)sin(yaw);
    sinpitch = (float)sin(pitch);
    sinroll = (float)sin(roll);
    matrix[0][0] = cosyaw*cosroll + sinyaw*sinpitch*sinroll;
    matrix[0][1] = -cosyaw*sinroll + sinyaw*sinpitch*cosroll;
    matrix[0][2] = sinyaw*cospitch;
    matrix[0][3] = xloc;
    matrix[1][0] = cospitch*sinroll;
    matrix[1][1] = cospitch*cosroll;
    matrix[1][2] = -sinpitch;
    matrix[1][3] = yloc;
    matrix[2][0] = -sinyaw*cosroll + cosyaw*sinpitch*sinroll;
    matrix[2][1] = sinyaw*sinroll + cosyaw*sinpitch*cosroll;
    matrix[2][2] = cosyaw*cospitch;
    matrix[2][3] = zloc;
    matrix[3][0] = 0.0f;
    matrix[3][1] = 0.0f;
    matrix[3][2] = 0.0f;
    matrix[3][3] = 1.0f;
}

#endif /* __UTIL3D_H__ */