Fix rotational alignment.

6 files changed, 52 insertions, 40 deletions

diff --git a/libdimension-python/dimension.pxd b/libdimension-python/dimension.pxd
index cd28f24..22e9764 100644
--- a/libdimension-python/dimension.pxd
+++ b/libdimension-python/dimension.pxd
@@ -91,9 +91,6 @@ cdef extern from "../libdimension/dimension.h":
   double dmnsn_vector_norm(dmnsn_vector v)
   dmnsn_vector dmnsn_vector_normalized(dmnsn_vector v)
 
-  double dmnsn_vector_axis_angle(dmnsn_vector v1, dmnsn_vector v2,
-                                 dmnsn_vector axis)
-
   dmnsn_vector dmnsn_zero
   dmnsn_vector dmnsn_x
   dmnsn_vector dmnsn_y
@@ -114,6 +111,8 @@ cdef extern from "../libdimension/dimension.h":
   dmnsn_matrix dmnsn_scale_matrix(dmnsn_vector s)
   dmnsn_matrix dmnsn_translation_matrix(dmnsn_vector d)
   dmnsn_matrix dmnsn_rotation_matrix(dmnsn_vector theta)
+  dmnsn_matrix dmnsn_alignment_matrix(dmnsn_vector frm, dmnsn_vector to,
+                                      dmnsn_vector axis1, dmnsn_vector axis2)
 
   ##########
   # Colors #
diff --git a/libdimension-python/dimension.pyx b/libdimension-python/dimension.pyx
index 9a5fd80..54b7e66 100644
--- a/libdimension-python/dimension.pyx
+++ b/libdimension-python/dimension.pyx
@@ -643,11 +643,7 @@ cdef class Cone(Object):
 
     cdef Matrix trans = translate(Y)
     trans = scale(1.0, dir.norm()/2, 1.0)*trans
-
-    cdef double thetaX = dmnsn_vector_axis_angle(dmnsn_y, dir._v, dmnsn_x)
-    cdef double thetaZ = dmnsn_vector_axis_angle(dmnsn_y, dir._v, dmnsn_z)
-    trans = _rawRotate(thetaX*X)*_rawRotate(thetaZ*Z)*trans
-
+    trans = _rawMatrix(dmnsn_alignment_matrix(dmnsn_y, dir._v, dmnsn_x, dmnsn_z))*trans
     trans = translate(bottom)*trans
 
     self._intrinsicTransform(trans)
@@ -786,22 +782,14 @@ cdef class PerspectiveCamera(Camera):
     cdef Vector vsky = Vector(sky)
 
     # Line up the top of the viewport with the sky vector
-    cdef double thetaSkyX = dmnsn_vector_axis_angle(dmnsn_y, vsky._v, dmnsn_x)
-    cdef double thetaSkyZ = dmnsn_vector_axis_angle(dmnsn_y, vsky._v, dmnsn_z)
-    cdef Matrix alignSky = _rawRotate(thetaSkyX*X)*_rawRotate(thetaSkyZ*Z)
-    self.transform(alignSky)
-    cdef Vector right = alignSky*X
+    cdef Matrix alignSky = _rawMatrix(dmnsn_alignment_matrix(dmnsn_y, vsky._v,
+                                                             dmnsn_z, dmnsn_x))
     cdef Vector forward = alignSky*Z
+    cdef Vector right   = alignSky*X
 
     # Line up the look at point with lookAt
-    cdef double thetaLookAtSky = dmnsn_vector_axis_angle(
-      forward._v, dir._v, vsky._v
-    )
-    cdef double thetaLookAtRight = dmnsn_vector_axis_angle(
-      forward._v, dir._v, right._v
-    )
-    self.transform(_rawRotate(thetaLookAtSky*vsky))
-    self.transform(_rawRotate(thetaLookAtRight*right))
+    self.transform(_rawMatrix(dmnsn_alignment_matrix(forward._v, dir._v,
+                                                     vsky._v, right._v)))
 
     # Move the camera into position
     self.transform(translate(Vector(location)))
diff --git a/libdimension-python/tests/demo.py b/libdimension-python/tests/demo.py
index 4b68114..c024812 100755
--- a/libdimension-python/tests/demo.py
+++ b/libdimension-python/tests/demo.py
@@ -153,10 +153,10 @@ skySphere = SkySphere(
 )
 
 # Scene
-scene = Scene(canvas      = canvas,
-              objects     = objects,
-              lights      = lights,
-              camera      = camera)
+scene = Scene(canvas  = canvas,
+              objects = objects,
+              lights  = lights,
+              camera  = camera)
 scene.defaultTexture = Texture(finish = Ambient(0.1) + Diffuse(0.6))
 scene.background     = Clear
 scene.skySphere      = skySphere
