/******************************************************************************

* The MIT License (MIT)

*

* Copyright (c) 2014 Crytek

*

* Permission is hereby granted, free of charge, to any person obtaining a copy

* of this software and associated documentation files (the "Software"), to deal

* in the Software without restriction, including without limitation the rights

* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

* copies of the Software, and to permit persons to whom the Software is

* furnished to do so, subject to the following conditions:

*

* The above copyright notice and this permission notice shall be included in

* all copies or substantial portions of the Software.

*

* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

* THE SOFTWARE.

******************************************************************************/

#include <string.h>

#include <stdlib.h>

#include <math.h>

#include <float.h>

#include "vec.h"

#include "matrix.h"

#include "quat.h"

static inline size_t matIdx(const size_t x, const size_t y) { return x+y*4; }

Matrix4f Matrix4f::Mul(const Matrix4f &o) const

{

Matrix4f m;

for(size_t x=0; x < 4; x++)

{

for(size_t y=0; y < 4; y++)

{

m[matIdx(x,y)] = (*this)[matIdx(x,0)] * o[matIdx(0,y)] +

(*this)[matIdx(x,1)] * o[matIdx(1,y)] +

(*this)[matIdx(x,2)] * o[matIdx(2,y)] +

(*this)[matIdx(x,3)] * o[matIdx(3,y)];

}

}

return m;

}

Matrix4f Matrix4f::Inverse() const

{

float a0 = (*this)[ 0]*(*this)[ 5] - (*this)[ 1]*(*this)[ 4];

float a1 = (*this)[ 0]*(*this)[ 6] - (*this)[ 2]*(*this)[ 4];

float a2 = (*this)[ 0]*(*this)[ 7] - (*this)[ 3]*(*this)[ 4];

float a3 = (*this)[ 1]*(*this)[ 6] - (*this)[ 2]*(*this)[ 5];

float a4 = (*this)[ 1]*(*this)[ 7] - (*this)[ 3]*(*this)[ 5];

float a5 = (*this)[ 2]*(*this)[ 7] - (*this)[ 3]*(*this)[ 6];

float b0 = (*this)[ 8]*(*this)[13] - (*this)[ 9]*(*this)[12];

float b1 = (*this)[ 8]*(*this)[14] - (*this)[10]*(*this)[12];

float b2 = (*this)[ 8]*(*this)[15] - (*this)[11]*(*this)[12];

float b3 = (*this)[ 9]*(*this)[14] - (*this)[10]*(*this)[13];

float b4 = (*this)[ 9]*(*this)[15] - (*this)[11]*(*this)[13];

float b5 = (*this)[10]*(*this)[15] - (*this)[11]*(*this)[14];

float det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;

if (fabs(det) > FLT_EPSILON)

{

Matrix4f inverse;

inverse[ 0] = + (*this)[ 5]*b5 - (*this)[ 6]*b4 + (*this)[ 7]*b3;

inverse[ 4] = - (*this)[ 4]*b5 + (*this)[ 6]*b2 - (*this)[ 7]*b1;

inverse[ 8] = + (*this)[ 4]*b4 - (*this)[ 5]*b2 + (*this)[ 7]*b0;

inverse[12] = - (*this)[ 4]*b3 + (*this)[ 5]*b1 - (*this)[ 6]*b0;

inverse[ 1] = - (*this)[ 1]*b5 + (*this)[ 2]*b4 - (*this)[ 3]*b3;

inverse[ 5] = + (*this)[ 0]*b5 - (*this)[ 2]*b2 + (*this)[ 3]*b1;

inverse[ 9] = - (*this)[ 0]*b4 + (*this)[ 1]*b2 - (*this)[ 3]*b0;

inverse[13] = + (*this)[ 0]*b3 - (*this)[ 1]*b1 + (*this)[ 2]*b0;

inverse[ 2] = + (*this)[13]*a5 - (*this)[14]*a4 + (*this)[15]*a3;

inverse[ 6] = - (*this)[12]*a5 + (*this)[14]*a2 - (*this)[15]*a1;

inverse[10] = + (*this)[12]*a4 - (*this)[13]*a2 + (*this)[15]*a0;

inverse[14] = - (*this)[12]*a3 + (*this)[13]*a1 - (*this)[14]*a0;

inverse[ 3] = - (*this)[ 9]*a5 + (*this)[10]*a4 - (*this)[11]*a3;

inverse[ 7] = + (*this)[ 8]*a5 - (*this)[10]*a2 + (*this)[11]*a1;

inverse[11] = - (*this)[ 8]*a4 + (*this)[ 9]*a2 - (*this)[11]*a0;

inverse[15] = + (*this)[ 8]*a3 - (*this)[ 9]*a1 + (*this)[10]*a0;

float invDet = 1.0f/det;

inverse[ 0] *= invDet;

inverse[ 1] *= invDet;

inverse[ 2] *= invDet;

inverse[ 3] *= invDet;

inverse[ 4] *= invDet;

inverse[ 5] *= invDet;

inverse[ 6] *= invDet;

inverse[ 7] *= invDet;

inverse[ 8] *= invDet;

inverse[ 9] *= invDet;

inverse[10] *= invDet;

inverse[11] *= invDet;

inverse[12] *= invDet;

inverse[13] *= invDet;

inverse[14] *= invDet;

inverse[15] *= invDet;

return inverse;

}

// no inverse

return Matrix4f::Identity();

}

