summaryrefslogtreecommitdiffstats
path: root/math.c
diff options
context:
space:
mode:
Diffstat (limited to 'math.c')
-rw-r--r--math.c165
1 files changed, 165 insertions, 0 deletions
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 <math.h>
+#include <neofx/math.h>
+#include <neofx/types.h>
+
+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;
+}