295 lines
6.7 KiB
C
295 lines
6.7 KiB
C
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <GL/glew.h>
|
|
#include <string.h>
|
|
|
|
#include "matrixMath.h"
|
|
|
|
// MATRICES IN COLUMN MAJOR
|
|
|
|
void vec3Zero(vec3* out) {
|
|
for (int i = 0; i < 3; i++) {
|
|
((GLfloat*)out)[i] = 0;
|
|
}
|
|
}
|
|
|
|
void vec3Add(vec3* out, vec3* a, vec3* b) {
|
|
for (int i = 0; i < 3; i++) {
|
|
((GLfloat*)out)[i] = ((GLfloat*)a)[i] + ((GLfloat*)b)[i];
|
|
}
|
|
}
|
|
|
|
void vec3Multiply(vec3* out, vec3* a, GLfloat x) {
|
|
for (int i = 0; i < 3; i++) {
|
|
((GLfloat*)out)[i] = ((GLfloat*)a)[i] * x;
|
|
}
|
|
}
|
|
|
|
void vec3Subtract(vec3* out, vec3* a, vec3* b) {
|
|
vec3 minusB;
|
|
vec3Multiply(&minusB, b, -1);
|
|
vec3Add(out, a, &minusB);
|
|
}
|
|
|
|
void vec3Cross(vec3* out, vec3* a, vec3* b) {
|
|
vec3 result;
|
|
result.x = a->y * b->z - a->z * b->y;
|
|
result.y = a->z * b->x - a->x * b->z;
|
|
result.z = a->x * b->y - a->y * b->x;
|
|
memcpy(out, &result, sizeof(vec3));
|
|
}
|
|
|
|
GLfloat vec3Length(vec3* a) {
|
|
return (GLfloat)sqrt(a->x * a->x + a->y * a->y + a->z * a->z);
|
|
}
|
|
|
|
GLfloat vec3Dot(vec3* a, vec3* b) {
|
|
return a->x * b->x + a->y * b->y + a->z * b->z;
|
|
}
|
|
|
|
void vec3Normalise(vec3* out, vec3* a) {
|
|
vec3Multiply(out, a, 1 / vec3Length(a));
|
|
}
|
|
// CREATE 4x4 IDENTITY MATRIX
|
|
void identity(mat4* out) {
|
|
for (int i = 0; i < 16; i++) {
|
|
((GLfloat*)out)[i] = (i % 4 == i / 4);
|
|
}
|
|
}
|
|
|
|
// CREATE 4x4 TRANSLATION MATRIX
|
|
void translation(mat4* out, vec3* v) {
|
|
identity(out);
|
|
out->m03 = v->x;
|
|
out->m13 = v->y;
|
|
out->m23 = v->z;
|
|
}
|
|
|
|
// CREATE 4x4 SCALING MATRIX
|
|
void scaling(mat4* out, vec3* v) {
|
|
identity(out);
|
|
out->m00 = v->x;
|
|
out->m11 = v->y;
|
|
out->m22 = v->z;
|
|
}
|
|
|
|
// CREATE 4x4 ROTATION MATRIX AROUND Z AXIS
|
|
/* cos a -sin a 0 0
|
|
* sin a cos a 0 0
|
|
* 0 0 1 0
|
|
* 0 0 0 1
|
|
*/
|
|
void rotationZ(mat4* out, GLfloat angle) {
|
|
identity(out);
|
|
out->m00 = cos(angle);
|
|
out->m10 = sin(angle);
|
|
out->m01 = -sin(angle);
|
|
out->m11 = cos(angle);
|
|
}
|
|
|
|
// CREATE 4x4 ROTATION MATRIX AROUND Y AXIS
|
|
/* cos a 0 sin a 0
|
|
* 0 1 0 0
|
|
* -sin a 0 cos a 0
|
|
* 0 0 0 1
|
|
*/
|
|
void rotationY(mat4* out, GLfloat angle) {
|
|
identity(out);
|
|
out->m00 = cos(angle);
|
|
out->m20 = -sin(angle);
|
|
out->m02 = sin(angle);
|
|
out->m22 = cos(angle);
|
|
}
|
|
|
|
// CREATE 4x4 ROTATION MATRIX AROUND Y AXIS
|
|
void rotationX(mat4* out, GLfloat angle) {
|
|
identity(out);
|
|
out->m11 = cos(angle);
|
|
out->m21 = sin(angle);
|
|
out->m12 = -sin(angle);
|
|
out->m22 = cos(angle);
|
|
}
|
|
|
|
// MULTIPLY ANY TO MATRICES
|
|
void multiplyAny(GLfloat* out, GLfloat* A, GLfloat* B, int wA, int hA, int wB) {
|
|
int sizeOut = hA * wB;
|
|
GLfloat* result = (GLfloat*) malloc(sizeOut * sizeof(GLfloat));
|
|
for (int i = 0; i < sizeOut; i++) {
|
|
result[i] = 0;
|
|
for (int j = 0; j < wA; j++) {
|
|
result[i] += A[j * hA + i % hA] * B[j + i / hA * wB];
|
|
}
|
|
}
|
|
memcpy(out, result, sizeOut * sizeof(GLfloat));
|
|
free(result);
|
|
result = NULL;
|
|
}
|
|
|
|
// MULTIPLY TWO 4x4 MATRICES
|
|
void multiply(mat4* out, mat4* A, mat4* B) {
|
|
multiplyAny((GLfloat*)out, (GLfloat*)A, (GLfloat*)B, 4, 4, 4);
|
|
}
|
|
|
|
// MULTIPLY in WITH TRANSLATION MATRIX OF v
|
|
void translate(mat4* out, mat4* in, vec3* v) {
|
|
mat4 translationMatrix;
|
|
translation(&translationMatrix, v);
|
|
multiply(out, &translationMatrix, in);
|
|
}
|
|
|
|
// MULTIPLY in WITH SCALING MATRIX OF v
|
|
void scale(mat4* out, mat4* in, vec3* v) {
|
|
mat4 scalingMatrix;
|
|
scaling(&scalingMatrix, v);
|
|
multiply(out, &scalingMatrix, in);
|
|
}
|
|
|
|
// MULTIPLY in WITH ROTATION MATRIX OF a AROUND Z AXIS
|
|
void rotateZ(mat4* out, mat4* in, GLfloat angle) {
|
|
mat4 rotationMatrix;
|
|
rotationZ(&rotationMatrix, angle);
|
|
multiply(out, &rotationMatrix, in);
|
|
}
|
|
// MULTIPLY in WITH ROTATION MATRIX OF a AROUND Y AXIS
|
|
void rotateY(mat4* out, mat4* in, GLfloat angle) {
|
|
mat4 rotationMatrix;
|
|
rotationY(&rotationMatrix, angle);
|
|
multiply(out, &rotationMatrix, in);
|
|
}
|
|
// MULTIPLY in WITH ROTATION MATRIX OF a AROUND X AXIS
|
|
void rotateX(mat4* out, mat4* in, GLfloat angle) {
|
|
mat4 rotationMatrix;
|
|
rotationX(&rotationMatrix, angle);
|
|
multiply(out, &rotationMatrix, in);
|
|
}
|
|
|
|
void transposeAny(GLfloat* out, GLfloat* in, int w, int h) {
|
|
int size = w * h;
|
|
GLfloat* result = (GLfloat*) malloc(size * sizeof(GLfloat));
|
|
|
|
for (int i = 0; i < size; i++) {
|
|
result[i] = in[(i % w) * h + i / w];
|
|
}
|
|
|
|
memcpy(out, result, size * sizeof(GLfloat));
|
|
free(result);
|
|
result = NULL;
|
|
}
|
|
|
|
|
|
void transpose(mat4* out, mat4* in) {
|
|
transposeAny((GLfloat*)out, (GLfloat*)in, 4, 4);
|
|
}
|
|
|
|
void printAny(GLfloat* M, int w, int h) {
|
|
GLfloat* transposed = (GLfloat*) malloc(w * h * sizeof(GLfloat));
|
|
transposeAny(transposed, M, w, h);
|
|
|
|
for (int i = 0; i < h; i++) {
|
|
for (int j = 0; j < w; j++) {
|
|
printf("%.4f ", transposed[i * w + j]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
free(transposed);
|
|
transposed = NULL;
|
|
}
|
|
|
|
void vec3Print(vec3* a) {
|
|
printAny((GLfloat*)a, 1, 3);
|
|
}
|
|
|
|
void mat4Print(mat4* m) {
|
|
printAny((GLfloat*)m, 4, 4);
|
|
}
|
|
|
|
void mat3Print(mat3* m) {
|
|
printAny((GLfloat*)m, 3, 3);
|
|
}
|
|
|
|
void mat3From4(mat3* out, mat4* in) {
|
|
memcpy(&out->m00, &in->m00, sizeof(vec3));
|
|
memcpy(&out->m01, &in->m01, sizeof(vec3));
|
|
memcpy(&out->m02, &in->m02, sizeof(vec3));
|
|
}
|
|
|
|
void mat3Transpose(mat3* out, mat3* in) {
|
|
transposeAny((GLfloat*)out, (GLfloat*)in, 3, 3);
|
|
}
|
|
|
|
/**
|
|
* a - m00 b - m01 c - m02
|
|
* d - m10 e - m11 f - m12
|
|
* g - m20 h - m21 i - m22
|
|
*/
|
|
void mat3Minor(mat3* out, mat3* in) {
|
|
mat3 result;
|
|
|
|
result.m00 = in->m11 * in->m22 - in->m21 * in->m12;
|
|
result.m01 = in->m10 * in->m22 - in->m20 * in->m12;
|
|
result.m02 = in->m10 * in->m21 - in->m20 * in->m11;
|
|
|
|
result.m10 = in->m01 * in->m22 - in->m21 * in->m02;
|
|
result.m11 = in->m00 * in->m22 - in->m20 * in->m02;
|
|
result.m12 = in->m00 * in->m21 - in->m20 * in->m01;
|
|
|
|
result.m20 = in->m01 * in->m12 - in->m11 * in->m02;
|
|
result.m21 = in->m00 * in->m12 - in->m10 * in->m02;
|
|
result.m22 = in->m00 * in->m11 - in->m10 * in->m01;
|
|
|
|
memcpy(out, &result, sizeof(mat3));
|
|
}
|
|
|
|
void mat3Cofactor(mat3* out, mat3* in) {
|
|
mat3 result;
|
|
mat3Minor(out, in);
|
|
|
|
out->m01 *= -1;
|
|
out->m10 *= -1;
|
|
out->m21 *= -1;
|
|
out->m12 *= -1;
|
|
}
|
|
|
|
void mat3Adjoint(mat3* out, mat3* in) {
|
|
mat3Cofactor(out, in);
|
|
mat3Transpose(out, out);
|
|
}
|
|
|
|
void mat3MultiplyScalar(mat3* out, mat3* in, GLfloat x) {
|
|
for (int i = 0; i < 9; i++) {
|
|
((GLfloat*)out)[i] = ((GLfloat*)in)[i] * x;
|
|
}
|
|
}
|
|
|
|
GLfloat mat3Determinant(mat3* M) {
|
|
return
|
|
M->m00 * M->m11 * M->m22
|
|
+ M->m01 * M->m12 * M->m20
|
|
+ M->m02 * M->m10 * M->m21
|
|
|
|
- M->m20 * M->m11 * M->m02
|
|
- M->m21 * M->m12 * M->m00
|
|
- M->m22 * M->m10 * M->m01
|
|
;
|
|
}
|
|
|
|
void mat3Inverse(mat3* out, mat3* in) {
|
|
mat3 result;
|
|
mat3Adjoint(&result, in);
|
|
mat3MultiplyScalar(&result, &result, 1 / mat3Determinant(in));
|
|
|
|
memcpy(out, &result, sizeof(mat3));
|
|
}
|
|
|
|
GLfloat sumDiffAny(GLfloat* A, GLfloat* B, int w, int h) {
|
|
GLfloat result = 0;
|
|
for (int i = 0; i < w * h; i++) {
|
|
result += fabs(A[i] - B[i]);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
GLfloat mat3SumDiff(mat3* A, mat3* B) {
|
|
return sumDiffAny((GLfloat*)A, (GLfloat*)B, 3, 3);
|
|
} |