objects: Implement smooth triangle fans.

5 files changed, 239 insertions, 55 deletions

diff --git a/libdimension-python/dimension.pxd b/libdimension-python/dimension.pxd
index 0c62df6..f002125 100644
--- a/libdimension-python/dimension.pxd
+++ b/libdimension-python/dimension.pxd
@@ -331,6 +331,7 @@ cdef extern from "../libdimension/dimension.h":
   dmnsn_object *dmnsn_new_triangle(dmnsn_pool *pool, dmnsn_vector vertices[3])
   dmnsn_object *dmnsn_new_smooth_triangle(dmnsn_pool *pool, dmnsn_vector vertices[3], dmnsn_vector normals[3])
   dmnsn_object *dmnsn_new_triangle_fan(dmnsn_pool *pool, dmnsn_vector *vertices, size_t nvertices)
+  dmnsn_object *dmnsn_new_smooth_triangle_fan(dmnsn_pool *pool, dmnsn_vector *vertices, dmnsn_vector *normals, size_t nvertices)
   dmnsn_object *dmnsn_new_plane(dmnsn_pool *pool, dmnsn_vector normal)
   dmnsn_object *dmnsn_new_sphere(dmnsn_pool *pool)
   dmnsn_object *dmnsn_new_cube(dmnsn_pool *pool)
diff --git a/libdimension-python/dimension.pyx b/libdimension-python/dimension.pyx
index 1ad6d0b..58b68e2 100644
--- a/libdimension-python/dimension.pyx
+++ b/libdimension-python/dimension.pyx
@@ -1125,32 +1125,32 @@ cdef class Triangle(Object):
 
     Additionally, Triangle() accepts any arguments that Object() accepts.
     """
-    if a_normal is None and b_normal is None and c_normal is None:
-      a_normal = cross(b - a, c - a)
-      b_normal = a_normal
-      c_normal = a_normal
-
     cdef dmnsn_vector vertices[3]
+    cdef dmnsn_vector normals[3]
+
     vertices[0] = Vector(a)._v
     vertices[1] = Vector(b)._v
     vertices[2] = Vector(c)._v
 
-    cdef dmnsn_vector normals[3]
-    normals[0] = Vector(a_normal)._v
-    normals[1] = Vector(b_normal)._v
-    normals[2] = Vector(c_normal)._v
+    if a_normal is None and b_normal is None and c_normal is None:
+      self._object = dmnsn_new_triangle(_get_pool(), vertices)
+    else:
+      normals[0] = Vector(a_normal)._v
+      normals[1] = Vector(b_normal)._v
+      normals[2] = Vector(c_normal)._v
+      self._object = dmnsn_new_smooth_triangle(_get_pool(), vertices, normals)
 
-    self._object = dmnsn_new_smooth_triangle(_get_pool(), vertices, normals)
     Object.__init__(self, *args, **kwargs)
 
 cdef class TriangleFan(Object):
-  """A triangle."""
-  def __init__(self, vertices, *args, **kwargs):
+  """A triangle fan."""
+  def __init__(self, vertices, normals = None, *args, **kwargs):
     """
     Create a TriangleFan.
 
     Keyword arguments:
     vertices -- the vertices of the fan, starting in the center
