165 lines
3.2 KiB
C
165 lines
3.2 KiB
C
#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 = sqrtf(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 sqrtf(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;
|
|
}
|