From 0373255087377122eeb10e006ffb8aa1b57e611c Mon Sep 17 00:00:00 2001 From: rgl Date: Mon, 3 Feb 2020 22:42:28 +0100 Subject: after a year or so of work, i dare create a proper repo. --- libgeometry/matrix.c | 348 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 348 insertions(+) create mode 100644 libgeometry/matrix.c (limited to 'libgeometry/matrix.c') diff --git a/libgeometry/matrix.c b/libgeometry/matrix.c new file mode 100644 index 0000000..b7efc84 --- /dev/null +++ b/libgeometry/matrix.c @@ -0,0 +1,348 @@ +#include +#include +#include "../geometry.h" + +/* 2D */ + +void +identity(Matrix m) +{ + memset(m, 0, 3*3*sizeof(double)); + m[0][0] = m[1][1] = m[2][2] = 1; +} + +void +addm(Matrix a, Matrix b) +{ + int i, j; + + for(i = 0; i < 3; i++) + for(j = 0; j < 3; j++) + a[i][j] += b[i][j]; +} + +void +subm(Matrix a, Matrix b) +{ + int i, j; + + for(i = 0; i < 3; i++) + for(j = 0; j < 3; j++) + a[i][j] -= b[i][j]; +} + +void +mulm(Matrix a, Matrix b) +{ + int i, j, k; + Matrix tmp; + + for(i = 0; i < 3; i++) + for(j = 0; j < 3; j++){ + tmp[i][j] = 0; + for(k = 0; k < 3; k++) + tmp[i][j] += a[i][k]*b[k][j]; + } + memmove(a, tmp, 3*3*sizeof(double)); +} + +void +smulm(Matrix m, double s) +{ + int i, j; + + for(i = 0; i < 3; i++) + for(j = 0; j < 3; j++) + m[i][j] *= s; +} + +void +transposem(Matrix m) +{ + int i, j; + double tmp; + + for(i = 0; i < 3; i++) + for(j = i; j < 3; j++){ + tmp = m[i][j]; + m[i][j] = m[j][i]; + m[j][i] = tmp; + } +} + +double +detm(Matrix m) +{ + return m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1])+ + m[0][1]*(m[1][2]*m[2][0] - m[1][0]*m[2][2])+ + m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]); +} + +double +tracem(Matrix m) +{ + return m[0][0] + m[1][1] + m[2][2]; +} + +void +adjm(Matrix m) +{ + Matrix tmp; + + tmp[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; + tmp[0][1] = -m[0][1]*m[2][2] + m[0][2]*m[2][1]; + tmp[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; + tmp[1][0] = -m[1][0]*m[2][2] + m[1][2]*m[2][0]; + tmp[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; + tmp[1][2] = -m[0][0]*m[1][2] + m[0][2]*m[1][0]; + tmp[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; + tmp[2][1] = -m[0][0]*m[2][1] + m[0][1]*m[2][0]; + tmp[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]; + memmove(m, tmp, 3*3*sizeof(double)); +} + +/* Cayley-Hamilton */ +//void +//invertm(Matrix m) +//{ +// Matrix m², r; +// double det, trm, trm²; +// +// det = detm(m); +// if(det == 0) +// return; +// trm = tracem(m); +// memmove(m², m, 3*3*sizeof(double)); +// mulm(m², m²); +// trm² = tracem(m²); +// identity(r); +// smulm(r, (trm*trm - trm²)/2); +// smulm(m, trm); +// subm(r, m); +// addm(r, m²); +// smulm(r, 1/det); +// memmove(m, r, 3*3*sizeof(double)); +//} + +/* Cramer's */ +void +invm(Matrix m) +{ + double det; + + det = detm(m); + if(det == 0) + return; /* singular matrices are not invertible */ + adjm(m); + smulm(m, 1/det); +} + +Point2 +xform(Point2 p, Matrix m) +{ + return (Point2){ + p.x*m[0][0] + p.y*m[0][1] + p.w*m[0][2], + p.x*m[1][0] + p.y*m[1][1] + p.w*m[1][2], + p.x*m[2][0] + p.y*m[2][1] + p.w*m[2][2] + }; +} + +/* 3D */ + +void +identity3(Matrix3 m) +{ + memset(m, 0, 4*4*sizeof(double)); + m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1; +} + +void +addm3(Matrix3 a, Matrix3 b) +{ + int i, j; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + a[i][j] += b[i][j]; +} + +void +subm3(Matrix3 a, Matrix3 b) +{ + int i, j; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + a[i][j] -= b[i][j]; +} + +void +mulm3(Matrix3 a, Matrix3 b) +{ + int i, j, k; + Matrix3 tmp; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++){ + tmp[i][j] = 0; + for(k = 0; k < 4; k++) + tmp[i][j] += a[i][k]*b[k][j]; + } + memmove(a, tmp, 4*4*sizeof(double)); +} + +void +smulm3(Matrix3 m, double s) +{ + int i, j; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + m[i][j] *= s; +} + +void +transposem3(Matrix3 m) +{ + int i, j; + double tmp; + + for(i = 0; i < 4; i++) + for(j = i; j < 4; j++){ + tmp = m[i][j]; + m[i][j] = m[j][i]; + m[j][i] = tmp; + } +} + +double +detm3(Matrix3 m) +{ + return m[0][0]*(m[1][1]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+ + m[1][2]*(m[2][3]*m[3][1] - m[2][1]*m[3][3])+ + m[1][3]*(m[2][1]*m[3][2] - m[2][2]*m[3][1])) + -m[0][1]*(m[1][0]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+ + m[1][2]*(m[2][3]*m[3][0] - m[2][0]*m[3][3])+ + m[1][3]*(m[2][0]*m[3][2] - m[2][2]*m[3][0])) + +m[0][2]*(m[1][0]*(m[2][1]*m[3][3] - m[2][3]*m[3][1])+ + m[1][1]*(m[2][3]*m[3][0] - m[2][0]*m[3][3])+ + m[1][3]*(m[2][0]*m[3][1] - m[2][1]*m[3][0])) + -m[0][3]*(m[1][0]*(m[2][1]*m[3][2] - m[2][2]*m[3][1])+ + m[1][1]*(m[2][2]*m[3][0] - m[2][0]*m[3][2])+ + m[1][2]*(m[2][0]*m[3][1] - m[2][1]*m[3][0])); +} + +double +tracem3(Matrix3 m) +{ + return m[0][0] + m[1][1] + m[2][2] + m[3][3]; +} + +void +adjm3(Matrix3 m) +{ + Matrix3 tmp; + + tmp[0][0]=m[1][1]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+ + m[2][1]*(m[1][3]*m[3][2] - m[1][2]*m[3][3])+ + m[3][1]*(m[1][2]*m[2][3] - m[1][3]*m[2][2]); + tmp[0][1]=m[0][1]*(m[2][3]*m[3][2] - m[2][2]*m[3][3])+ + m[2][1]*(m[0][2]*m[3][3] - m[0][3]*m[3][2])+ + m[3][1]*(m[0][3]*m[2][2] - m[0][2]*m[2][3]); + tmp[0][2]=m[0][1]*(m[1][2]*m[3][3] - m[1][3]*m[3][2])+ + m[1][1]*(m[0][3]*m[3][2] - m[0][2]*m[3][3])+ + m[3][1]*(m[0][2]*m[1][3] - m[0][3]*m[1][2]); + tmp[0][3]=m[0][1]*(m[1][3]*m[2][2] - m[1][2]*m[2][3])+ + m[1][1]*(m[0][2]*m[2][3] - m[0][3]*m[2][2])+ + m[2][1]*(m[0][3]*m[1][2] - m[0][2]*m[1][3]); + tmp[1][0]=m[1][0]*(m[2][3]*m[3][2] - m[2][2]*m[3][3])+ + m[2][0]*(m[1][2]*m[3][3] - m[1][3]*m[3][2])+ + m[3][0]*(m[1][3]*m[2][2] - m[1][2]*m[2][3]); + tmp[1][1]=m[0][0]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+ + m[2][0]*(m[0][3]*m[3][2] - m[0][2]*m[3][3])+ + m[3][0]*(m[0][2]*m[2][3] - m[0][3]*m[2][2]); + tmp[1][2]=m[0][0]*(m[1][3]*m[3][2] - m[1][2]*m[3][3])+ + m[1][0]*(m[0][2]*m[3][3] - m[0][3]*m[3][2])+ + m[3][0]*(m[0][3]*m[1][2] - m[0][2]*m[1][3]); + tmp[1][3]=m[0][0]*(m[1][2]*m[2][3] - m[1][3]*m[2][2])+ + m[1][0]*(m[0][3]*m[2][2] - m[0][2]*m[2][3])+ + m[2][0]*(m[0][2]*m[1][3] - m[0][3]*m[1][2]); + tmp[2][0]=m[1][0]*(m[2][1]*m[3][3] - m[2][3]*m[3][1])+ + m[2][0]*(m[1][3]*m[3][1] - m[1][1]*m[3][3])+ + m[3][0]*(m[1][1]*m[2][3] - m[1][3]*m[2][1]); + tmp[2][1]=m[0][0]*(m[2][3]*m[3][1] - m[2][1]*m[3][3])+ + m[2][0]*(m[0][1]*m[3][3] - m[0][3]*m[3][1])+ + m[3][0]*(m[0][3]*m[2][1] - m[0][1]*m[2][3]); + tmp[2][2]=m[0][0]*(m[1][1]*m[3][3] - m[1][3]*m[3][1])+ + m[1][0]*(m[0][3]*m[3][1] - m[0][1]*m[3][3])+ + m[3][0]*(m[0][1]*m[1][3] - m[0][3]*m[1][1]); + tmp[2][3]=m[0][0]*(m[1][3]*m[2][1] - m[1][1]*m[2][3])+ + m[1][0]*(m[0][1]*m[2][3] - m[0][3]*m[2][1])+ + m[2][0]*(m[0][3]*m[1][1] - m[0][1]*m[1][3]); + tmp[3][0]=m[1][0]*(m[2][2]*m[3][1] - m[2][1]*m[3][2])+ + m[2][0]*(m[1][1]*m[3][2] - m[1][2]*m[3][1])+ + m[3][0]*(m[1][2]*m[2][1] - m[1][1]*m[2][2]); + tmp[3][1]=m[0][0]*(m[2][1]*m[3][2] - m[2][2]*m[3][1])+ + m[2][0]*(m[0][2]*m[3][1] - m[0][1]*m[3][2])+ + m[3][0]*(m[0][1]*m[2][2] - m[0][2]*m[2][1]); + tmp[3][2]=m[0][0]*(m[1][2]*m[3][1] - m[1][1]*m[3][2])+ + m[1][0]*(m[0][1]*m[3][2] - m[0][2]*m[3][1])+ + m[3][0]*(m[0][2]*m[1][1] - m[0][1]*m[1][2]); + tmp[3][3]=m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1])+ + m[1][0]*(m[0][2]*m[2][1] - m[0][1]*m[2][2])+ + m[2][0]*(m[0][1]*m[1][2] - m[0][2]*m[1][1]); + memmove(m, tmp, 4*4*sizeof(double)); +} + +/* Cayley-Hamilton */ +//void +//invertm3(Matrix3 m) +//{ +// Matrix3 m², m³, r; +// double det, trm, trm², trm³; +// +// det = detm3(m); +// if(det == 0) +// return; +// trm = tracem3(m); +// memmove(m³, m, 4*4*sizeof(double)); +// mulm(m³, m³); +// mulm(m³, m); +// trm³ = tracem3(m³); +// memmove(m², m, 4*4*sizeof(double)); +// mulm(m², m²); +// trm² = tracem3(m²); +// identity3(r); +// smulm3(r, (trm*trm*trm - 3*trm*trm² + 2*trm³)/6); +// smulm3(m, (trm*trm - trm²)/2); +// smulm3(m², trm); +// subm(r, m); +// addm(r, m²); +// subm(r, m³); +// smulm(r, 1/det); +// memmove(m, r, 4*4*sizeof(double)); +//} + +/* Cramer's */ +void +invm3(Matrix3 m) +{ + double det; + + det = detm3(m); + if(det == 0) + return; /* singular matrices are not invertible */ + adjm3(m); + smulm3(m, 1/det); +} + +Point3 +xform3(Point3 p, Matrix3 m) +{ + return (Point3){ + p.x*m[0][0] + p.y*m[0][1] + p.z*m[0][2] + p.w*m[0][3], + p.x*m[1][0] + p.y*m[1][1] + p.z*m[1][2] + p.w*m[1][3], + p.x*m[2][0] + p.y*m[2][1] + p.z*m[2][2] + p.w*m[2][3], + p.x*m[3][0] + p.y*m[3][1] + p.z*m[3][2] + p.w*m[3][3], + }; +} -- cgit v1.2.3