120 lines
2 KiB
C
120 lines
2 KiB
C
/*
|
|
* This software is licensed under the terms of the MIT-License
|
|
* See COPYING for further information.
|
|
* ---
|
|
* Copyright (C) 2011, Lukas Weber <laochailan@web.de>
|
|
*/
|
|
|
|
#include "matrix.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
Matrix _identity = {
|
|
{1, 0, 0, 0},
|
|
{0, 1, 0, 0},
|
|
{0, 0, 1, 0},
|
|
{0, 0, 0, 1}
|
|
};
|
|
|
|
void matcpy(Matrix dest, Matrix src) {
|
|
memcpy(dest, src, sizeof(Matrix));
|
|
}
|
|
|
|
void matmul(Matrix dest, Matrix a, Matrix b) {
|
|
int i,j,k;
|
|
for(i = 0; i < 4; i++) {
|
|
for(j = 0; j < 4; j++) {
|
|
dest[i][j] = 0;
|
|
for(k = 0; k < 4; k++) {
|
|
dest[i][j] += a[i][k] * b[k][j];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void matrotate(Matrix md, Matrix ms, float angle, float x, float y, float z) {
|
|
float c = cos(angle);
|
|
float s = sin(angle);
|
|
|
|
Matrix a = {
|
|
{x*x*(1-c)+c, x*y*(1-c)-z*s, x*z*(1-c)+y*s, 0},
|
|
{y*x*(1-c)+z*s, y*y*(1-c)+c, y*z*(1-c)-x*s, 0},
|
|
{x*z*(1-c)-y*s, y*z*(1-c)+x*s, z*z*(1-c)+c, 0},
|
|
{0, 0, 0, 1}
|
|
};
|
|
|
|
matmul(md, ms, a);
|
|
}
|
|
|
|
void matrotatez(Matrix md, Matrix ms, float angle) {
|
|
float c = cos(angle);
|
|
float s = sin(angle);
|
|
|
|
Matrix a = {
|
|
{c, -s, 0, 0},
|
|
{s, c, 0, 0},
|
|
{0, 0, 1, 0},
|
|
{0, 0, 0, 1}
|
|
};
|
|
|
|
matmul(md, ms, a);
|
|
}
|
|
|
|
void mattranslate(Matrix md, Matrix ms, float x, float y, float z) {
|
|
Matrix a = {
|
|
{1, 0, 0, x},
|
|
{0, 1, 0, y},
|
|
{0, 0, 1, z},
|
|
{0, 0, 0, 1}
|
|
};
|
|
|
|
matmul(md, ms, a);
|
|
}
|
|
|
|
void matscale(Matrix md, Matrix ms, float x, float y, float z) {
|
|
Matrix a = {
|
|
{x, 0, 0, 0},
|
|
{0, y, 0, 0},
|
|
{0, 0, z, 0},
|
|
{0, 0, 0, 1}
|
|
};
|
|
|
|
matmul(md, ms, a);
|
|
}
|
|
|
|
void matvec(Matrix m, Vector v) {
|
|
Vector tmp;
|
|
memcpy(tmp, v, sizeof(Vector));
|
|
|
|
int i, k;
|
|
for(i = 0; i < 3; i++) {
|
|
v[i] = 0;
|
|
for(k = 0; k < 4; k++) {
|
|
float t = 1;
|
|
if(k < 3)
|
|
t = tmp[k];
|
|
|
|
v[i] += m[i][k] * t;
|
|
}
|
|
}
|
|
}
|
|
|
|
void matvecv(Matrix m, Vector *v, int n) {
|
|
int i;
|
|
for(i = 0; i < n; i++) {
|
|
matvec(m, v[i]);
|
|
}
|
|
}
|
|
|
|
void normalize(Vector v) {
|
|
int i;
|
|
|
|
float len = length(v);
|
|
|
|
for(i = 0; i < 3; i++)
|
|
v[i] /= len;
|
|
}
|
|
|
|
float length(Vector v) {
|
|
return sqrt(pow(v[0],2) + pow(v[1],2) + pow(v[2],2));
|
|
}
|