diff options
Diffstat (limited to 'libdimension/geometry.c')
| -rw-r--r-- | libdimension/geometry.c | 144 |
1 files changed, 139 insertions, 5 deletions
diff --git a/libdimension/geometry.c b/libdimension/geometry.c index 0eab10b..548aae1 100644 --- a/libdimension/geometry.c +++ b/libdimension/geometry.c @@ -19,15 +19,78 @@ *************************************************************************/ #include "dimension.h" +#include <math.h> /* Construct a vector from x, y, and z. Just for convienence. */ dmnsn_vector -dmnsn_vector_construct(dmnsn_scalar x, dmnsn_scalar y, dmnsn_scalar z) +dmnsn_vector_construct(double x, double y, double z) { dmnsn_vector v = { .x = x, .y = y, .z = z }; return v; } +/* Construct a matrix. */ +dmnsn_matrix +dmnsn_matrix_construct(double a0, double a1, double a2, double a3, + double b0, double b1, double b2, double b3, + double c0, double c1, double c2, double c3, + double d0, double d1, double d2, double d3) +{ + dmnsn_matrix m = { .n00 = a0, .n01 = a1, .n02 = a2, .n03 = a3, + .n10 = b0, .n11 = b1, .n12 = b2, .n13 = b3, + .n20 = c0, .n21 = c1, .n22 = c2, .n23 = c3, + .n30 = d0, .n31 = d1, .n32 = d2, .n33 = d3 }; + return m; +} + +/* Translation matrix */ +dmnsn_matrix +dmnsn_translation_matrix(dmnsn_vector d) +{ + return dmnsn_matrix_construct(1.0, 0.0, 0.0, d.x, + 0.0, 1.0, 0.0, d.y, + 0.0, 0.0, 1.0, d.z, + 0.0, 0.0, 0.0, 1.0); +} + +/* Rotation matrix; theta/|theta| = axis, |theta| = angle */ +dmnsn_matrix +dmnsn_rotation_matrix(dmnsn_vector theta) +{ + dmnsn_vector axis, n1, n2, n3; + double angle, s, t, x, y, z; + + axis = dmnsn_vector_normalize(theta); + angle = dmnsn_vector_norm(theta); + + /* Shorthand to fit logical lines on one line */ + + s = sin(angle); + t = 1.0 - cos(angle); + + x = axis.x; + y = axis.y; + z = axis.z; + + /* Construct vectors, then a matrix, so our dmnsn_matrix_construct() call + is reasonably small */ + + n1 = dmnsn_vector_construct(1.0 + t*(x*x - 1.0), + z*s + t*x*y, + -y*s + t*x*z); + n2 = dmnsn_vector_construct(-z*s + t*x*y, + 1.0 + t*(y*y - 1.0), + x*s + t*y*z); + n3 = dmnsn_vector_construct(y*s + t*x*z, + -x*s + t*y*z, + 1.0 + t*(z*z - 1.0)); + + return dmnsn_matrix_construct(n1.x, n2.x, n3.x, 0.0, + n1.y, n2.y, n3.y, 0.0, + n1.z, n2.z, n3.z, 0.0, + 0.0, 0.0, 0.0, 1.0); +} + /* Add two vectors */ dmnsn_vector dmnsn_vector_add(dmnsn_vector lhs, dmnsn_vector rhs) @@ -50,7 +113,7 @@ dmnsn_vector_sub(dmnsn_vector lhs, dmnsn_vector rhs) /* Multiply a vector by a scalar */ dmnsn_vector -dmnsn_vector_mul(dmnsn_scalar lhs, dmnsn_vector rhs) +dmnsn_vector_mul(double lhs, dmnsn_vector rhs) { dmnsn_vector v = { .x = lhs*rhs.x, .y = lhs*rhs.y, .z = lhs*rhs.z }; return v; @@ -58,14 +121,14 @@ dmnsn_vector_mul(dmnsn_scalar lhs, dmnsn_vector rhs) /* Divide a vector by a scalar */ dmnsn_vector -dmnsn_vector_div(dmnsn_vector lhs, dmnsn_scalar rhs) +dmnsn_vector_div(dmnsn_vector lhs, double rhs) { dmnsn_vector v = { .x = lhs.x/rhs, .y = lhs.y/rhs, .z = lhs.z/rhs }; return v; } /* Dot product */ -dmnsn_scalar +double dmnsn_vector_dot(dmnsn_vector lhs, dmnsn_vector rhs) { return lhs.x*rhs.x + lhs.y*rhs.y + lhs.z*rhs.z; @@ -81,9 +144,80 @@ dmnsn_vector_cross(dmnsn_vector lhs, dmnsn_vector rhs) return v; } +/* Length of vector */ +double +dmnsn_vector_norm(dmnsn_vector n) +{ + return sqrt(n.x*n.x + n.y*n.y + n.z*n.z); +} + +/* Normalized vector */ +dmnsn_vector +dmnsn_vector_normalize(dmnsn_vector n) +{ + return dmnsn_vector_div(n, dmnsn_vector_norm(n)); +} + +/* 4x4 matrix multiplication */ +dmnsn_matrix +dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs) +{ + dmnsn_matrix r; + + r.n00 = lhs.n00*rhs.n00 + lhs.n01*rhs.n10 + lhs.n02*rhs.n20 + lhs.n03*rhs.n30; + r.n01 = lhs.n00*rhs.n01 + lhs.n01*rhs.n11 + lhs.n02*rhs.n21 + lhs.n03*rhs.n31; + r.n02 = lhs.n00*rhs.n02 + lhs.n01*rhs.n12 + lhs.n02*rhs.n22 + lhs.n03*rhs.n32; + r.n03 = lhs.n00*rhs.n03 + lhs.n01*rhs.n13 + lhs.n02*rhs.n23 + lhs.n03*rhs.n33; + + r.n10 = lhs.n10*rhs.n00 + lhs.n11*rhs.n10 + lhs.n12*rhs.n20 + lhs.n13*rhs.n30; + r.n11 = lhs.n10*rhs.n01 + lhs.n11*rhs.n11 + lhs.n12*rhs.n21 + lhs.n13*rhs.n31; + r.n12 = lhs.n10*rhs.n02 + lhs.n11*rhs.n12 + lhs.n12*rhs.n22 + lhs.n13*rhs.n32; + r.n13 = lhs.n10*rhs.n03 + lhs.n11*rhs.n13 + lhs.n12*rhs.n23 + lhs.n13*rhs.n33; + + r.n20 = lhs.n20*rhs.n00 + lhs.n21*rhs.n10 + lhs.n22*rhs.n20 + lhs.n23*rhs.n30; + r.n21 = lhs.n20*rhs.n01 + lhs.n21*rhs.n11 + lhs.n22*rhs.n21 + lhs.n23*rhs.n31; + r.n22 = lhs.n20*rhs.n02 + lhs.n21*rhs.n12 + lhs.n22*rhs.n22 + lhs.n23*rhs.n32; + r.n23 = lhs.n20*rhs.n03 + lhs.n21*rhs.n13 + lhs.n22*rhs.n23 + lhs.n23*rhs.n33; + + r.n30 = lhs.n30*rhs.n00 + lhs.n31*rhs.n10 + lhs.n32*rhs.n20 + lhs.n33*rhs.n30; + r.n31 = lhs.n30*rhs.n01 + lhs.n31*rhs.n11 + lhs.n32*rhs.n21 + lhs.n33*rhs.n31; + r.n32 = lhs.n30*rhs.n02 + lhs.n31*rhs.n12 + lhs.n32*rhs.n22 + lhs.n33*rhs.n32; + r.n33 = lhs.n30*rhs.n03 + lhs.n31*rhs.n13 + lhs.n32*rhs.n23 + lhs.n33*rhs.n33; + + return r; +} + +/* Affine transformation; lhs*(x,y,z,1), normalized so the fourth element is + 1 */ +dmnsn_vector +dmnsn_matrix_vector_mul(dmnsn_matrix lhs, dmnsn_vector rhs) +{ + dmnsn_vector r; + double w; + + r.x = lhs.n00*rhs.x + lhs.n01*rhs.y + lhs.n02*rhs.z + lhs.n03; + r.x = lhs.n10*rhs.x + lhs.n11*rhs.y + lhs.n12*rhs.z + lhs.n13; + r.x = lhs.n20*rhs.x + lhs.n21*rhs.y + lhs.n22*rhs.z + lhs.n23; + w = lhs.n30*rhs.x + lhs.n31*rhs.y + lhs.n32*rhs.z + lhs.n33; + + return dmnsn_vector_div(r, w); +} + +/* Affine line transformation; n = lhs*(x0 + n) - lhs*x0, x0 *= lhs */ +dmnsn_line +dmnsn_matrix_line_mul(dmnsn_matrix lhs, dmnsn_line rhs) +{ + dmnsn_line l; + l.x0 = dmnsn_matrix_vector_mul(lhs, rhs.x0); + l.n = dmnsn_vector_sub( + dmnsn_matrix_vector_mul(lhs, dmnsn_vector_add(rhs.x0, rhs.n)), + l.x0); + return l; +} + /* A point on a line, l. Returns l.x0 + t*l.n */ dmnsn_vector -dmnsn_line_point(dmnsn_line l, dmnsn_scalar t) +dmnsn_line_point(dmnsn_line l, double t) { return dmnsn_vector_add(l.x0, dmnsn_vector_mul(t, l.n)); } |