// Trigonometric Functions Converted To Degrees,
// Because OpenGL Uses Degrees.
float sind (float theta)
{
    return sin (theta * M_PI/180.0f);
}
float cosd (float theta)
{
    return cos (theta * M_PI/180.0f);
}
float tand (float theta)
{
    return tan (theta * M_PI/180.0f);
}

// 2D Vector Functions.  I'm Not Sure
// Why I Left Them In Here.  Maybe For
// Texture Coordinate Purposes?  Some
// Operators Are Missing Here, Like
// Negating A Vector.
class vec2
{
    public:

    float array [2];

    // Default Constructor Sets All Values To 0.
    // Custom Constructor Sets x/y Values.
    vec2 (){array[0]=0; array[1]=0;}
    vec2 (float x, float y)
    {
        array[0] = x;
        array[1] = y;
    }

    // Add A Vector To Another Vector.
    // Useful When Adding Vectors...
    vec2 operator+ (vec2 a)
    {
        return vec2 (array[0]+a.array[0], array[1]+a.array[1]);
    }

    // Subtract A Vector From Another Vector.
    // Useful In Determining The Distance
    // Between Two Points.
    vec2 operator- (vec2 a)
    {
        return vec2 (array[0]-a.array[0], array[1]-a.array[1]);
    }

    // Multiply A Vector By Another Vector.
    // Returns The Dot Product Of The Vectors.
    float operator* (vec2 a)
    {
        return (float) (array[0]*a.array[0] + array[1]*a.array[1]);
    }
};

// These Are The Most Often Used Vectors,
// For Polygon Vertices, Collision Detection
// Normals, various Magnitude Vectors For
// Physics, Etc...
class vec3
{
    public:
    float array [3];

    // Default Constructor Sets Vec3 to (0, 0, 0),
    // Custom Constructor Sets Vec3 to (x, y, z).
    vec3 (){array[0]=0; array[1]=0; array[2]=0;}
    vec3 (float xIn, float yIn, float zIn)
    {
        array[0] = xIn;
        array[1] = yIn;
        array[2] = zIn;
    }

    // Add/Subtract One Vector To/From Another
    // Vector.  The Function Returns The
    // Sum/Difference Of The Vectors.
    vec3 operator+ (vec3 a)
    {
        return vec3 (array[0] + a.array[0],
                     array[1] + a.array[1],
                     array[2] + a.array[2]);
    }
    vec3 operator- (vec3 a)
    {
        return vec3 (array[0] - a.array[0],
                     array[1] - a.array[1],
                     array[2] - a.array[2]);
    }

    // Sets A Vector To Any Other Array
    // Of 3 floating point values.
    vec3 operator= (float Array [3])
    {
        array [0] = Array [0];
        array [1] = Array [1];
        array [2] = Array [2];
    }

    // Returns The Vector Negated.
    vec3 operator- ()
    {
        return vec3 (-array[0],
                     -array[1],
                     -array[2]);
    }

    // Multiplies The Vector By Another Vector.
    // Returns The Dot Product Of The Two Vectors.
    float operator* (vec3 a)
    {
        return (float)(array[0]*a.array[0] + array[1]*a.array[1] + array[2]*a.array[2]);
    }
};

// Returns The CrossProduct Of Two Vectors.
// The Result Is Another Vector.
vec3 CrossProduct (vec3 a, vec3 b)
{
    return vec3 (a.array[1]*b.array[2] - a.array[2]*b.array[1],
                 a.array[2]*b.array[1] - a.array[1]*b.array[2],
                 a.array[0]*b.array[1] - a.array[1]*b.array[0]);
}

// Returns The Length Of Any 2 or 3 Unit Vector.
float Length (vec2 a)
{
    return sqrt (a.array[0]*a.array[0] + a.array[1]*a.array[1]);
}
float Length (vec3 a)
{
    return sqrt (a.array[0]*a.array[0] + a.array[1]*a.array[1] + a.array[2]*a.array[2]);
}

// Normalizes The Vector So That
// It Has A Magnitude Of 1.
vec2 Normalize (vec2 a)
{
    float length;
    length = Length (a);
    if (!(length==0))
    {
        return vec2 (a.array[0]/length,
                     a.array[1]/length);
    }
    else
    {
        return vec2 (0, 0);
    }
}
vec3 Normalize (vec3 a)
{
    float length;
    length = Length (a);
    if (!(length==0))
    {
        return vec3 (a.array[0]/length,
                     a.array[1]/length,
                     a.array[2]/length);
    }
    else
    {
        return vec3 (0, 0, 0);
    }
}


class matrix
{
    public:
    float array [4][4];

