2 files changed, 33 insertions, 0 deletions
diff --git a/libdimension/dimension/geometry.h b/libdimension/dimension/geometry.h
index 68359aa..3011216 100644
--- a/libdimension/dimension/geometry.h
+++ b/libdimension/dimension/geometry.h
@@ -39,6 +39,12 @@ typedef struct {
   dmnsn_vector n;  /* A normal vector; the direction of the line */
 } dmnsn_line;
 
+/* Vector constants */
+static const dmnsn_vector dmnsn_zero = { 0.0, 0.0, 0.0 };
+static const dmnsn_vector dmnsn_x    = { 1.0, 0.0, 0.0 };
+static const dmnsn_vector dmnsn_y    = { 0.0, 1.0, 0.0 };
+static const dmnsn_vector dmnsn_z    = { 0.0, 0.0, 1.0 };
+
 /* Shorthand for vector/matrix construction */
 
 DMNSN_INLINE dmnsn_vector
@@ -133,6 +139,13 @@ dmnsn_vector_cross(dmnsn_vector lhs, dmnsn_vector rhs)
   return v;
 }
 
+DMNSN_INLINE dmnsn_vector
+dmnsn_vector_proj(dmnsn_vector u, dmnsn_vector d)
+{
+  /* 1 division, 9 multiplications, 4 additions */
+  return dmnsn_vector_mul(dmnsn_vector_dot(u, d)/dmnsn_vector_dot(d, d), d);
+}
+
 DMNSN_INLINE double
 dmnsn_vector_norm(dmnsn_vector n)
 {
@@ -147,6 +160,9 @@ dmnsn_vector_normalize(dmnsn_vector n)
   return dmnsn_vector_div(n, dmnsn_vector_norm(n));
 }
 
+double dmnsn_vector_axis_angle(dmnsn_vector v1, dmnsn_vector v2,
+                               dmnsn_vector axis);
+
 dmnsn_matrix dmnsn_matrix_inverse(dmnsn_matrix A);
 dmnsn_matrix dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs);
 dmnsn_vector dmnsn_matrix_vector_mul(dmnsn_matrix lhs, dmnsn_vector rhs);
diff --git a/libdimension/geometry.c b/libdimension/geometry.c
index cbcc2c1..4aa46ec 100644
--- a/libdimension/geometry.c
+++ b/libdimension/geometry.c
@@ -80,6 +80,23 @@ dmnsn_rotation_matrix(dmnsn_vector theta)
   );
 }
 
+/* Find the angle between two vectors with respect to an axis */
+double
+dmnsn_vector_axis_angle(dmnsn_vector v1, dmnsn_vector v2, dmnsn_vector axis)
+{
+  dmnsn_vector d    = dmnsn_vector_sub(v1, v2);
+  dmnsn_vector proj = dmnsn_vector_add(dmnsn_vector_proj(d, axis), v2);
+
+  double c = dmnsn_vector_dot(dmnsn_vector_normalize(v1),
+                              dmnsn_vector_normalize(proj));
+  double angle = acos(c);
+
+  if (dmnsn_vector_dot(dmnsn_vector_cross(v1, proj), axis) > 0)
+    return angle;
+  else
+    return -angle;
+}
+
 /* Matrix inversion helper functions */
 
 typedef struct { double n[2][2]; } dmnsn_matrix2;