410 lines
11 KiB
C
410 lines
11 KiB
C
|
#include <math.h>
|
||
|
|
|
||
|
|
#include "shared/functions.h"
|
||
|
|
#include "shared/types.h"
|
||
|
|
|
||
|
|
Vector3 VECTOR_ZERO = {0.0, 0.0, 0.0};
|
||
|
|
|
||
|
|
void v3Save(Vector3 v, FILE* f)
|
||
|
|
{
|
||
|
|
toolsSaveDouble(f, v.x);
|
||
|
|
toolsSaveDouble(f, v.y);
|
||
|
|
toolsSaveDouble(f, v.z);
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Load(FILE* f)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
|
||
|
|
result.x = toolsLoadDouble(f);
|
||
|
|
result.y = toolsLoadDouble(f);
|
||
|
|
result.z = toolsLoadDouble(f);
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
void m4Save(Matrix4 m, FILE* f)
|
||
|
|
{
|
||
|
|
toolsSaveDouble(f, m.a);
|
||
|
|
toolsSaveDouble(f, m.b);
|
||
|
|
toolsSaveDouble(f, m.c);
|
||
|
|
toolsSaveDouble(f, m.d);
|
||
|
|
toolsSaveDouble(f, m.e);
|
||
|
|
toolsSaveDouble(f, m.f);
|
||
|
|
toolsSaveDouble(f, m.g);
|
||
|
|
toolsSaveDouble(f, m.h);
|
||
|
|
toolsSaveDouble(f, m.i);
|
||
|
|
toolsSaveDouble(f, m.j);
|
||
|
|
toolsSaveDouble(f, m.k);
|
||
|
|
toolsSaveDouble(f, m.l);
|
||
|
|
toolsSaveDouble(f, m.m);
|
||
|
|
toolsSaveDouble(f, m.n);
|
||
|
|
toolsSaveDouble(f, m.o);
|
||
|
|
toolsSaveDouble(f, m.p);
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4Load(FILE* f)
|
||
|
|
{
|
||
|
|
Matrix4 result;
|
||
|
|
|
||
|
|
result.a = toolsLoadDouble(f);
|
||
|
|
result.b = toolsLoadDouble(f);
|
||
|
|
result.c = toolsLoadDouble(f);
|
||
|
|
result.d = toolsLoadDouble(f);
|
||
|
|
result.e = toolsLoadDouble(f);
|
||
|
|
result.f = toolsLoadDouble(f);
|
||
|
|
result.g = toolsLoadDouble(f);
|
||
|
|
result.h = toolsLoadDouble(f);
|
||
|
|
result.i = toolsLoadDouble(f);
|
||
|
|
result.j = toolsLoadDouble(f);
|
||
|
|
result.k = toolsLoadDouble(f);
|
||
|
|
result.l = toolsLoadDouble(f);
|
||
|
|
result.m = toolsLoadDouble(f);
|
||
|
|
result.n = toolsLoadDouble(f);
|
||
|
|
result.o = toolsLoadDouble(f);
|
||
|
|
result.p = toolsLoadDouble(f);
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Translate(Vector3 v1, double x, double y, double z)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = v1.x + x;
|
||
|
|
result.y = v1.y + y;
|
||
|
|
result.z = v1.z + z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Add(Vector3 v1, Vector3 v2)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = v1.x + v2.x;
|
||
|
|
result.y = v1.y + v2.y;
|
||
|
|
result.z = v1.z + v2.z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Sub(Vector3 v1, Vector3 v2)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = v1.x - v2.x;
|
||
|
|
result.y = v1.y - v2.y;
|
||
|
|
result.z = v1.z - v2.z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Neg(Vector3 v)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = -v.x;
|
||
|
|
result.y = -v.y;
|
||
|
|
result.z = -v.z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Scale(Vector3 v, double scale)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = v.x * scale;
|
||
|
|
result.y = v.y * scale;
|
||
|
|
result.z = v.z * scale;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
double v3Norm(Vector3 v)
|
||
|
|
{
|
||
|
|
return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Normalize(Vector3 v)
|
||
|
|
{
|
||
|
|
double norm = v3Norm(v);
|
||
|
|
if (norm == 0.0)
|
||
|
|
{
|
||
|
|
return VECTOR_ZERO;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
return v3Scale(v, 1.0 / norm);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
double v3Dot(Vector3 v1, Vector3 v2)
|
||
|
|
{
|
||
|
|
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 v3Cross(Vector3 v1, Vector3 v2)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = v1.y * v2.z - v1.z * v2.y;
|
||
|
|
result.y = v1.z * v2.x - v1.x * v2.z;
|
||
|
|
result.z = v1.x * v2.y - v1.y * v2.x;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewIdentity()
|
||
|
|
{
|
||
|
|
Matrix4 result;
|
||
|
|
result.a = result.f = result.k = result.p = 1.0;
|
||
|
|
result.b = result.c = result.d = result.e = result.g = result.h = 0.0;
|
||
|
|
result.i = result.j = result.l = result.m = result.n = result.o = 0.0;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4Mult(Matrix4 m1, Matrix4 m2)
|
||
|
|
{
|
||
|
|
Matrix4 result;
|
||
|
|
result.a = m1.a * m2.a + m1.b * m2.e + m1.c * m2.i + m1.d * m2.m;
|
||
|
|
result.b = m1.a * m2.b + m1.b * m2.f + m1.c * m2.j + m1.d * m2.n;
|
||
|
|
result.c = m1.a * m2.c + m1.b * m2.g + m1.c * m2.k + m1.d * m2.o;
|
||
|
|
result.d = m1.a * m2.d + m1.b * m2.h + m1.c * m2.l + m1.d * m2.p;
|
||
|
|
result.e = m1.e * m2.a + m1.f * m2.e + m1.g * m2.i + m1.h * m2.m;
|
||
|
|
result.f = m1.e * m2.b + m1.f * m2.f + m1.g * m2.j + m1.h * m2.n;
|
||
|
|
result.g = m1.e * m2.c + m1.f * m2.g + m1.g * m2.k + m1.h * m2.o;
|
||
|
|
result.h = m1.e * m2.d + m1.f * m2.h + m1.g * m2.l + m1.h * m2.p;
|
||
|
|
result.i = m1.i * m2.a + m1.j * m2.e + m1.k * m2.i + m1.l * m2.m;
|
||
|
|
result.j = m1.i * m2.b + m1.j * m2.f + m1.k * m2.j + m1.l * m2.n;
|
||
|
|
result.k = m1.i * m2.c + m1.j * m2.g + m1.k * m2.k + m1.l * m2.o;
|
||
|
|
result.l = m1.i * m2.d + m1.j * m2.h + m1.k * m2.l + m1.l * m2.p;
|
||
|
|
result.m = m1.m * m2.a + m1.n * m2.e + m1.o * m2.i + m1.p * m2.m;
|
||
|
|
result.n = m1.m * m2.b + m1.n * m2.f + m1.o * m2.j + m1.p * m2.n;
|
||
|
|
result.o = m1.m * m2.c + m1.n * m2.g + m1.o * m2.k + m1.p * m2.o;
|
||
|
|
result.p = m1.m * m2.d + m1.n * m2.h + m1.o * m2.l + m1.p * m2.p;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 m4MultPoint(Matrix4 m, Vector3 v)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
result.x = m.a * v.x + m.b * v.y + m.c * v.z + m.d;
|
||
|
|
result.y = m.e * v.x + m.f * v.y + m.g * v.z + m.h;
|
||
|
|
result.z = m.i * v.x + m.j * v.y + m.k * v.z + m.l;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector3 m4Transform(Matrix4 m, Vector3 v)
|
||
|
|
{
|
||
|
|
Vector3 result;
|
||
|
|
double w;
|
||
|
|
result.x = m.a * v.x + m.b * v.y + m.c * v.z + m.d;
|
||
|
|
result.y = m.e * v.x + m.f * v.y + m.g * v.z + m.h;
|
||
|
|
result.z = m.i * v.x + m.j * v.y + m.k * v.z + m.l;
|
||
|
|
w = m.m * v.x + m.n * v.y + m.o * v.z + m.p;
|
||
|
|
if (w != 0.0)
|
||
|
|
{
|
||
|
|
result.x /= w;
|
||
|
|
result.y /= w;
|
||
|
|
result.z /= w;
|
||
|
|
}
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4Transpose(Matrix4 m)
|
||
|
|
{
|
||
|
|
Matrix4 result;
|
||
|
|
result.a = m.a;
|
||
|
|
result.e = m.b;
|
||
|
|
result.i = m.c;
|
||
|
|
result.m = m.d;
|
||
|
|
result.b = m.e;
|
||
|
|
result.f = m.f;
|
||
|
|
result.j = m.g;
|
||
|
|
result.n = m.h;
|
||
|
|
result.c = m.i;
|
||
|
|
result.g = m.j;
|
||
|
|
result.k = m.k;
|
||
|
|
result.o = m.l;
|
||
|
|
result.d = m.m;
|
||
|
|
result.h = m.n;
|
||
|
|
result.l = m.o;
|
||
|
|
result.p = m.p;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewScale(double x, double y, double z)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
result.a = x;
|
||
|
|
result.f = y;
|
||
|
|
result.k = z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewTranslate(double x, double y, double z)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
result.d = x;
|
||
|
|
result.h = y;
|
||
|
|
result.l = z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateX(double angle)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double s = sin(angle);
|
||
|
|
double c = cos(angle);
|
||
|
|
result.f = result.k = c;
|
||
|
|
result.g = -s;
|
||
|
|
result.j = s;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateY(double angle)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double s = sin(angle);
|
||
|
|
double c = cos(angle);
|
||
|
|
result.a = result.k = c;
|
||
|
|
result.c = s;
|
||
|
|
result.i = -s;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateZ(double angle)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double s = sin(angle);
|
||
|
|
double c = cos(angle);
|
||
|
|
result.a = result.f = c;
|
||
|
|
result.b = -s;
|
||
|
|
result.e = s;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateAxis(double angle, Vector3 axis)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double s = sin(angle);
|
||
|
|
double c = cos(angle);
|
||
|
|
double c1 = 1.0 - c;
|
||
|
|
axis = v3Normalize(axis);
|
||
|
|
result.a = axis.x * axis.x * c1 + c;
|
||
|
|
result.b = axis.x * axis.y * c1 - axis.z * s;
|
||
|
|
result.c = axis.x * axis.z * c1 + axis.y * s;
|
||
|
|
result.e = axis.y * axis.x * c1 + axis.z * s;
|
||
|
|
result.f = axis.y * axis.y * c1 + c;
|
||
|
|
result.g = axis.y * axis.z * c1 - axis.x * s;
|
||
|
|
result.i = axis.x * axis.z * c1 - axis.y * s;
|
||
|
|
result.j = axis.y * axis.z * c1 + axis.x * s;
|
||
|
|
result.k = axis.z * axis.z * c1 + c;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateEuler(double heading, double attitude, double bank)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double ch = cos(heading);
|
||
|
|
double sh = sin(heading);
|
||
|
|
double ca = cos(attitude);
|
||
|
|
double sa = sin(attitude);
|
||
|
|
double cb = cos(bank);
|
||
|
|
double sb = sin(bank);
|
||
|
|
result.a = ch * ca;
|
||
|
|
result.b = sh * sb - ch * sa * cb;
|
||
|
|
result.c = ch * sa * sb + sh * cb;
|
||
|
|
result.e = sa;
|
||
|
|
result.f = ca * cb;
|
||
|
|
result.g = -ca * sb;
|
||
|
|
result.i = -sh * ca;
|
||
|
|
result.j = sh * sa * cb + ch * sb;
|
||
|
|
result.k = -sh * sa * sb + ch * cb;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewRotateTripleAxis(Vector3 x, Vector3 y, Vector3 z)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
result.a = x.x;
|
||
|
|
result.b = y.x;
|
||
|
|
result.c = z.x;
|
||
|
|
result.e = x.y;
|
||
|
|
result.f = y.y;
|
||
|
|
result.g = z.y;
|
||
|
|
result.i = x.z;
|
||
|
|
result.j = y.z;
|
||
|
|
result.k = z.z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewLookAt(Vector3 eye, Vector3 at, Vector3 up)
|
||
|
|
{
|
||
|
|
Vector3 z = v3Normalize(v3Sub(eye, at));
|
||
|
|
Vector3 x = v3Normalize(v3Cross(up, z));
|
||
|
|
Vector3 y = v3Cross(z, x);
|
||
|
|
Matrix4 result = m4NewRotateTripleAxis(x, y, v3Neg(z));
|
||
|
|
result.d = eye.x;
|
||
|
|
result.h = eye.y;
|
||
|
|
result.l = eye.z;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4NewPerspective(double fov_y, double aspect, double near, double far)
|
||
|
|
{
|
||
|
|
Matrix4 result = m4NewIdentity();
|
||
|
|
double f = 1 / tan(fov_y / 2.0);
|
||
|
|
result.a = f / aspect;
|
||
|
|
result.f = f;
|
||
|
|
result.k = (far + near) / (near - far);
|
||
|
|
result.l = 2.0 * far * near / (near - far);
|
||
|
|
result.o = -1.0;
|
||
|
|
result.p = 0.0;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
double m4Determinant(Matrix4 m)
|
||
|
|
{
|
||
|
|
return ((m.a * m.f - m.e * m.b)
|
||
|
|
* (m.k * m.p - m.o * m.l)
|
||
|
|
- (m.a * m.j - m.i * m.b)
|
||
|
|
* (m.g * m.p - m.o * m.h)
|
||
|
|
+ (m.a * m.n - m.m * m.b)
|
||
|
|
* (m.g * m.l - m.k * m.h)
|
||
|
|
+ (m.e * m.j - m.i * m.f)
|
||
|
|
* (m.c * m.p - m.o * m.d)
|
||
|
|
- (m.e * m.n - m.m * m.f)
|
||
|
|
* (m.c * m.l - m.k * m.d)
|
||
|
|
+ (m.i * m.n - m.m * m.j)
|
||
|
|
* (m.c * m.h - m.g * m.d));
|
||
|
|
}
|
||
|
|
|
||
|
|
Matrix4 m4Inverse(Matrix4 m)
|
||
|
|
{
|
||
|
|
Matrix4 result;
|
||
|
|
double d = m4Determinant(m);
|
||
|
|
|
||
|
|
if (fabs(d) < 0.00001)
|
||
|
|
{
|
||
|
|
return m4NewIdentity();
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
d = 1.0 / d;
|
||
|
|
|
||
|
|
result.a = d * (m.f * (m.k * m.p - m.o * m.l) + m.j * (m.o * m.h - m.g * m.p) + m.n * (m.g * m.l - m.k * m.h));
|
||
|
|
result.e = d * (m.g * (m.i * m.p - m.m * m.l) + m.k * (m.m * m.h - m.e * m.p) + m.o * (m.e * m.l - m.i * m.h));
|
||
|
|
result.i = d * (m.h * (m.i * m.n - m.m * m.j) + m.l * (m.m * m.f - m.e * m.n) + m.p * (m.e * m.j - m.i * m.f));
|
||
|
|
result.m = d * (m.e * (m.n * m.k - m.j * m.o) + m.i * (m.f * m.o - m.n * m.g) + m.m * (m.j * m.g - m.f * m.k));
|
||
|
|
|
||
|
|
result.b = d * (m.j * (m.c * m.p - m.o * m.d) + m.n * (m.k * m.d - m.c * m.l) + m.b * (m.o * m.l - m.k * m.p));
|
||
|
|
result.f = d * (m.k * (m.a * m.p - m.m * m.d) + m.o * (m.i * m.d - m.a * m.l) + m.c * (m.m * m.l - m.i * m.p));
|
||
|
|
result.j = d * (m.l * (m.a * m.n - m.m * m.b) + m.p * (m.i * m.b - m.a * m.j) + m.d * (m.m * m.j - m.i * m.n));
|
||
|
|
result.n = d * (m.i * (m.n * m.c - m.b * m.o) + m.m * (m.b * m.k - m.j * m.c) + m.a * (m.j * m.o - m.n * m.k));
|
||
|
|
|
||
|
|
result.c = d * (m.n * (m.c * m.h - m.g * m.d) + m.b * (m.g * m.p - m.o * m.h) + m.f * (m.o * m.d - m.c * m.p));
|
||
|
|
result.g = d * (m.o * (m.a * m.h - m.e * m.d) + m.c * (m.e * m.p - m.m * m.h) + m.g * (m.m * m.d - m.a * m.p));
|
||
|
|
result.k = d * (m.p * (m.a * m.f - m.e * m.b) + m.d * (m.e * m.n - m.m * m.f) + m.h * (m.m * m.b - m.a * m.n));
|
||
|
|
result.o = d * (m.m * (m.f * m.c - m.b * m.g) + m.a * (m.n * m.g - m.f * m.o) + m.e * (m.b * m.o - m.n * m.c));
|
||
|
|
|
||
|
|
result.d = d * (m.b * (m.k * m.h - m.g * m.l) + m.f * (m.c * m.l - m.k * m.d) + m.j * (m.g * m.d - m.c * m.h));
|
||
|
|
result.h = d * (m.c * (m.i * m.h - m.e * m.l) + m.g * (m.a * m.l - m.i * m.d) + m.k * (m.e * m.d - m.a * m.h));
|
||
|
|
result.l = d * (m.d * (m.i * m.f - m.e * m.j) + m.h * (m.a * m.j - m.i * m.b) + m.l * (m.e * m.b - m.a * m.f));
|
||
|
|
result.p = d * (m.a * (m.f * m.k - m.j * m.g) + m.e * (m.j * m.c - m.b * m.k) + m.i * (m.b * m.g - m.f * m.c));
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|