    // Default Constructor Initializes The Matrix
    // To The Identity Matrix (Default Matrix).
    matrix ()
    {
        for (int i = 0; i<=3; i++)
        {
            for (int j = 0; j<=3; j++)
            {
                if (i-j==0)
                {
                    array [i][j] = 1.0f;
                }
                else
                {
                    array [i][j] = 0.0f;
                }
            }
        }
    }

    // Custom Constructor Allows Exact Manipulation
    // Of Every Element Within The Matrix.  Useful
    // To Manually Create Simple Rotation/Translations.
    matrix (float a, float b, float c, float d,
            float e, float f, float g, float h,
            float i, float j, float k, float l,
            float m, float n, float o, float p)
    {
        array [0][0] = a;
        array [0][1] = b;
        array [0][2] = c;
        array [0][3] = d;

        array [1][0] = e;
        array [1][1] = f;
        array [1][2] = g;
        array [1][3] = h;

        array [2][0] = i;
        array [2][1] = j;
        array [2][2] = k;
        array [2][3] = l;

        array [3][0] = m;
        array [3][1] = n;
        array [3][2] = o;
        array [3][3] = p;
    }

    // Add/Subtract A Vector To The Matrix.
    // This Returns A Matrix With The Same Direction,
    // But Adds/Subtracts The Vector To The Position.
    matrix operator+(vec3 a)
    {
        // TheMatrix () Function Returns The
        // Matrix Which Called The Function.
        matrix temp = TheMatrix();
        for (int i=0;i<=2;i++)
        {temp.array[3][i]+=a.array[i];}
        return temp;
    }
    matrix operator-(vec3 a)
    {
        matrix temp = TheMatrix();
        for (int i=0;i<=2;i++)
        {temp.array[3][i]-=a.array[i];}
        return temp;
    }

    // Add/Subtract A Floating Point Value To The Matrix.
    // Will Move The Matrix x Units Along The Forward Vector.
    // Handy For Making The Camera Move Forward/Backward.
    matrix operator+(float x)
    {
        matrix temp = TheMatrix();
        for (int i=0; i<=2; i++)
        {temp.array[3][i] -= array [2][i] *x;}
        return temp;
    }
    matrix operator-(float x)
    {
        matrix temp = TheMatrix();
        for (int i=0; i<=3; i++)
        {temp.array[3][i] += array [2][i] *x;}
        return temp;
    }

    // Add/Subtract A Matrix From Another Matrix.
    // I'm Not Sure Why Anybody Would Ever Want To
    // Do It, But At Least They Can.
    matrix operator+(matrix a)
    {
        return AddMatrix (a);
    }
    matrix operator-(matrix a)
    {
        matrix temp = -a;
        return AddMatrix (temp);
    }

    // Negate A Matrix.
    // Again, Not Sure Why Anybody Would Want To...
    matrix operator-()
    {
        matrix temp;
        for (int i=0; i<=3; i++)
        {
            for (int j=0; j<=3; j++)
            {
                temp.array [i][j] = -array[i][j];
            }
        }
        return temp;
    }

    // Multiply A Matrix By Another Matrix.
    // Very Useful.  This Is How A New Translation
    // Or Rotation Is Applied To The Matrix.
    matrix operator* (matrix a)
    {
        matrix temp;
        float value;

        for (int i=0; i<=3; i++)
        {
            for (int j=0; j<=3; j++)
            {
                value = 0;
                for (int k=0; k<=3; k++)
                {
                    value = value + (array [k][j] * a.array [i][k]);
                }
                temp.array [i][j] = value;
            }
        }
        return temp;
    }

    // Multiply A Vector (3) By A Matrix.
    // Extremely Useful.
    vec3 operator* (vec3 a)
    {
        vec3 temp;
        float value;
        for (int i=0; i<=2; i++)
        {
            value = 0;
            for (int j=0; j<=2; j++)
            {
                value = value + a.array[j] * array [j][i];
            }
            temp.array[i] = value + array [3][i];
        }
        return temp;
    }

    private:

    // Function To Return The Matrix Which Called
    // The Original Function.  I'm Sure There's An
    // Easier Way To Do This...
    matrix TheMatrix ()
    {
        matrix temp;
        for (int i=0; i<=3; i++)
        {
            for (int j=0; j<=3; j++)
            {
                temp.array [i][j] = array [i][j];
            }
        }
        return temp;
    }

    // Returns The Matrix In Parenthesis Added To
    // The Matrix Which Called The Original Function.
    // Again, There's Probably An Easier Way...
    matrix AddMatrix (matrix &a)
    {
        matrix temp;
        for (int i=0; i<=3; i++)
        {
            for (int j=0; j<=3; j++)
            {
                temp.array[i][j] = a.array [i][j] + array[i][j];
            }
        }
        return temp;
    }
};