diff --git a/libdimension/dimension/geometry.h b/libdimension/dimension/geometry.h
index ba8f531..7e466dd 100644
--- a/libdimension/dimension/geometry.h
+++ b/libdimension/dimension/geometry.h
@@ -178,6 +178,16 @@ dmnsn_matrix dmnsn_translation_matrix(dmnsn_vector d);
  * @return The transformation matrix.
  */
 dmnsn_matrix dmnsn_rotation_matrix(dmnsn_vector theta);
+/**
+ * An alignment matrix.
+ * @param[in] from   The initial vector.
+ * @param[in] to     The desired direction.
+ * @param[in] axis1  The first axis about which to rotate.
+ * @param[in] axis2  The second axis about which to rotate.
+ * @return A transformation matrix that will rotate \p from to \p to.
+ */
+dmnsn_matrix dmnsn_alignment_matrix(dmnsn_vector from, dmnsn_vector to,
+                                    dmnsn_vector axis1, dmnsn_vector axis2);
 
 /**
  * Construct a new line.
@@ -359,10 +369,6 @@ dmnsn_vector_max(dmnsn_vector a, dmnsn_vector b)
   );
 }
 
-/** Return the angle between two vectors with respect to an axis. */
-double dmnsn_vector_axis_angle(dmnsn_vector v1, dmnsn_vector v2,
-                               dmnsn_vector axis);
-
 /** Invert a matrix. */
 dmnsn_matrix dmnsn_matrix_inverse(dmnsn_matrix A);
 
diff --git a/libdimension/geometry.c b/libdimension/geometry.c
index b36b9c3..f7c6842 100644
--- a/libdimension/geometry.c
+++ b/libdimension/geometry.c
@@ -82,27 +82,46 @@ 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)
+static double
+dmnsn_axis_angle(dmnsn_vector from, dmnsn_vector to, dmnsn_vector axis)
 {
-  dmnsn_vector d    = dmnsn_vector_sub(v1, v2);
-  dmnsn_vector proj = dmnsn_vector_add(dmnsn_vector_proj(d, axis), v2);
+  from = dmnsn_vector_sub(from, dmnsn_vector_proj(from, axis));
+  to   = dmnsn_vector_sub(to,   dmnsn_vector_proj(to, axis));
 
-  double projn = dmnsn_vector_norm(proj);
-  if (fabs(projn) < dmnsn_epsilon)
+  double fromnorm = dmnsn_vector_norm(from);
+  double tonorm   = dmnsn_vector_norm(to);
+  if (fromnorm < dmnsn_epsilon || tonorm < dmnsn_epsilon) {
     return 0.0;
+  }
+
+  from = dmnsn_vector_div(from, fromnorm);
+  to   = dmnsn_vector_div(to,   tonorm);
 
-  double c = dmnsn_vector_dot(dmnsn_vector_normalized(v1),
-                              dmnsn_vector_div(proj, projn));
-  double angle = acos(c);
+  double angle = acos(dmnsn_vector_dot(from, to));
 
-  if (dmnsn_vector_dot(dmnsn_vector_cross(v1, proj), axis) > 0.0) {
+  if (dmnsn_vector_dot(dmnsn_vector_cross(from, to), axis) > 0.0) {
     return angle;
   } else {
     return -angle;
   }
 }
 
+/* Alignment matrix */
+dmnsn_matrix
+dmnsn_alignment_matrix(dmnsn_vector from, dmnsn_vector to,
+                       dmnsn_vector axis1, dmnsn_vector axis2)
+{
+  double theta1 = dmnsn_axis_angle(from, to, axis1);
+  dmnsn_matrix align1 = dmnsn_rotation_matrix(dmnsn_vector_mul(theta1, axis1));
+  from  = dmnsn_transform_vector(align1, from);
+  axis2 = dmnsn_transform_vector(align1, axis2);
+
+  double theta2 = dmnsn_axis_angle(from, to, axis2);
+  dmnsn_matrix align2 = dmnsn_rotation_matrix(dmnsn_vector_mul(theta2, axis2));
+
+  return dmnsn_matrix_mul(align2, align1);
+}
+
 /* Matrix inversion helper functions */
 
 /** A 2x2 matrix for inversion by partitioning. */
diff --git a/libdimension/libdimension.pc.in b/libdimension/libdimension.pc.in
index 97a3cb5..fd5649f 100644
--- a/libdimension/libdimension.pc.in
+++ b/libdimension/libdimension.pc.in
@@ -6,6 +6,6 @@ includedir=@includedir@
 Name: libdimension
 Description: The Dimension Library
 Requires:
-Version: @VERSION@
+Version: @PACKAGE_VERSION@
 Libs: -L${libdir} -ldimension
 Cflags: