summaryrefslogtreecommitdiffstats
path: root/libdimension/tests/physics.c
diff options
context:
space:
mode:
Diffstat (limited to 'libdimension/tests/physics.c')
-rw-r--r--libdimension/tests/physics.c481
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;