+    normals  -- the (optional) normal vectors for each vertex
 
     Additionally, TriangleFan() accepts any arguments that Object() accepts.
     """
@@ -1158,14 +1158,26 @@ cdef class TriangleFan(Object):
     if nvertices < 3:
       raise TypeError("expected at least 3 vertices")
 
-    cdef dmnsn_vector *varray
+    cdef dmnsn_vector *varray = NULL
+    cdef dmnsn_vector *narray = NULL
     try:
       varray = <dmnsn_vector *>dmnsn_malloc(nvertices*sizeof(dmnsn_vector))
       for i in range(nvertices):
         varray[i] = Vector(vertices[i])._v
 
-      self._object = dmnsn_new_triangle_fan(_get_pool(), varray, nvertices)
+      if normals is None:
+        self._object = dmnsn_new_triangle_fan(_get_pool(), varray, nvertices)
+      else:
+        if len(normals) != nvertices:
+          raise TypeError("expected same number of vertices and normals")
+
+        narray = <dmnsn_vector *>dmnsn_malloc(nvertices*sizeof(dmnsn_vector))
+        for i in range(nvertices):
+          narray[i] = Vector(normals[i])._v
+
+        self._object = dmnsn_new_smooth_triangle_fan(_get_pool(), varray, narray, nvertices)
     finally:
+      dmnsn_free(narray)
       dmnsn_free(varray)
 
     Object.__init__(self, *args, **kwargs)
diff --git a/libdimension/dimension/objects.h b/libdimension/dimension/objects.h
index b328025..e5a39c5 100644
--- a/libdimension/dimension/objects.h
+++ b/libdimension/dimension/objects.h
@@ -49,8 +49,17 @@ dmnsn_object *dmnsn_new_smooth_triangle(dmnsn_pool *pool, dmnsn_vector vertices[
  * @param[in] nvertices  The number of vertices.
  * @return A triangle fan.
  */
-dmnsn_object *
-dmnsn_new_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], size_t nvertices);
+dmnsn_object *dmnsn_new_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], size_t nvertices);
+
+/**
+ * A smooth triangle fan.
+ * @param[in] pool  The memory pool to allocate from.
+ * @param[in] vertices  The vertices of the fan, starting in the center.
+ * @param[in] vertices  The normal vector for each vertex.
+ * @param[in] nvertices  The number of vertices.
+ * @return A triangle fan.
+ */
+dmnsn_object *dmnsn_new_smooth_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], dmnsn_vector normals[], size_t nvertices);
 
 /**
  * A plane.
diff --git a/libdimension/triangle.c b/libdimension/triangle.c
index 4a70c59..fdd2b96 100644
--- a/libdimension/triangle.c
+++ b/libdimension/triangle.c
@@ -25,7 +25,7 @@
  * for a description of the intersection algorithm.
  */
 
-#include "dimension.h"
+#include "dimension-internal.h"
 
 /// Optimized ray/triangle intersection test.
 static inline bool
@@ -39,7 +39,7 @@ dmnsn_ray_triangle_intersection(dmnsn_line l, double *t, double *u, double *v)
 }
 
 /// Triangle intersection callback.
-static bool
+DMNSN_HOT static bool
 dmnsn_triangle_intersection_fn(const dmnsn_object *object, dmnsn_line l,
                                dmnsn_intersection *intersection)
 {
@@ -88,7 +88,7 @@ typedef struct {
 } dmnsn_smooth_triangle;
 
 /// Smooth triangle intersection callback.
-static bool
+DMNSN_HOT static bool
 dmnsn_smooth_triangle_intersection_fn(const dmnsn_object *object, dmnsn_line l,
                                       dmnsn_intersection *intersection)
 {
diff --git a/libdimension/triangle_fan.c b/libdimension/triangle_fan.c
index 0165a51..9940614 100644
--- a/libdimension/triangle_fan.c
+++ b/libdimension/triangle_fan.c
@@ -25,7 +25,7 @@
  * for a description of the intersection algorithm.
  */
 
-#include "dimension.h"
+#include "dimension-internal.h"
 
 /// Triangle fan type.
 typedef struct {
@@ -56,18 +56,65 @@ dmnsn_change_normal_basis(const double coeffs[6], dmnsn_vector n)
   );
 }
 
-/// Triangle intersection callback.
-static bool
+/// Change basis from one triangle to the next for a line
+static inline dmnsn_line
+dmnsn_change_line_basis(const double coeffs[6], dmnsn_line l)
+{
+  return dmnsn_new_line(dmnsn_change_basis(coeffs, l.x0), dmnsn_change_basis(coeffs, l.n));
+}
+
+/// Store the compressed incremental matrix.
+static inline void
+dmnsn_compress_coeffs(double coeffs[6], dmnsn_matrix incremental)
+{
+  coeffs[0] = incremental.n[0][0];
+  coeffs[1] = incremental.n[0][2];
+  coeffs[2] = incremental.n[1][0];
+  coeffs[3] = incremental.n[1][2];
+  coeffs[4] = incremental.n[2][0];
+  coeffs[5] = incremental.n[2][2];
+}
+
+/// Decompress the incremental matrix.
+static inline dmnsn_matrix
+dmnsn_decompress_coeffs(const double coeffs[6])
+{
+  dmnsn_matrix incremental = dmnsn_new_matrix(
+    coeffs[0], 1.0, coeffs[1], 0.0,
+    coeffs[2], 0.0, coeffs[3], 0.0,
+    coeffs[4], 0.0, coeffs[5], 0.0
+  );
+  return incremental;
+}
+
+/// Make a change-of-basis matrix for a triangle.
+static inline dmnsn_matrix
+dmnsn_triangle_basis(dmnsn_vector a, dmnsn_vector ab, dmnsn_vector ac)
+{
+  dmnsn_vector normal = dmnsn_vector_cross(ab, ac);
+  return dmnsn_new_matrix4(ab, ac, normal, a);
+}
+
+/// Optimized ray/triangle intersection test.
+static inline bool
+dmnsn_ray_triangle_intersection(dmnsn_line l, double *t, double *u, double *v)
+{
+  *t = -l.x0.z/l.n.z;
+  *u = l.x0.x + (*t)*l.n.x;
+  *v = l.x0.y + (*t)*l.n.y;
+  return *t >= 0.0 && *u >= 0.0 && *v >= 0.0 && *u + *v <= 1.0;
+}
+
+/// Triangle fan intersection callback.
+DMNSN_HOT static bool
 dmnsn_triangle_fan_intersection_fn(const dmnsn_object *object, dmnsn_line l, dmnsn_intersection *intersection)
 {
   const dmnsn_triangle_fan *fan = (const dmnsn_triangle_fan *)object;
 
-  double t = -l.x0.z/l.n.z;
-  double u = l.x0.x + t*l.n.x;
-  double v = l.x0.y + t*l.n.y;
+  double t, u, v;
 
   double best_t = INFINITY;
-  if (t >= 0.0 && u >= 0.0 && v >= 0.0 && u + v <= 1.0) {
+  if (dmnsn_ray_triangle_intersection(l, &t, &u, &v)) {
     best_t = t;
   }
 
@@ -76,14 +123,10 @@ dmnsn_triangle_fan_intersection_fn(const dmnsn_object *object, dmnsn_line l, dmn
 
   for (size_t i = 0; i < fan->ncoeffs; ++i) {
     const double *coeffs = fan->coeffs[i];
-    l = dmnsn_new_line(dmnsn_change_basis(coeffs, l.x0), dmnsn_change_basis(coeffs, l.n));
+    l = dmnsn_change_line_basis(coeffs, l);
     normal = dmnsn_change_normal_basis(coeffs, normal);
 
-    t = -l.x0.z/l.n.z;
-    u = l.x0.x + t*l.n.x;
-    v = l.x0.y + t*l.n.y;
-
-    if (t >= 0.0 && t < best_t && u >= 0.0 && v >= 0.0 && u + v <= 1.0) {
+    if (dmnsn_ray_triangle_intersection(l, &t, &u, &v) && t < best_t) {
       best_t = t;
       best_normal = normal;
     }
@@ -105,12 +148,10 @@ dmnsn_triangle_fan_inside_fn(const dmnsn_object *object, dmnsn_vector point)
   return false;
 }
 
-/// Triangle fan bounding callback.
-static dmnsn_bounding_box
-dmnsn_triangle_fan_bounding_fn(const dmnsn_object *object, dmnsn_matrix trans)
+/// Computes the bounding box for the first triangle
+static inline dmnsn_bounding_box
+dmnsn_bound_first_triangle(dmnsn_matrix trans)
 {
-  const dmnsn_triangle_fan *fan = (const dmnsn_triangle_fan *)object;
-
   dmnsn_vector a = dmnsn_transform_point(trans, dmnsn_zero);
   dmnsn_vector b = dmnsn_transform_point(trans, dmnsn_x);
   dmnsn_vector c = dmnsn_transform_point(trans, dmnsn_y);
@@ -119,16 +160,22 @@ dmnsn_triangle_fan_bounding_fn(const dmnsn_object *object, dmnsn_matrix trans)
   box = dmnsn_bounding_box_swallow(box, b);
   box = dmnsn_bounding_box_swallow(box, c);
 
-  dmnsn_matrix P = trans;
+  return box;
+}
+
+/// Triangle fan bounding callback.
+static dmnsn_bounding_box
+dmnsn_triangle_fan_bounding_fn(const dmnsn_object *object, dmnsn_matrix trans)
+{
+  const dmnsn_triangle_fan *fan = (const dmnsn_triangle_fan *)object;
+
+  dmnsn_bounding_box box = dmnsn_bound_first_triangle(trans);
 
   for (size_t i = 0; i < fan->ncoeffs; ++i) {
-    const double *coeffs = fan->coeffs[i];
-    dmnsn_matrix incremental = dmnsn_new_matrix(coeffs[0], 1.0, coeffs[1], 0.0,
-                                                coeffs[2], 0.0, coeffs[3], 0.0,
-                                                coeffs[4], 0.0, coeffs[5], 0.0);
-    P = dmnsn_matrix_mul(P, dmnsn_matrix_inverse(incremental));
-    dmnsn_vector d = dmnsn_transform_point(P, dmnsn_y);
-    box = dmnsn_bounding_box_swallow(box, d);
+    dmnsn_matrix incremental = dmnsn_decompress_coeffs(fan->coeffs[i]);
+    trans = dmnsn_matrix_mul(trans, dmnsn_matrix_inverse(incremental));
+    dmnsn_vector vertex = dmnsn_transform_point(trans, dmnsn_y);
+    box = dmnsn_bounding_box_swallow(box, vertex);
   }
 
   return box;
@@ -141,6 +188,78 @@ static dmnsn_object_vtable dmnsn_triangle_fan_vtable = {
   .bounding_fn = dmnsn_triangle_fan_bounding_fn,
 };
 
+/// Smooth triangle fan type.
+typedef struct dmnsn_smooth_triangle_fan {
+  dmnsn_object object;
+  dmnsn_vector na, nab, nac;
+  size_t ncoeffs;
+  double coeffs[][9]; ///< 0-6 is same as dmnsn_triangle_fan, 6-9 is the normal
+} dmnsn_smooth_triangle_fan;
+
+/// Smooth triangle fan intersection callback.
+DMNSN_HOT static bool
+dmnsn_smooth_triangle_fan_intersection_fn(const dmnsn_object *object, dmnsn_line l, dmnsn_intersection *intersection)
+{
+  const dmnsn_smooth_triangle_fan *fan = (const dmnsn_smooth_triangle_fan *)object;
+
+  dmnsn_vector nab = fan->nab;
+  dmnsn_vector nac = fan->nac;
+
+  double t, u, v;
+
+  double best_t = INFINITY;
+  dmnsn_vector best_normal;
+  if (dmnsn_ray_triangle_intersection(l, &t, &u, &v)) {
+    best_t = t;
+    best_normal = dmnsn_vector_add(dmnsn_vector_mul(u, nab), dmnsn_vector_mul(v, nac));
+  }
+
+  for (size_t i = 0; i < fan->ncoeffs; ++i) {
+    const double *coeffs = fan->coeffs[i];
+    l = dmnsn_change_line_basis(coeffs, l);
+    nab = nac;
+    nac = dmnsn_new_vector(coeffs[6], coeffs[7], coeffs[8]);
+
+    if (dmnsn_ray_triangle_intersection(l, &t, &u, &v) && t < best_t) {
+      best_t = t;
+      best_normal = dmnsn_vector_add(dmnsn_vector_mul(u, nab), dmnsn_vector_mul(v, nac));
+    }
+  }
+
+  if (!isinf(best_t)) {
+    intersection->t = t;
+    intersection->normal = dmnsn_vector_add(fan->na, best_normal);
+    return true;
+  }
+
+  return false;
+}
+
+/// Smooth triangle fan bounding callback.
+static dmnsn_bounding_box
+dmnsn_smooth_triangle_fan_bounding_fn(const dmnsn_object *object, dmnsn_matrix trans)
+{
+  const dmnsn_smooth_triangle_fan *fan = (const dmnsn_smooth_triangle_fan *)object;
+
+  dmnsn_bounding_box box = dmnsn_bound_first_triangle(trans);
+
+  for (size_t i = 0; i < fan->ncoeffs; ++i) {
+    dmnsn_matrix incremental = dmnsn_decompress_coeffs(fan->coeffs[i]);
+    trans = dmnsn_matrix_mul(trans, dmnsn_matrix_inverse(incremental));
+    dmnsn_vector vertex = dmnsn_transform_point(trans, dmnsn_y);
+    box = dmnsn_bounding_box_swallow(box, vertex);
+  }
+
+  return box;
+}
+
+/// Smooth triangle fan vtable.
+static dmnsn_object_vtable dmnsn_smooth_triangle_fan_vtable = {
+  .intersection_fn = dmnsn_smooth_triangle_fan_intersection_fn,
+  .inside_fn = dmnsn_triangle_fan_inside_fn,
+  .bounding_fn = dmnsn_smooth_triangle_fan_bounding_fn,
+};
+
 dmnsn_object *
 dmnsn_new_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], size_t nvertices)
 {
@@ -158,24 +277,67 @@ dmnsn_new_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], size_t nvertic
   dmnsn_vector a = vertices[0];
   dmnsn_vector ab = dmnsn_vector_sub(vertices[1], a);
   dmnsn_vector ac = dmnsn_vector_sub(vertices[2], a);
-  dmnsn_vector normal = dmnsn_vector_cross(ab, ac);
-  dmnsn_matrix P = dmnsn_new_matrix4(ab, ac, normal, a);
+  dmnsn_matrix P = dmnsn_triangle_basis(a, ab, ac);
   object->intrinsic_trans = P;
 
   for (size_t i = 0; i < ncoeffs; ++i) {
     ab = ac;
     ac = dmnsn_vector_sub(vertices[i + 3], a);
-    normal = dmnsn_vector_cross(ab, ac);
 
-    dmnsn_matrix newP = dmnsn_new_matrix4(ab, ac, normal, a);
+    dmnsn_matrix newP = dmnsn_triangle_basis(a, ab, ac);
+    dmnsn_matrix incremental = dmnsn_matrix_mul(dmnsn_matrix_inverse(newP), P);
+    dmnsn_compress_coeffs(fan->coeffs[i], incremental);
+
+    P = newP;
+  }
+
+  return object;
+}
+
+dmnsn_object *
+dmnsn_new_smooth_triangle_fan(dmnsn_pool *pool, dmnsn_vector vertices[], dmnsn_vector normals[], size_t nvertices)
+{
+  dmnsn_assert(nvertices >= 3, "Not enough vertices for one triangle");
+
+  size_t ncoeffs = nvertices - 3;
+  dmnsn_smooth_triangle_fan *fan = dmnsn_palloc(pool, sizeof(dmnsn_smooth_triangle_fan) + ncoeffs*sizeof(double[9]));
+  fan->ncoeffs = ncoeffs;
+
+  dmnsn_object *object = &fan->object;
+  dmnsn_init_object(object);
+  object->vtable = &dmnsn_smooth_triangle_fan_vtable;
+
+  // Compute the initial matrix
+  dmnsn_vector a = vertices[0];
+  dmnsn_vector ab = dmnsn_vector_sub(vertices[1], a);
+  dmnsn_vector ac = dmnsn_vector_sub(vertices[2], a);
+  dmnsn_matrix P = dmnsn_triangle_basis(a, ab, ac);
+  dmnsn_matrix Pabc = P;
+  object->intrinsic_trans = P;
+
+  // Transform the first three normals
+  dmnsn_vector na = dmnsn_vector_normalized(dmnsn_transform_normal(P, normals[0]));
+  dmnsn_vector nb = dmnsn_vector_normalized(dmnsn_transform_normal(P, normals[1]));
+  dmnsn_vector nc = dmnsn_vector_normalized(dmnsn_transform_normal(P, normals[2]));
+  fan->na = na;
+  fan->nab = dmnsn_vector_sub(nb, na);
+  fan->nac = dmnsn_vector_sub(nc, na);
+
+  // Compute the coefficients
+  for (size_t i = 0; i < ncoeffs; ++i) {
+    ab = ac;
+    ac = dmnsn_vector_sub(vertices[i + 3], a);
+
+    dmnsn_matrix newP = dmnsn_triangle_basis(a, ab, ac);
     dmnsn_matrix incremental = dmnsn_matrix_mul(dmnsn_matrix_inverse(newP), P);
     double *coeffs = fan->coeffs[i];
-    coeffs[0] = incremental.n[0][0];
-    coeffs[1] = incremental.n[0][2];
-    coeffs[2] = incremental.n[1][0];
-    coeffs[3] = incremental.n[1][2];
-    coeffs[4] = incremental.n[2][0];
-    coeffs[5] = incremental.n[2][2];
+    dmnsn_compress_coeffs(coeffs, incremental);
+
+    nc = dmnsn_vector_normalized(dmnsn_transform_normal(Pabc, normals[i + 3]));
+    dmnsn_vector nac = dmnsn_vector_sub(nc, na);
+    coeffs[6] = nac.x;
+    coeffs[7] = nac.y;
+    coeffs[8] = nac.z;
 
     P = newP;
   }