#include <cmath>
#include "Math.h"
namespace details
{
inline float e(const float *m, int j, int i)
{
return m[(j % 4) * 4 + (i % 4)];
}
inline float invf(int i, int j, const float *m)
{
int o = 2 + (j - i);
i += 4 + o;
j += 4 - o;
float inv =
+ e(m, j-1, i+1) * e(m, j+0, i+0) * e(m, j+1, i-1)
+ e(m, j+1, i+1) * e(m, j-1, i+0) * e(m, j+0, i-1)
+ e(m, j-1, i-1) * e(m, j+0, i+1) * e(m, j+1, i+0)
- e(m, j-1, i-1) * e(m, j+0, i+0) * e(m, j+1, i+1)
- e(m, j+1, i-1) * e(m, j-1, i+0) * e(m, j+0, i+1)
- e(m, j-1, i+1) * e(m, j+0, i-1) * e(m, j+1, i+0);
return (o % 2) ? inv : -inv;
}
bool inverseMatrix4x4(float *restrict out, const float *restrict m)
{
float inv[16];
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
inv[j*4+i] = invf(i,j,m);
double determinant = 0;
for (int k = 0; k < 4; k++)
determinant += m[k] * inv[k*4];
if (determinant == 0)
return false;
float invdet = float(1.0 / determinant);
for (int i = 0; i < 16; i++)
out[i] = inv[i] * invdet;
return true;
}
bool matrix4x4ToNormalMatrix3x3(float *restrict out, const float *restrict m)
{
double determinant =
e(m,0,0) * ( e(m,1,1)*e(m,2,2) - e(m,2,1)*e(m,1,2) )
- e(m,0,1) * ( e(m,1,0)*e(m,2,2) - e(m,1,2)*e(m,2,0) )
+ e(m,0,2) * ( e(m,1,0)*e(m,2,1) - e(m,1,1)*e(m,2,0) );
if (determinant == 0)
return false;
float invdet = float(1.0 / determinant);
out[0] = (e(m,1,1)*e(m,2,2)-e(m,2,1)*e(m,1,2))*invdet;
out[1] = -(e(m,0,1)*e(m,2,2)-e(m,0,2)*e(m,2,1))*invdet;
out[2] = (e(m,0,1)*e(m,1,2)-e(m,0,2)*e(m,1,1))*invdet;
out[3] = -(e(m,1,0)*e(m,2,2)-e(m,1,2)*e(m,2,0))*invdet;
out[4] = (e(m,0,0)*e(m,2,2)-e(m,0,2)*e(m,2,0))*invdet;
out[5] = -(e(m,0,0)*e(m,1,2)-e(m,1,0)*e(m,0,2))*invdet;
out[6] = (e(m,1,0)*e(m,2,1)-e(m,2,0)*e(m,1,1))*invdet;
out[7] = -(e(m,0,0)*e(m,2,1)-e(m,2,0)*e(m,0,1))*invdet;
out[8] = (e(m,0,0)*e(m,1,1)-e(m,1,0)*e(m,0,1))*invdet;
return true;
}
void setMatrix4x4(float* out,
float m11, float m12, float m13, float m14,
float m21, float m22, float m23, float m24,
float m31, float m32, float m33, float m34,
float m41, float m42, float m43, float m44)
{
out[0] = m11; out[1] = m12; out[2] = m13; out[3] = m14;
out[4] = m21; out[5] = m22; out[6] = m23; out[7] = m24;
out[8] = m31; out[9] = m32; out[10] = m33; out[11] = m34;
out[12] = m41; out[13] = m42; out[14] = m43; out[15] = m44;
}
}
namespace Math
{
float dotProduct(const float *v1, const float *v2, int stride1, int stride2, int count)
{
float result = 0;
for (int i = 0; i < count; i++)
result += v1[i * stride1] * v2[i * stride2];
return result;
}
void crossProduct(float *restrict out, const float *restrict v1, const float *restrict v2)
{
for (int x = 0; x < 3; x++)
out[x] = v1[(x+1)%3] * v2[(x+2)%3] - v1[(x+2)%3] * v2[(x+1)%3];
}
void transformVector(float *restrict vout, const float *restrict m, const float *restrict vin)
{
for (int i = 0; i < 4; i++)
vout[i] = dotProduct(m+i, vin, 4);
}
void normalizeVector(float *vInOut, int count)
{
float invMag = 1.0f / sqrt(dotProduct(vInOut, vInOut, 1, 1, 3));
for (int i = 0; i < count; i++)
vInOut[i] *= invMag;
}
void makeIdentityMatrix4x4(float* a_out)
{
for (int j = 0; j < 16; j++)
a_out[j] = 0;
a_out[0] = a_out[5] = a_out[10] = a_out[15] = 1;
}
void makePerspectiveMatrix4x4(float* a_out, float a_fov, float a_aspect, float a_near, float a_far)
{
float ctan = 1.0f / tanf(a_fov / 2.0f);
float inlf = 1.0f / (a_near - a_far);
details::setMatrix4x4(a_out, ctan / a_aspect, 0.0f, 0.0f, 0.0f, 0.0f, ctan, 0.0f, 0.0f,
0.0f, 0.0f, (a_far + a_near) * inlf, -1.0f, 0.0f, 0.0f, (2.0f * a_far * a_near) * inlf, 0.0f);
}
void makeOrthographicMatrix4x4(float* a_out, float a_left, float a_right, float a_bottom, float a_top, float a_near, float a_far)
{
float ral = a_right + a_left;
float tab = a_top + a_bottom;
float fan = a_far + a_near;
float rsl = 1.0f / (a_right - a_left);
float tsb = 1.0f / (a_top - a_bottom);
float fsn = 1.0f / (a_far - a_near);
details::setMatrix4x4(a_out, 2.0f * rsl, 0.0f, 0.0f, 0.0f, 0.0f, 2.0f * tsb, 0.0f, 0.0f,
0.0f, 0.0f, -2.0f * fsn, 0.0f, -ral * rsl, -tab * tsb, -fan * fsn, 1.0f);
}
void translateMatrix4x4(float *restrict a_inOut, const float *restrict vec3)
{
// similarities to transformVector
for (int i = 0; i < 4; i++)
a_inOut[12+i] += dotProduct(a_inOut+i, vec3, 4, 1, 3);
}
void translateMatrix4x4(float* a_inOut, float x, float y, float z)
{
float xyz[3] = { x, y, z };
translateMatrix4x4(a_inOut, xyz);
}
void scaleMatrix4x4(float *restrict a_inOut, const float *restrict vec3)
{
for (int j = 0; j < 3; j++)
for (int i = 0; i < 4; i++)
a_inOut[j*4+i] *= vec3[j];
}
void scaleMatrix4x4(float* a_inOut, float x, float y, float z)
{
float xyz[3] = { x, y, z };
scaleMatrix4x4(a_inOut, xyz);
}
void rotateMatrix4x4(float *restrict a_inOut, float radians, float *restrict v)
{
normalizeVector(v, 3);
float cos = cosf(radians);
float cosp = 1.0f - cos;
float sin = sinf(radians);
float rotm[16];
details::setMatrix4x4(rotm,
cos + cosp * v[0] * v[0], cosp * v[0] * v[1] + v[2] * sin, cosp * v[0] * v[2] - v[1] * sin, 0.0f,
cosp * v[0] * v[1] - v[2] * sin, cos + cosp * v[1] * v[1], cosp * v[1] * v[2] + v[0] * sin, 0.0f,
cosp * v[0] * v[2] + v[1] * sin, cosp * v[1] * v[2] - v[0] * sin, cos + cosp * v[2] * v[2], 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
float m[16];
for (int i = 0; i < 16; i++)
m[i] = a_inOut[i];
multiplyMatrix4x4(a_inOut, m, rotm); // is it possible to do the multiply in-place?
}
void rotateMatrix4x4(float* a_inOut, float radians, float x, float y, float z)
{
float xyz[3] = { x, y, z };
rotateMatrix4x4(a_inOut, radians, xyz);
}
// Helpers
void multiplyMatrix4x4(float *restrict out, const float *restrict m1, const float *restrict m2)
{
for (int j = 0; j < 4; j++)
for (int k = 0; k < 4; k++)
out[j+k*4] = dotProduct(m1+j, m2+k*4, 4);
}
bool matrix4x4ToNormalMatrix3x3(float *restrict out, const float *restrict m)
{
return details::matrix4x4ToNormalMatrix3x3(out, m);
}
bool inverseMatrix4x4(float *restrict out, const float *restrict m)
{
return details::inverseMatrix4x4(out, m);
}
void transposeMatrix4x4(float *restrict out, const float *restrict m)
{
details::setMatrix4x4(out, m[15], m[14], m[13], m[12], m[11], m[10],
m[9], m[8], m[7], m[6], m[5], m[4], m[3], m[2], m[1], m[0]);
}
}
/*
//
// Might be worth comparing with optimizations enabled how inverse compares to this if need more speed from it:
//
bool gluInvertMatrix(const float m[16], float invOut[16])
{
float inv[16], det;
int i;
inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] +
m[9] * m[7] * m[14] +
m[13] * m[6] * m[11] -
m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] +
m[4] * m[11] * m[14] +
m[8] * m[6] * m[15] -
m[8] * m[7] * m[14] -
m[12] * m[6] * m[11] +
m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] -
m[4] * m[11] * m[13] -
m[8] * m[5] * m[15] +
m[8] * m[7] * m[13] +
m[12] * m[5] * m[11] -
m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] +
m[4] * m[10] * m[13] +
m[8] * m[5] * m[14] -
m[8] * m[6] * m[13] -
m[12] * m[5] * m[10] +
m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] +
m[1] * m[11] * m[14] +
m[9] * m[2] * m[15] -
m[9] * m[3] * m[14] -
m[13] * m[2] * m[11] +
m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] -
m[0] * m[11] * m[14] -
m[8] * m[2] * m[15] +
m[8] * m[3] * m[14] +
m[12] * m[2] * m[11] -
m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] +
m[0] * m[11] * m[13] +
m[8] * m[1] * m[15] -
m[8] * m[3] * m[13] -
m[12] * m[1] * m[11] +
m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] -
m[0] * m[10] * m[13] -
m[8] * m[1] * m[14] +
m[8] * m[2] * m[13] +
m[12] * m[1] * m[10] -
m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] -
m[1] * m[7] * m[14] -
m[5] * m[2] * m[15] +
m[5] * m[3] * m[14] +
m[13] * m[2] * m[7] -
m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] +
m[0] * m[7] * m[14] +
m[4] * m[2] * m[15] -
m[4] * m[3] * m[14] -
m[12] * m[2] * m[7] +
m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] -
m[0] * m[7] * m[13] -
m[4] * m[1] * m[15] +
m[4] * m[3] * m[13] +
m[12] * m[1] * m[7] -
m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] +
m[0] * m[6] * m[13] +
m[4] * m[1] * m[14] -
m[4] * m[2] * m[13] -
m[12] * m[1] * m[6] +
m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] +
m[1] * m[7] * m[10] +
m[5] * m[2] * m[11] -
m[5] * m[3] * m[10] -
m[9] * m[2] * m[7] +
m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] -
m[0] * m[7] * m[10] -
m[4] * m[2] * m[11] +
m[4] * m[3] * m[10] +
m[8] * m[2] * m[7] -
m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] +
m[0] * m[7] * m[9] +
m[4] * m[1] * m[11] -
m[4] * m[3] * m[9] -
m[8] * m[1] * m[7] +
m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] -
m[0] * m[6] * m[9] -
m[4] * m[1] * m[10] +
m[4] * m[2] * m[9] +
m[8] * m[1] * m[6] -
m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if (det == 0)
return false;
det = 1.0 / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
return true;
}
*/