Vec3f Matrix4f::Transform(const Vec3f &v, const float w) const

{

Vec3f vout = Vec3f((*this)[matIdx(0,0)] * v.x + (*this)[matIdx(0,1)] * v.y + (*this)[matIdx(0,2)] * v.z + (*this)[matIdx(0,3)] * w,

(*this)[matIdx(1,0)] * v.x + (*this)[matIdx(1,1)] * v.y + (*this)[matIdx(1,2)] * v.z + (*this)[matIdx(1,3)] * w,

(*this)[matIdx(2,0)] * v.x + (*this)[matIdx(2,1)] * v.y + (*this)[matIdx(2,2)] * v.z + (*this)[matIdx(2,3)] * w);

float wout = (*this)[matIdx(3,0)] * v.x + (*this)[matIdx(3,1)] * v.y + (*this)[matIdx(3,2)] * v.z + (*this)[matIdx(3,3)] * w;

return vout*(1.0f/wout);

}

const Vec3f Matrix4f::GetPosition() const

{

return Vec3f(f[12], f[13], f[14]);

}

const Vec3f Matrix4f::GetForward() const

{

return Vec3f(f[8], f[9], f[10]);

}

const Vec3f Matrix4f::GetRight() const

{

return Vec3f(f[0], f[1], f[2]);

}

const Vec3f Matrix4f::GetUp() const

{

return Vec3f(f[4], f[5], f[6]);

}

Matrix4f Matrix4f::Translation(const Vec3f &t)

{

Matrix4f trans = Matrix4f::Identity();

trans[12] = t.x;

trans[13] = t.y;

trans[14] = t.z;

return trans;

}

Matrix4f Matrix4f::RotationX(const float r)

{

float m[16] = { 1.0f, 0.0f, 0.0f, 0.0f,

0.0f, cosf(r), -sinf(r), 0.0f,

0.0f, sinf(r), cosf(r), 0.0f,

0.0f, 0.0f, 0.0f, 1.0f,

};

return Matrix4f(m);

}

Matrix4f Matrix4f::RotationY(const float r)

{

float m[16] = { cosf(r), 0.0f, sinf(r), 0.0f,

0.0f, 1.0f, 0.0f, 0.0f,

-sinf(r), 0.0f, cosf(r), 0.0f,

0.0f, 0.0f, 0.0f, 1.0f,

};

return Matrix4f(m);

}

Matrix4f Matrix4f::RotationZ(const float r)

{

float m[16] = { cosf(r), -sinf(r), 0.0f, 0.0f,

sinf(r), cosf(r), 0.0f, 0.0f,

0.0f, 0.0f, 1.0f, 0.0f,

0.0f, 0.0f, 0.0f, 1.0f,

};

return Matrix4f(m);

}

Matrix4f Matrix4f::RotationZYX(const Vec3f &rot)

{

Quatf Qx = Quatf::AxisAngle(Vec3f(1.0f, 0.0f, 0.0f), rot.x);

Quatf Qy = Quatf::AxisAngle(Vec3f(0.0f, 1.0f, 0.0f), rot.y);

Quatf Qz = Quatf::AxisAngle(Vec3f(0.0f, 0.0f, 1.0f), rot.z);

Quatf R = Qx * Qy * Qz;

return R.GetMatrix();

}

Matrix4f Matrix4f::RotationXYZ(const Vec3f &rot)

{

Quatf Qx = Quatf::AxisAngle(Vec3f(1.0f, 0.0f, 0.0f), rot.x);

Quatf Qy = Quatf::AxisAngle(Vec3f(0.0f, 1.0f, 0.0f), rot.y);

Quatf Qz = Quatf::AxisAngle(Vec3f(0.0f, 0.0f, 1.0f), rot.z);

Quatf R = Qz * Qy * Qx;

return R.GetMatrix();

}

Matrix4f Matrix4f::Orthographic(const float near, const float far)

{

float L = -10.0f;

float R = 10.0f;

float T = 10.0f;

float B = -10.0f;

float N = -abs(far-near)*0.5f;

float F = abs(far-near)*0.5f;

if(far < near)

{

float tmp = F;

F = N;

N = tmp;

}

float ortho[16] = { 2.0f/(R-L), 0.0f, 0.0f, (L+R)/(L-R),

0.0f, 2.0f/(T-B), 0.0f, (T+B)/(B-T),

0.0f, 0.0f, 1.0f/(F-N), (F+N)/(N-F),

0.0f, 0.0f, 0.0f, 1.0f,

};

return Matrix4f(ortho);

}

Matrix4f Matrix4f::Perspective(const float degfov, const float N, const float F, const float A)

{

const float radfov = degfov * (3.1415926535f/180.0f);

float S = 1/tanf(radfov * 0.5f);

float persp[16] = { S/A, 0.0f, 0.0f, 0.0f,

0.0f, S, 0.0f, 0.0f,

0.0f, 0.0f, F/(F-N), 1.0f,

0.0f, 0.0f, -(F*N)/(F-N), 0.0f,

};

return Matrix4f(persp);

}