summaryrefslogtreecommitdiffstats
path: root/libdimension/geometry.c
diff options
context:
space:
mode:
Diffstat (limited to 'libdimension/geometry.c')
-rw-r--r--libdimension/geometry.c144
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));
}