2 files changed, 34 insertions, 3 deletions
diff --git a/libdimension/dimension/geometry.h b/libdimension/dimension/geometry.h
index c90c3c8..87e7ab7 100644
--- a/libdimension/dimension/geometry.h
+++ b/libdimension/dimension/geometry.h
@@ -281,7 +281,24 @@ double dmnsn_vector_axis_angle(dmnsn_vector v1, dmnsn_vector v2,
 
 dmnsn_matrix dmnsn_matrix_inverse(dmnsn_matrix A);
 dmnsn_matrix dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs);
-dmnsn_vector dmnsn_transform_vector(dmnsn_matrix lhs, dmnsn_vector rhs);
+
+/* Affine transformation; lhs*(x,y,z,1), normalized so the fourth element is
+   1 */
+DMNSN_INLINE dmnsn_vector
+dmnsn_transform_vector(dmnsn_matrix lhs, dmnsn_vector rhs)
+{
+  /* 12 multiplications, 3 divisions, 12 additions */
+  dmnsn_vector r;
+  double w;
+
+  r.x = lhs.n[0][0]*rhs.x + lhs.n[0][1]*rhs.y + lhs.n[0][2]*rhs.z + lhs.n[0][3];
+  r.y = lhs.n[1][0]*rhs.x + lhs.n[1][1]*rhs.y + lhs.n[1][2]*rhs.z + lhs.n[1][3];
+  r.z = lhs.n[2][0]*rhs.x + lhs.n[2][1]*rhs.y + lhs.n[2][2]*rhs.z + lhs.n[2][3];
+  w   = lhs.n[3][0]*rhs.x + lhs.n[3][1]*rhs.y + lhs.n[3][2]*rhs.z + lhs.n[3][3];
+
+  return dmnsn_vector_div(r, w);
+}
+
 dmnsn_bounding_box dmnsn_transform_bounding_box(dmnsn_matrix lhs,
                                                 dmnsn_bounding_box rhs);
 
diff --git a/libdimension/dimension/object.h b/libdimension/dimension/object.h
index f633a2d..cef1acb 100644
--- a/libdimension/dimension/object.h
+++ b/libdimension/dimension/object.h
@@ -41,8 +41,6 @@ typedef struct dmnsn_intersection {
   const dmnsn_interior *interior;
 } dmnsn_intersection;
 
-dmnsn_vector dmnsn_transform_normal(dmnsn_matrix trans, dmnsn_vector normal);
-
 /* Forward-declare dmnsn_object */
 typedef struct dmnsn_object dmnsn_object;
 
@@ -89,4 +87,20 @@ void dmnsn_delete_object(dmnsn_object *object);
 
 void dmnsn_object_init(dmnsn_object *object);
 
+/* Useful function to transform a normal vector */
+DMNSN_INLINE dmnsn_vector
+dmnsn_transform_normal(dmnsn_matrix trans, dmnsn_vector normal)
+{
+  return dmnsn_vector_normalize(
+    dmnsn_vector_sub(
+      dmnsn_transform_vector(trans, normal),
+      /* Optimized form of dmnsn_transform_vector(trans, dmnsn_zero) */
+      dmnsn_vector_div(
+        dmnsn_new_vector(trans.n[0][3], trans.n[1][3], trans.n[2][3]),
+        trans.n[3][3]
+      )
+    )
+  );
+}
+
 #endif /* DIMENSION_OBJECT_H */