From ad0845d0bd7ee0f90c4e73de4950656248d98a3d Mon Sep 17 00:00:00 2001 From: neoraider Date: Fri, 22 Apr 2005 19:51:02 +0000 Subject: Libneofx von Libzoom abgespalten --- math.c | 165 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 math.c (limited to 'math.c') diff --git a/math.c b/math.c new file mode 100644 index 0000000..799de5b --- /dev/null +++ b/math.c @@ -0,0 +1,165 @@ +#include +#include +#include + +float VectorDot(VECTOR v1, VECTOR v2) { + return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; +} + +VECTOR VectorCross(VECTOR v1, VECTOR v2) { + VECTOR vec; + + vec.x = v1.y * v2.z - v1.z * v2.y; + vec.y = v1.z * v2.x - v1.x * v2.z; + vec.z = v1.x * v2.y - v1.y * v2.x; + + return vec; +} + +VECTOR VectorSub(VECTOR v1, VECTOR v2) { + VECTOR vec; + + vec.x = v1.x - v2.x; + vec.y = v1.y - v2.y; + vec.z = v1.z - v2.z; + + return vec; +} + +VECTOR VectorAdd(VECTOR v1, VECTOR v2) { + VECTOR vec; + + vec.x = v1.x + v2.x; + vec.y = v1.y + v2.y; + vec.z = v1.z + v2.z; + + return vec; +} + +VECTOR VectorMul(VECTOR v, float f) { + v.x *= f; + v.y *= f; + v.z *= f; + + return v; +} + +VECTOR VectorNormalize(VECTOR v) { + float f = sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + + v.x /= f; + v.y /= f; + v.z /= f; + + return v; +} + +VECTOR VectorNeg(VECTOR v) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + + return v; +} + +float VectorLength(VECTOR v) { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} + +float VectorLengthSq(VECTOR v) { + return (v.x*v.x + v.y*v.y + v.z*v.z); +} + +int VectorEqual(VECTOR v1, VECTOR v2) { + if(v1.x == v2.x && v1.y == v2.y && v1.z == v2.z) return 1; + return 0; +} + +MATRIX MatrixIdentity() { + MATRIX m = { + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0 + }; + return m; +} + +MATRIX MatrixMul(MATRIX m1, MATRIX m2) { + MATRIX m = { + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0 + }; + short i, j, k; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + for(k = 0; k < 4; k++) + m.m[i][j] += m1.m[i][k] * m2.m[k][j]; + + return m; +} + +MATRIX VectorMatrix(VERTEX p1, VECTOR v1, VERTEX p2, VECTOR v2) { + MATRIX translate; + MATRIX rotate; + VECTOR axis; + float s, c; + + axis = VectorCross(v1, v2); + s = VectorLength(axis); + c = VectorDot(v1, v2); + if(s) axis = VectorMul(axis, 1/s); + + rotate.m[0][0] = (axis.x * axis.x) * (1.0 - c) + c; + rotate.m[0][1] = (axis.x * axis.y) * (1.0 - c) - (axis.z * s); + rotate.m[0][2] = (axis.x * axis.z) * (1.0 - c) + (axis.y * s); + rotate.m[0][3] = 0.0; + + rotate.m[1][0] = (axis.y * axis.x) * (1.0 - c) + (axis.z * s); + rotate.m[1][1] = (axis.y * axis.y) * (1.0 - c) + c; + rotate.m[1][2] = (axis.y * axis.z) * (1.0 - c) - (axis.x * s); + rotate.m[1][3] = 0.0; + + rotate.m[2][0] = (axis.z * axis.x) * (1.0 - c) - (axis.y * s); + rotate.m[2][1] = (axis.z * axis.y) * (1.0 - c) + (axis.x * s); + rotate.m[2][2] = (axis.z * axis.z) * (1.0 - c) + c; + rotate.m[2][3] = 0.0; + + rotate.m[3][0] = 0.0; + rotate.m[3][1] = 0.0; + rotate.m[3][2] = 0.0; + rotate.m[3][3] = 1.0; + + translate = MatrixIdentity(); + + translate.m[3][0] = p1.x; + translate.m[3][1] = p1.y; + translate.m[3][2] = p1.z; + + rotate = MatrixMul(rotate, translate); + + translate.m[3][0] = -p2.x; + translate.m[3][1] = -p2.y; + translate.m[3][2] = -p2.z; + + return MatrixMul(translate, rotate); +} + +VECTOR VectorMatrixMul(VECTOR v, MATRIX m) { + VECTOR r; + float w; + + r.x = v.x*m.m[0][0] + v.y*m.m[1][0] + v.z*m.m[2][0] + m.m[3][0]; + r.y = v.x*m.m[0][1] + v.y*m.m[1][1] + v.z*m.m[2][1] + m.m[3][1]; + r.z = v.x*m.m[0][2] + v.y*m.m[1][2] + v.z*m.m[2][2] + m.m[3][2]; + w = v.x*m.m[0][3] + v.y*m.m[1][3] + v.z*m.m[2][3] + m.m[3][3]; + + r.x /= w; + r.y /= w; + r.z /= w; + + return r; +} -- cgit v1.2.3