#include <cstdlib>

#include "geometry.h"

Real real_random()

    { return rand()/(Real)RAND_MAX; }

Matrix4 Matrix4::operator*(const Matrix4 &m) const {

    return Matrix4(

        m00*m.m00 + m10*m.m01 + m20*m.m02 + m30*m.m02,

        m01*m.m00 + m11*m.m01 + m21*m.m02 + m31*m.m02,

        m02*m.m00 + m12*m.m01 + m22*m.m02 + m32*m.m02,

        m03*m.m00 + m13*m.m01 + m23*m.m02 + m33*m.m02,

        m00*m.m10 + m10*m.m11 + m20*m.m12 + m30*m.m12,

        m01*m.m10 + m11*m.m11 + m21*m.m12 + m31*m.m12,

        m02*m.m10 + m12*m.m11 + m22*m.m12 + m32*m.m12,

        m03*m.m10 + m13*m.m11 + m23*m.m12 + m33*m.m12,

        m00*m.m20 + m10*m.m21 + m20*m.m22 + m30*m.m22,

        m01*m.m20 + m11*m.m21 + m21*m.m22 + m31*m.m22,

        m02*m.m20 + m12*m.m21 + m22*m.m22 + m32*m.m22,

        m03*m.m20 + m13*m.m21 + m23*m.m22 + m33*m.m22,

        m00*m.m30 + m10*m.m31 + m20*m.m32 + m30*m.m32,

        m01*m.m30 + m11*m.m31 + m21*m.m32 + m31*m.m32,

        m02*m.m30 + m12*m.m31 + m22*m.m32 + m32*m.m32,

        m03*m.m30 + m13*m.m31 + m23*m.m32 + m33*m.m32 );

}

Real Matrix4::det() const {

    return m00 * (m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31))

         + m01 * (m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32))

         + m02 * (m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31))

         + m03 * (m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));

}

Matrix4 Matrix4::inv() const {

    Matrix4 r;

    r.m00 = m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31);

    r.m10 = m10*(m23*m32 - m22*m33) + m12*(m20*m33 - m23*m30) + m13*(m22*m30 - m20*m32);

    r.m20 = m10*(m21*m33 - m23*m31) + m11*(m23*m30 - m20*m33) + m13*(m20*m31 - m21*m30);

    r.m30 = m10*(m22*m31 - m21*m32) + m11*(m20*m32 - m22*m30) + m12*(m21*m30 - m20*m31);

    double det = m00*r.m00 + m01*r.m10 + m02*r.m20 + m03*r.m30;

    if (fabs(det) <= precision) return zero();

    det = 1/det;

    r.m00 *= det;

    r.m10 *= det;

    r.m20 *= det;

    r.m30 *= det;

    r.m01 = det*(m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32));

    r.m11 = det*(m00*(m22*m33 - m23*m32) + m02*(m23*m30 - m20*m33) + m03*(m20*m32 - m22*m30));

    r.m21 = det*(m00*(m23*m31 - m21*m33) + m01*(m20*m33 - m23*m30) + m03*(m21*m30 - m20*m31));

    r.m31 = det*(m00*(m21*m32 - m22*m31) + m01*(m22*m30 - m20*m32) + m02*(m20*m31 - m21*m30));

    r.m02 = det*(m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31));

    r.m12 = det*(m00*(m13*m32 - m12*m33) + m02*(m10*m33 - m13*m30) + m03*(m12*m30 - m10*m32));

    r.m22 = det*(m00*(m11*m33 - m13*m31) + m01*(m13*m30 - m10*m33) + m03*(m10*m31 - m11*m30));

    r.m32 = det*(m00*(m12*m31 - m11*m32) + m01*(m10*m32 - m12*m30) + m02*(m11*m30 - m10*m31));

    r.m03 = det*(m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));

    r.m13 = det*(m00*(m12*m23 - m13*m22) + m02*(m13*m20 - m10*m23) + m03*(m10*m22 - m12*m20));

    r.m23 = det*(m00*(m13*m21 - m11*m23) + m01*(m10*m23 - m13*m20) + m03*(m11*m20 - m10*m21));

    r.m33 = det*(m00*(m11*m22 - m12*m21) + m01*(m12*m20 - m10*m22) + m02*(m10*m21 - m11*m20));

    return r;

}

Matrix4 Matrix4::zero() {

    return Matrix4(

        0, 0, 0, 0,

        0, 0, 0, 0,

        0, 0, 0, 0,

        0, 0, 0, 0 );

}

Matrix4 Matrix4::translation(const Vector3 &translate) {

    return Matrix4(

        1, 0, 0, 0,

        0, 1, 0, 0,

        0, 0, 1, 0,

        translate.x, translate.y, translate.z, 1 );

}

Matrix4 Matrix4::scaling(const Vector3 &scale) {

    return Matrix4(

        scale.x, 0, 0, 0,

        0, scale.y, 0, 0,

        0, 0, scale.z, 0,

        0, 0, 0, 1 );

}

Matrix4 Matrix4::rotation(const Vector3 &axis, Real angle) {

    Real len = axis.len();

    if (len < precision) return Matrix4();

    Real s = sin(angle);

    Real c = cos(angle);

    Vector3 a = axis/len;

    Vector3 ac = a*(1 - c);

    return Matrix4(

        a.x*ac.x + c    , a.y*ac.x + a.z*s, a.z*ac.x - a.y*s, 0,

        a.x*ac.y - a.z*s, a.y*ac.y + c    , a.z*ac.y + a.x*s, 0,

        a.x*ac.z + a.y*s, a.y*ac.z - a.x*s, a.z*ac.z + c    , 0,

        0               , 0               , 0               , 1 );

}

Matrix4 Matrix4::perspective(Real fovy, Real aspect, Real z_near, Real z_far) {

    if (fabs(fovy) <= precision) return zero();

    if (fabs(fovy) >= 180.0-precision) return zero();

    if (fabs(aspect) <= precision) return zero();

    if (fabs(z_near - z_far) <= precision) return zero();

    Real f = 1/tan(0.5*fovy/180.0*M_PI);

    Real d = 1/(z_near - z_far);

    return Matrix4(

        f/aspect, 0, 0                 ,  0,

        0       , f, 0                 ,  0,

        0       , 0, (z_near + z_far)*d, -1,

        0       , 0, 2*z_near*z_far*d  ,  0 );

}