// Rotational Matrices.  The Functions Return A Matrix
// Which Can Be Multiplied By Another To Rotate It.
matrix MatrixRotateX (float theta)
{
    theta = -theta;
    return matrix (1, 0,             0,            0,
                   0, cosd (theta), -sind (theta), 0,
                   0, sind (theta),  cosd (theta), 0,
                   0, 0,             0,            1);
}
matrix MatrixRotateY (float theta)
{
    theta = -theta;
    return matrix (cosd (theta), 0, sind (theta), 0,
                   0,            1, 0,            0,
                   -sind (theta),0, cosd (theta), 0,
                   0,            0, 0,            1);
}
matrix MatrixRotateZ (float theta)
{
    theta = -theta;
    return matrix (cosd (theta), sind (theta), 0, 0,
                   -sind(theta), cosd (theta), 0, 0,
                   0,            0,            1, 0,
                   0,            0,            0, 1);
}

// Translational Matrix.  This Matrix Can Be Multiplied
// By Another To Move It By (x, y, z) Values.  This Will
// Be The Same Effect As Adding A Vector(3) Of (x, y, z)
// To The Matrix.
matrix MatrixTranslate (float x, float y, float z)
{
    return matrix (1, 0, 0, 0,
                   0, 1, 0, 0,
                   0, 0, 1, 0,
                   x, y, z, 1);
}

matrix Normalize (matrix a)
{
    float length;
    vec3 tempvec;
    matrix tempmatrix;
    for (int i=0; i<=2; i++)
    {
        tempvec = a.array [i];
        tempvec = Normalize (tempvec);
        if (!(length==0))
        {
            tempmatrix.array[i][0] = tempvec.array[0];
            tempmatrix.array[i][1] = tempvec.array[1];
            tempmatrix.array[i][2] = tempvec.array[2];
        }
        else
        {
            tempmatrix.array[i][0] = 0;
            tempmatrix.array[i][1] = 0;
            tempmatrix.array[i][2] = 0;
        }
    }
    return tempmatrix;
}

// Determinant/Inverting functions:

float Det (float a, float b,                // Takes a 2 X 2 Matrix And Returns The Determinant.
           float c, float d)
{
    return (float) (a*d - b*c);
}

float Det (float a1, float b1, float c1,    // Takes a 3 X 3 Matrix And Returns The Determinant.
           float a2, float b2, float c2,
           float a3, float b3, float c3)
{
    float temp = 0;
    temp += a1*(b2*c3 - c2*b3);
    temp += b1*(c2*a3 - a2*c3);
    temp += c1*(a2*b3 - b2*a3);
    return temp;
}

matrix RTInvert (matrix a)                  // Takes A Matrix And Returns The Inverse.
{
    matrix temp;
    float determinant;
    determinant = Det (a.array[0][0], a.array[0][1], a.array[0][2],
                       a.array[1][0], a.array[1][1], a.array[1][2],
                       a.array[2][0], a.array[2][1], a.array[2][2]);

    temp.array [0][0] = Det (a.array[1][1], a.array[1][2],
                             a.array[2][1], a.array[2][2]);
    temp.array [0][1] = Det (a.array[0][2], a.array[0][1],
                             a.array[2][2], a.array[2][1]);
    temp.array [0][2] = Det (a.array[0][1], a.array[0][2],
                             a.array[1][1], a.array[1][2]);
    temp.array [1][0] = Det (a.array[1][2], a.array[1][0],
                             a.array[2][2], a.array[2][0]);
    temp.array [1][1] = Det (a.array[0][0], a.array[0][2],
                             a.array[2][0], a.array[2][2]);
    temp.array [1][2] = Det (a.array[0][2], a.array[0][0],
                             a.array[1][2], a.array[1][0]);
    temp.array [2][0] = Det (a.array[1][0], a.array[1][1],
                             a.array[2][0], a.array[2][1]);
    temp.array [2][1] = Det (a.array[0][1], a.array[0][0],
                             a.array[2][1], a.array[2][0]);
    temp.array [2][2] = Det (a.array[0][0], a.array[0][1],
                             a.array[1][0], a.array[1][1]);

    for (int i=0; i<=2; i++)
    {
        for (int j=0; j<=2; j++)
        {
            temp.array [i][j] =  temp.array [i][j] / determinant;
        }
    }

    vec3 tempvec (a.array [3][0], a.array [3][1], a.array[3][2]);
    tempvec = temp * tempvec;
    temp.array [3][0] = -tempvec.array [0];
    temp.array [3][1] = -tempvec.array [1];
    temp.array [3][2] = -tempvec.array [2];

    return temp;
}