#include "geometry.h"


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 );
}