Use a PR-tree to speed up collision detection.

1 files changed, 442 insertions, 39 deletions

diff --git a/libdimension/tests/physics.c b/libdimension/tests/physics.c
index 14eeb07..ace6c1f 100644
--- a/libdimension/tests/physics.c
+++ b/libdimension/tests/physics.c
@@ -1,14 +1,14 @@
 #include "dimension.h"
 #include <math.h>
 
-typedef struct sphere {
+typedef struct physics_sphere {
   dmnsn_vector center;
   dmnsn_vector velocity;
   double radius;
   dmnsn_color color;
-} sphere;
+} physics_sphere;
 
-sphere
+physics_sphere
 make_sphere(size_t x, size_t y, size_t z, size_t size)
 {
   --size;
@@ -36,7 +36,7 @@ make_sphere(size_t x, size_t y, size_t z, size_t size)
     dmnsn_new_color((double)x/size, (double)y/size, (double)z/size)
   );
 
-  sphere s = {
+  physics_sphere s = {
     .center = c,
     .velocity = dmnsn_zero,
     .radius = r,
@@ -49,12 +49,12 @@ dmnsn_array *
 make_spheres()
 {
   const size_t size = 10;
-  dmnsn_array *spheres = dmnsn_new_array(sizeof(sphere));
+  dmnsn_array *spheres = dmnsn_new_array(sizeof(physics_sphere));
   for (size_t x = 0; x < size; ++x) {
     for (size_t y = 0; y < size; ++y) {
       for (size_t z = 0; z < size; ++z) {
-        sphere s = make_sphere(x, y, z, size);
-        dmnsn_array_push(spheres, &s);
+        physics_sphere sphere = make_sphere(x, y, z, size);
+        dmnsn_array_push(spheres, &sphere);
       }
     }
   }
@@ -85,11 +85,11 @@ make_scene(dmnsn_array *spheres, dmnsn_canvas *canvas, dmnsn_camera *camera)
     )
   );
 
-  DMNSN_ARRAY_FOREACH (sphere *, s, spheres) {
-    dmnsn_object *sph = dmnsn_new_sphere();
+  DMNSN_ARRAY_FOREACH (physics_sphere *, s, spheres) {
+    dmnsn_object *sphere = dmnsn_new_sphere();
 
-    sph->texture = dmnsn_new_texture();
-    sph->texture->pigment = dmnsn_new_solid_pigment(s->color);
+    sphere->texture = dmnsn_new_texture();
+    sphere->texture->pigment = dmnsn_new_solid_pigment(s->color);
     double maxcomponent = dmnsn_max(
       dmnsn_max(s->color.R, s->color.G),
       s->color.B
@@ -100,16 +100,16 @@ make_scene(dmnsn_array *spheres, dmnsn_canvas *canvas, dmnsn_camera *camera)
     } else {
       reflcolor = dmnsn_color_mul(0.25, dmnsn_white);
     }
-    sph->texture->finish.reflection = dmnsn_new_basic_reflection(
+    sphere->texture->finish.reflection = dmnsn_new_basic_reflection(
       dmnsn_black, reflcolor, 1.0
     );
 
-    sph->trans = dmnsn_matrix_mul(
+    sphere->trans = dmnsn_matrix_mul(
       dmnsn_translation_matrix(s->center),
       dmnsn_scale_matrix(dmnsn_new_vector(s->radius, s->radius, s->radius))
     );
 
-    dmnsn_array_push(scene->objects, &sph);
+    dmnsn_array_push(scene->objects, &sphere);
   }
 
   dmnsn_object *plane = dmnsn_new_plane(dmnsn_y);
@@ -149,48 +149,451 @@ make_scene(dmnsn_array *spheres, dmnsn_canvas *canvas, dmnsn_camera *camera)
   return scene;
 }
 
+#include "dimension-internal.h"
+#include <pthread.h>
+#include <stdlib.h>
+
+/** Number of children per PR-node. */
+#define PRTREE_B 8
+/** Number of priority leaves per pseudo-PR-node (must be 2*ndimensions). */
+#define PSEUDO_B 6
+
+/** A flat node for storing in an array for fast pre-order traversal. */
+typedef struct physics_flat_prnode {
+  dmnsn_bounding_box bounding_box;
+  physics_sphere *object;
+  size_t skip;
+} physics_flat_prnode;
+
+/** The side of the split that a node ended up on. */
+typedef enum physics_prnode_location {
+  PRTREE_LEAF, /**< Priority leaf. */
+  PRTREE_LEFT, /**< Left child. */
+  PRTREE_RIGHT /**< Right child. */
+} physics_prnode_location;
+
+/** Pseudo PR-tree node. */
+typedef struct physics_prnode {
+  dmnsn_bounding_box bounding_box;
+
+  physics_sphere *object;
+  struct physics_prnode *children[PRTREE_B];
+
+  physics_prnode_location location;
+} physics_prnode;
+
+/** Construct an empty PR-node. */
+static inline physics_prnode *
+physics_new_prnode(void)
+{
+  physics_prnode *node = dmnsn_malloc(sizeof(physics_prnode));
+  node->bounding_box = dmnsn_zero_bounding_box();
+  node->object       = NULL;
+  node->location     = PRTREE_LEFT; /* Mustn't be _LEAF */
+  for (size_t i = 0; i < PRTREE_B; ++i) {
+    node->children[i] = NULL;
+  }
+  return node;
+}
+
+/** Free a non-flat PR-tree. */
+static void
+physics_delete_prnode(physics_prnode *node)
+{
+  if (node) {
+    for (size_t i = 0; i < PRTREE_B && node->children[i]; ++i) {
+      physics_delete_prnode(node->children[i]);
+    }
+    dmnsn_free(node);
+  }
+}
+
+/** Expand a node to contain the bounding box \p box. */
+static void
+physics_prnode_swallow(physics_prnode *node, dmnsn_bounding_box box)
+{
+  node->bounding_box.min = dmnsn_vector_min(node->bounding_box.min, box.min);
+  node->bounding_box.max = dmnsn_vector_max(node->bounding_box.max, box.max);
+}
+
+/** Comparator types. */
+enum {
+  XMIN,
+  YMIN,
+  ZMIN,
+  XMAX,
+  YMAX,
+  ZMAX
+};
+
+/** Get a coordinate of the bounding box of a node. */
+static inline double
+physics_get_coordinate(const physics_prnode * const *node, int comparator)
+{
+  switch (comparator) {
+  case XMIN:
+    return (*node)->bounding_box.min.x;
+  case YMIN:
+    return (*node)->bounding_box.min.y;
+  case ZMIN:
+    return (*node)->bounding_box.min.z;
+
+  case XMAX:
+    return -(*node)->bounding_box.max.x;
+  case YMAX:
+    return -(*node)->bounding_box.max.y;
+  case ZMAX:
+    return -(*node)->bounding_box.max.z;
+
+  default:
+    dmnsn_unreachable("Invalid comparator.");
+  }
+}
+
+/* List sorting comparators */
+
+static int
+physics_xmin_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, XMIN);
+  double rval = physics_get_coordinate(r, XMIN);
+  return (lval > rval) - (lval < rval);
+}
+
+static int
+physics_ymin_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, YMIN);
+  double rval = physics_get_coordinate(r, YMIN);
+  return (lval > rval) - (lval < rval);
+}
+
+static int
+physics_zmin_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, ZMIN);
+  double rval = physics_get_coordinate(r, ZMIN);
+  return (lval > rval) - (lval < rval);
+}
+
+static int
+physics_xmax_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, XMAX);
+  double rval = physics_get_coordinate(r, XMAX);
+  return (lval > rval) - (lval < rval);
+}
+
+static int
+physics_ymax_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, YMAX);
+  double rval = physics_get_coordinate(r, YMAX);
+  return (lval > rval) - (lval < rval);
+}
+
+static int
+physics_zmax_comp(const void *l, const void *r)
+{
+  double lval = physics_get_coordinate(l, ZMAX);
+  double rval = physics_get_coordinate(r, ZMAX);
+  return (lval > rval) - (lval < rval);
+}
+
+/** All comparators. */
+static dmnsn_array_comparator_fn *const physics_comparators[PSEUDO_B] = {
+  [XMIN] = physics_xmin_comp,
+  [YMIN] = physics_ymin_comp,
+  [ZMIN] = physics_zmin_comp,
+  [XMAX] = physics_xmax_comp,
+  [YMAX] = physics_ymax_comp,
+  [ZMAX] = physics_zmax_comp
+};
+
+/** Add the priority leaves for this level. */
+static void
+physics_add_priority_leaves(dmnsn_array *sorted_leaves[PSEUDO_B],
+                          dmnsn_array *new_leaves)
+{
+  for (size_t i = 0; i < PSEUDO_B; ++i) {
+    physics_prnode *leaf = NULL;
+    physics_prnode **leaves = dmnsn_array_first(sorted_leaves[i]);
+    for (size_t j = 0, count = 0, size = dmnsn_array_size(sorted_leaves[i]);
+         j < size && count < PRTREE_B;
+         ++j)
+    {
+      /* Skip all the previously found extreme nodes */
+      if (leaves[j]->location == PRTREE_LEAF) {
+        continue;
+      }
+
+      if (!leaf) {
+        leaf = physics_new_prnode();
+      }
+      leaves[j]->location = PRTREE_LEAF;
+      leaf->children[count++] = leaves[j];
+      physics_prnode_swallow(leaf, leaves[j]->bounding_box);
+    }
+
+    if (leaf) {
+      dmnsn_array_push(new_leaves, &leaf);
+    } else {
+      return;
+    }
+  }
+}
+
+/** Split the sorted lists into the left and right subtrees. */
+static bool
+physics_split_sorted_leaves(dmnsn_array *sorted_leaves[PSEUDO_B],
+                          dmnsn_array *right_sorted_leaves[PSEUDO_B],
+                          size_t i)
+{
+  /* Get rid of the extreme nodes */
+  physics_prnode **leaves = dmnsn_array_first(sorted_leaves[i]);
+  size_t j, skip;
+  for (j = 0, skip = 0; j < dmnsn_array_size(sorted_leaves[i]); ++j) {
+    if (leaves[j]->location == PRTREE_LEAF) {
+      ++skip;
+    } else {
+      leaves[j - skip] = leaves[j];
+    }
+  }
+  dmnsn_array_resize(sorted_leaves[i], j - skip);
+
+  if (dmnsn_array_size(sorted_leaves[i]) == 0) {
+    return false;
+  }
+
+  /* Split the appropriate list and mark the left and right child nodes */
+  right_sorted_leaves[i] = dmnsn_array_split(sorted_leaves[i]);
+  DMNSN_ARRAY_FOREACH (physics_prnode **, node, sorted_leaves[i]) {
+    (*node)->location = PRTREE_LEFT;
+  }
+  DMNSN_ARRAY_FOREACH (physics_prnode **, node, right_sorted_leaves[i]) {
+    (*node)->location = PRTREE_RIGHT;
+  }
+
+  /* Split the rest of the lists */
+  for (size_t j = 0; j < PSEUDO_B; ++j) {
+    if (j != i) {
+      right_sorted_leaves[j] = dmnsn_new_array(sizeof(physics_prnode *));
+
+      physics_prnode **leaves = dmnsn_array_first(sorted_leaves[j]);
+      size_t k, skip;
+      for (k = 0, skip = 0; k < dmnsn_array_size(sorted_leaves[j]); ++k) {
+        if (leaves[k]->location == PRTREE_LEAF) {
+          ++skip;
+        } else if (leaves[k]->location == PRTREE_RIGHT) {
+          dmnsn_array_push(right_sorted_leaves[j], &leaves[k]);
+          ++skip;
+        } else {
+          leaves[k - skip] = leaves[k];
+        }
+      }
+      dmnsn_array_resize(sorted_leaves[j], k - skip);
+    }
+  }
+
+  return true;
+}
+
+/** Recursively constructs an implicit pseudo-PR-tree and collects the priority
+    leaves. */
+static void
+physics_priority_leaves_recursive(dmnsn_array *sorted_leaves[PSEUDO_B],
+                                dmnsn_array *new_leaves,
+                                int comparator)
+{
+  physics_add_priority_leaves(sorted_leaves, new_leaves);
+
+  dmnsn_array *right_sorted_leaves[PSEUDO_B];
+  if (physics_split_sorted_leaves(sorted_leaves, right_sorted_leaves, comparator))
+  {
+    physics_priority_leaves_recursive(right_sorted_leaves, new_leaves,
+                                    (comparator + 1)%PSEUDO_B);
+    for (size_t i = 0; i < PSEUDO_B; ++i) {
+      dmnsn_delete_array(right_sorted_leaves[i]);
+    }
+
+    physics_priority_leaves_recursive(sorted_leaves, new_leaves,
+                                    (comparator + 1)%PSEUDO_B);
+  }
+}
+
+/** Constructs an implicit pseudo-PR-tree and returns the priority leaves. */
+static dmnsn_array *
+physics_priority_leaves(const dmnsn_array *leaves)
+{
+  dmnsn_array *sorted_leaves[PSEUDO_B];
+  for (size_t i = 0; i < PSEUDO_B; ++i) {
+    sorted_leaves[i] = dmnsn_array_copy(leaves);
+    dmnsn_array_sort(sorted_leaves[i], physics_comparators[i]);
+  }
+
+  dmnsn_array *new_leaves = dmnsn_new_array(sizeof(physics_prnode *));
+  physics_priority_leaves_recursive(sorted_leaves, new_leaves, 0);
+
+  for (size_t i = 0; i < PSEUDO_B; ++i) {
+    dmnsn_delete_array(sorted_leaves[i]);
+  }
+
+  return new_leaves;
+}
+
+static inline dmnsn_bounding_box
+physics_sphere_bounding_box(const physics_sphere *sphere)
+{
+  dmnsn_vector rvec = dmnsn_new_vector(
+    sphere->radius,
+    sphere->radius,
+    sphere->radius
+  );
+  dmnsn_bounding_box box = {
+    .min = dmnsn_vector_sub(sphere->center, rvec),
+    .max = dmnsn_vector_add(sphere->center, rvec),
+  };
+  return box;
+}
+
+/** Construct a non-flat PR-tree. */
+static physics_prnode *
+physics_make_prtree(const dmnsn_array *objects)
+{
+  if (dmnsn_array_size(objects) == 0) {
+    return NULL;
+  }
+
+  /* Make the initial array of leaves */
+  dmnsn_array *leaves = dmnsn_new_array(sizeof(physics_prnode *));
+  DMNSN_ARRAY_FOREACH (physics_sphere *, object, objects) {
+    physics_prnode *node = physics_new_prnode();
+    node->bounding_box = physics_sphere_bounding_box(object);
+    node->object       = object;
+    dmnsn_array_push(leaves, &node);
+  }
+
+  while (dmnsn_array_size(leaves) > 1) {
+    dmnsn_array *new_leaves = physics_priority_leaves(leaves);
+    dmnsn_delete_array(leaves);
+    leaves = new_leaves;
+  }
+
+  physics_prnode *root = *(physics_prnode **)dmnsn_array_first(leaves);
+  dmnsn_delete_array(leaves);
+  return root;
+}
+
+/** Recursively flatten a PR-tree into an array of flat nodes. */
+static void
+physics_flatten_prtree_recursive(physics_prnode *node, dmnsn_array *flat)
+{
+  size_t currenti = dmnsn_array_size(flat);
+  dmnsn_array_resize(flat, currenti + 1);
+  physics_flat_prnode *flatnode = dmnsn_array_at(flat, currenti);
+
+  flatnode->bounding_box = node->bounding_box;
+  flatnode->object       = node->object;
+
+  for (size_t i = 0; i < PRTREE_B && node->children[i]; ++i) {
+    physics_flatten_prtree_recursive(node->children[i], flat);
+  }
+
+  /* Array could have been realloc()'d somewhere else above */
+  flatnode = dmnsn_array_at(flat, currenti);
+  flatnode->skip = dmnsn_array_size(flat) - currenti;
+}
+
+/** Flatten a PR-tree into an array of flat nodes. */
+static dmnsn_array *
+physics_flatten_prtree(physics_prnode *root)
+{
+  dmnsn_array *flat = dmnsn_new_array(sizeof(physics_flat_prnode));
+  if (root) {
+    physics_flatten_prtree_recursive(root, flat);
+  }
+  return flat;
+}
+
+/* Construct a PR-tree from a bulk of objects */
+dmnsn_array *
+physics_new_prtree(const dmnsn_array *objects)
+{
+  physics_prnode *root = physics_make_prtree(objects);
+  dmnsn_array *ret = physics_flatten_prtree(root);
+  physics_delete_prnode(root);
+  return ret;
+}
+
+static inline bool
+physics_bounding_box_intersects(dmnsn_bounding_box box,
+                                const physics_sphere *sphere)
+{
+  dmnsn_vector rvec = dmnsn_new_vector(
+    sphere->radius,
+    sphere->radius,
+    sphere->radius
+  );
+  box.min = dmnsn_vector_max(box.min, dmnsn_vector_sub(sphere->center, rvec));
+  box.max = dmnsn_vector_min(box.max, dmnsn_vector_add(sphere->center, rvec));
+  return box.min.x < box.max.x
+      && box.min.y < box.max.y
+      && box.min.z < box.max.z;
+}
+
 void
 integrate_spheres(dmnsn_array *spheres, double h)
 {
   static const double g = 5.0;
 
   /* Inter-object collision detection */
-  DMNSN_ARRAY_FOREACH (sphere *, s1, spheres) {
-    DMNSN_ARRAY_FOREACH (sphere *, s2, spheres) {
-      if (s1 == s2) continue;
-
-      dmnsn_vector deltar = dmnsn_vector_sub(s1->center, s2->center);
-      dmnsn_vector deltav = dmnsn_vector_sub(s1->velocity, s2->velocity);
-      if (dmnsn_vector_norm(deltar) <= s1->radius + s2->radius
-          && dmnsn_vector_dot(deltar, deltav) < 0.0)
-      {
-        dmnsn_vector x = dmnsn_vector_normalized(deltar);
-        dmnsn_vector v1 = s1->velocity;
-        double x1 = dmnsn_vector_dot(x, v1);
-        dmnsn_vector v1x = dmnsn_vector_mul(x1, x);
-        dmnsn_vector v1y = dmnsn_vector_sub(v1, v1x);
-
-        x = dmnsn_vector_negate(x);
-        dmnsn_vector v2 = s2->velocity;
-        double x2 = dmnsn_vector_dot(x, v2);
-        dmnsn_vector v2x = dmnsn_vector_mul(x2, x);
-        dmnsn_vector v2y = dmnsn_vector_sub(v2, v2x);
-
-        s1->velocity = dmnsn_vector_add(v2x, v1y);
-        s2->velocity = dmnsn_vector_add(v1x, v2y);
+  dmnsn_array *prtree = physics_new_prtree(spheres);
+  DMNSN_ARRAY_FOREACH (physics_sphere *, s1, spheres) {
+    physics_flat_prnode *node = dmnsn_array_first(prtree);
+    physics_flat_prnode *last = dmnsn_array_last(prtree);
+    while (node <= last) {
+      physics_sphere *s2 = node->object;
+      if (physics_bounding_box_intersects(node->bounding_box, s1)) {
+        if (s2 && s1 != s2) {
+          dmnsn_vector deltar = dmnsn_vector_sub(s1->center, s2->center);
+          dmnsn_vector deltav = dmnsn_vector_sub(s1->velocity, s2->velocity);
+          if (dmnsn_vector_norm(deltar) <= s1->radius + s2->radius
+              && dmnsn_vector_dot(deltar, deltav) < 0.0)
+          {
+            dmnsn_vector x = dmnsn_vector_normalized(deltar);
+            dmnsn_vector v1 = s1->velocity;
+            double x1 = dmnsn_vector_dot(x, v1);
+            dmnsn_vector v1x = dmnsn_vector_mul(x1, x);
+            dmnsn_vector v1y = dmnsn_vector_sub(v1, v1x);
+
+            x = dmnsn_vector_negate(x);
+            dmnsn_vector v2 = s2->velocity;
+            double x2 = dmnsn_vector_dot(x, v2);
+            dmnsn_vector v2x = dmnsn_vector_mul(x2, x);
+            dmnsn_vector v2y = dmnsn_vector_sub(v2, v2x);
+
+            s1->velocity = dmnsn_vector_add(v2x, v1y);
+            s2->velocity = dmnsn_vector_add(v1x, v2y);
+          }
+        }
+
+        ++node;
+      } else {
+        node += node->skip;
       }
     }
   }
+  dmnsn_delete_array(prtree);
 
   /* Floor collision detection */
-  DMNSN_ARRAY_FOREACH (sphere *, s, spheres) {
+  DMNSN_ARRAY_FOREACH (physics_sphere *, s, spheres) {
     if (s->center.y - s->radius <= -4.0) {
       s->velocity.y = fabs(s->velocity.y);
     }
   }
 
   /* Advance by the timestep */
-  DMNSN_ARRAY_FOREACH (sphere *, s, spheres) {
+  DMNSN_ARRAY_FOREACH (physics_sphere *, s, spheres) {
     s->center = dmnsn_vector_add(s->center, dmnsn_vector_mul(h, s->velocity));
     s->center.y -= g*h*h/2.0;
     s->velocity.y -= g*h;