/*
-Copyright (c) 2021 Ashley Rose Hale (LadyHavoc)
+Copyright (c) 2022 Ashley Rose Hale (LadyHavoc)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
THE SOFTWARE.
*/
+#include <math.h>
#include "convex.h"
-typedef struct convex_builder_state_s
+void convex_builder_initialize(convex_builder_state_t* b, float epsilon)
{
- // planes chosen to describe the volume
- int numplanes;
- float planes[64][4];
+ b->numcorners = 0;
+ b->numfaces = 0;
+ b->epsilon = 0.0f;
+}
- // corners of the solid described by the planes chosen so far
- int numcorners;
- float corners[128][3];
+// this is a variant of QuickHull that relies on the caller to provide points
+// in a reasonable order - the result will be the same regardless of point order
+// but it's more efficient if the furthest points are provided first
+//
+// this could be a little more efficient if we kept track of edges during the
+// build, but I think it may be more numerically stable this way
+void convex_builder_add_point(convex_builder_state_t* b, float x, float y, float z)
+{
+ int i, j, l;
+ convex_corner_t corner;
+ unsigned char removedcorner[CONVEX_MAX_CORNERS];
+ unsigned char removedface[CONVEX_MAX_FACES];
- // provided point cloud which we are trying to find an optimal fit for
- int numpoints;
- const float* points3f;
+ // we can't add any new points after max generations is reached
+ if (b->numcorners > CONVEX_MAX_CORNERS - 1 || b->numfaces > CONVEX_MAX_FACES - b->numcorners - 2)
+ return;
- // we consider points to be equivalent if they are within this distance
- float epsilon;
-}
-convex_builder_state_t;
+ // make a corner struct with the same layout we expect to use for vector ops
+ corner.x = x;
+ corner.y = y;
+ corner.z = z;
+ corner.w = 1.0f;
-float convex_normal_distance(const float *normal3f, int numpoints, const float* points3f)
-{
- int i;
- float d;
- float best = 0;
- best = points3f[0] * normal3f[0] + points3f[1] * normal3f[1] + points3f[2] * normal3f[2];
- for (i = 1; i < numpoints; i++)
+ float epsilon = b->epsilon;
+
+ // add the new corner to the bounding box
+ if (b->numcorners == 0)
{
- d = points3f[i * 3 + 0] * normal3f[0] + points3f[i * 3 + 1] * normal3f[1] + points3f[i * 3 + 2] * normal3f[2];
- if (best < d)
- best = d;
+ b->extents[0][0] = corner.x;
+ b->extents[0][1] = corner.y;
+ b->extents[0][2] = corner.z;
+ b->extents[1][0] = corner.x;
+ b->extents[1][1] = corner.y;
+ b->extents[1][2] = corner.z;
+ }
+ else
+ {
+ if (b->extents[0][0] > corner.x)
+ b->extents[0][0] = corner.x;
+ if (b->extents[0][1] > corner.y)
+ b->extents[0][1] = corner.y;
+ if (b->extents[0][2] > corner.z)
+ b->extents[0][2] = corner.z;
+ if (b->extents[1][0] < corner.x)
+ b->extents[1][0] = corner.x;
+ if (b->extents[1][1] < corner.y)
+ b->extents[1][1] = corner.y;
+ if (b->extents[1][2] < corner.z)
+ b->extents[1][2] = corner.z;
}
- return best;
-}
-void convex_builder_initialize_for_point_cloud(convex_builder_state_t* b, int numpoints, const float* points3f)
-{
- int i, j, k, l;
- float aabb[2][3], e;
-
- // we'll be continuing to read the points provided by the caller
- b->numpoints = numpoints;
- b->points3f = points3f;
-
- // figure out the bounding box first as a starting point, this can be a
- // reasonable fit on its own, but more importantly it ensures we never
- // produce an unbounded solid
- aabb[0][0] = aabb[1][0] = points3f[0];
- aabb[0][1] = aabb[1][1] = points3f[1];
- aabb[0][2] = aabb[1][2] = points3f[2];
- b->epsilon = 0.0f;
- for (i = 0; i < numpoints; i++)
+ if (b->numfaces > 0)
{
- for (j = 0; j < 3; j++)
+ // determine which faces will be inside the resulting solid
+ for (i = 0; i < b->numfaces; i++)
{
- e = fabs(points3f[i * 3 + j]) * (1.0f / 1048576.0f);
- if (b->epsilon < e)
- b->epsilon = e;
- if (aabb[0][j] > points3f[i * 3 + j])
- aabb[0][j] = points3f[i * 3 + j];
- if (aabb[0][j] < points3f[i * 3 + j])
- aabb[0][j] = points3f[i * 3 + j];
+ convex_face_t* f = b->faces + i;
+ // face will be removed if it places this corner outside the solid
+ removedface[i] = (f->x * corner.x + f->y * corner.y + f->z * corner.z + f->w * corner.w) > epsilon;
}
- }
- b->numplanes = 6;
- for (i = 0; i < 6; i++)
- for (j = 0;j < 4;j++)
- b->planes[i][j] = 0;
- for (i = 0;i < 3;i++)
- {
- b->planes[i * 2 + 0][i] = 1;
- b->planes[i * 2 + 0][3] = aabb[1][i];
- b->planes[i * 2 + 1][i] = -1;
- b->planes[i * 2 + 1][3] = -aabb[0][i];
+
+ // scan for removed faces
+ for (i = 0; i < b->numfaces; i++)
+ if (removedface[i])
+ break;
+
+ // exit early if point is completely inside the solid
+ if (i == b->numfaces)
+ return;
+
+ // garbage collect the removed faces
+ for (j = i + 1; j < b->numfaces; j++)
+ if (!removedface[j])
+ b->faces[i++] = b->faces[j];
+ b->numfaces = i;
}
- // create the corners of the box
- b->numcorners = 8;
- for (i = 0; i < 2; i++)
+ // iterate active corners to create replacement faces using the new corner
+ for (i = 0; i < b->numcorners; i++)
{
- for (j = 0; j < 2; j++)
+ convex_corner_t ca = b->corners[i];
+ for (j = 0; j < b->numcorners; j++)
{
- for (k = 0; k < 2; k++)
+ // using the same point twice would make a degenerate plane
+ if (i == j)
+ continue;
+ convex_corner_t cb = b->corners[j];
+ // calculate the edge directions
+ convex_corner_t d, e;
+ convex_face_t face;
+ d.x = ca.x - cb.x;
+ d.y = ca.y - cb.y;
+ d.z = ca.z - cb.z;
+ d.w = 0.0f;
+ e.x = corner.x - cb.x;
+ e.y = corner.y - cb.y;
+ e.z = corner.z - cb.z;
+ e.w = 0.0f;
+ // cross product to produce a normal; this is not unit length,
+ // its length is the volume of the triangle *2
+ face.x = d.y * e.z - d.z * e.y;
+ face.y = d.z * e.x - d.x * e.z;
+ face.z = d.x * e.y - d.y * e.x;
+ float len2 = face.x * face.x + face.y * face.y + face.z * face.z;
+ if (len2 == 0.0f)
+ {
+ // we can't do anything with a degenerate plane
+ continue;
+ }
+ // normalize the plane normal
+ float inv = 1.0f / sqrt(len2);
+ face.x *= inv;
+ face.y *= inv;
+ face.z *= inv;
+ face.w = -(corner.x * face.x + corner.y * face.y + corner.z * face.z);
+ // flip the face if it's backwards (not facing center)
+ if ((b->extents[0][0] + b->extents[1][0]) * 0.5f * face.x + (b->extents[0][1] + b->extents[1][1]) * 0.5f * face.y + (b->extents[0][2] + b->extents[1][2]) * 0.5f * face.z + face.w > 0.0f)
+ {
+ face.x *= -1.0f;
+ face.y *= -1.0f;
+ face.z *= -1.0f;
+ face.w *= -1.0f;
+ }
+ // discard the proposed face if it slices through the solid
+ for (l = 0; l < b->numcorners; l++)
{
- b->corners[i * 4 + j * 2 + k][0] = aabb[i][0];
- b->corners[i * 4 + j * 2 + k][1] = aabb[j][1];
- b->corners[i * 4 + j * 2 + k][2] = aabb[k][2];
+ convex_corner_t cl = b->corners[l];
+ if (cl.x * face.x + cl.y * face.y + cl.z * face.z + face.w > epsilon)
+ break;
}
+ if (l < b->numcorners)
+ continue;
+ // add the new face
+ b->faces[b->numfaces++] = face;
}
}
-}
+ // discard any corners that are no longer on the surface of the solid
+ for (i = 0; i < b->numcorners; i++)
+ {
+ convex_corner_t ca = b->corners[i];
+ for (j = 0; j < b->numfaces; j++)
+ {
+ const convex_face_t *f = b->faces + j;
+ if (ca.x * f->x + ca.y * f->y + ca.z * f->z + ca.w * f->w > -epsilon)
+ break;
+ }
+ // if we didn't find any face that uses this corner, remove the corner
+ removedcorner[i] = (j == b->numfaces);
+ }
-void convex_builder_pick_best_planes(convex_builder_state_t* b, int maxplanes)
+ // scan for removed corners and remove them
+ for (i = 0; i < b->numcorners; i++)
+ if (removedcorner[i])
+ break;
+ for (j = i + 1;j < b->numcorners;j++)
+ if (!removedcorner[j])
+ b->corners[i++] = b->corners[j];
+ b->numcorners = i;
+
+ // add the new corner
+ b->corners[b->numcorners++] = corner;
+}
+
+int convex_builder_get_planes4f(convex_builder_state_t* b, float* outplanes4f, int maxplanes, int positivew)
{
- int i, j, k, l;
- int numplanes = 0;
- float planes[64][4];
- float aabb[2][3], ca[3], cb[3], cn[3], plane[2][4], p[3][3], d[2];
- float volume = 0, clen2, inv;
-
- // iterate all possible planes we could construct from the
- // provided points
- for (i = 0; i < b->numpoints - 2; i++)
+ int i;
+ int n = b->numfaces < maxplanes ? b->numfaces : maxplanes;
+ if (positivew)
{
- for (j = i + 1; j < b->numpoints - 1; j++)
+ for (i = 0; i < n; i++)
{
- for (k = j + 1; k < b->numpoints; k++)
- {
- // for each unique triplet of points [i,j,k] we visit only the
- // canonical ordering i<j<k, so we have to produce two opposite
- // planes; it would be worse to visit all orderings of [i,j,k]
- // because that would produce 6 planes using 6 cross products,
- // this way we produce two planes using one cross product.
-
- // calculate the edge directions
- for (l = 0; l < 3; l++)
- {
- p[0][l] = b->points3f[i * 3 + l];
- p[1][l] = b->points3f[j * 3 + l];
- p[2][l] = b->points3f[k * 3 + l];
- ca[l] = p[1][l] - p[0][l];
- cb[l] = p[2][l] - p[0][l];
- }
- // cross product to produce a normal; this is not unit length,
- // its length is the volume of the triangle *2
- cn[0] = ca[1] * cb[2] - ca[2] * cb[1];
- cn[1] = ca[2] * cb[0] - ca[0] * cb[2];
- cn[2] = ca[0] * cb[1] - ca[1] * cb[0];
- clen2 = cn[0] * cn[0] + cn[1] * cn[1] + cn[2] * cn[2];
- if (clen2 == 0.0f)
- {
- // we can't do anything with a degenerate plane
- continue;
- }
- // normalize the plane normal
- inv = 1.0f / sqrt(clen2);
- for (l = 0; l < 3; l++)
- {
- plane[0][l] = cn[l] * inv;
- plane[1][l] = plane[0][l] * -1.0f;
- }
- // calculate the plane distance of the point triplet
- plane[0][3] = convex_normal_distance(plane[0], 3, p);
- plane[1][3] = plane[0][3] * -1.0f;
- for (l = 0; l < 2; l++)
- {
- // reject the plane if it puts any points outside of the solid
- d[l] = convex_normal_distance(plane[l], b->numpoints, b->points3f);
- if (d[l] - plane[l][3] > b->epsilon)
- continue;
- // measure how much this plane carves the volume
- TODO;
- }
- }
+ const convex_face_t* f = b->faces + i;
+ outplanes4f[i * 4 + 0] = f->x;
+ outplanes4f[i * 4 + 1] = f->y;
+ outplanes4f[i * 4 + 2] = f->z;
+ outplanes4f[i * 4 + 3] = f->w * -1.0f;
+ }
+ }
+ else
+ {
+ for (i = 0; i < n; i++)
+ {
+ const convex_face_t* f = b->faces + i;
+ outplanes4f[i * 4 + 0] = f->x;
+ outplanes4f[i * 4 + 1] = f->y;
+ outplanes4f[i * 4 + 2] = f->z;
+ outplanes4f[i * 4 + 3] = f->w;
}
}
+ return b->numfaces;
}
-void convex_planes_for_point_cloud(int* outnumplanes, float* outplanes4f, int maxplanes, int numpoints, float* points3f)
+int convex_builder_get_points3f(convex_builder_state_t *b, float* outpoints3f, int maxpoints)
{
- // The algorithm here is starting with a suboptimal fit such as an axis-aligned bounding box, and then attempting to carve the largest portions of it away by picking better planes (i.e. largest volume removed) from triangles composed of the arbitrary points, so this means we need a way to measure volume of the carved space.
- convex_builder_state_t b;
-
- // return early if there are no points, rather than crash
- *outnumplanes = 0;
- if (numpoints < 1)
- return;
-
- // first we create a box from the points
- convex_builder_initialize_for_point_cloud(&b, numpoints, points3f);
-
- // optimize the convex solid as best we can
- convex_builder_pick_best_planes(&b, maxplanes);
-
+ int i;
+ int n = b->numcorners < maxpoints ? b->numcorners : maxpoints;
+ for (i = 0; i < n; i++)
+ {
+ const convex_corner_t* c = b->corners + i;
+ outpoints3f[i * 3 + 0] = c->x;
+ outpoints3f[i * 3 + 1] = c->y;
+ outpoints3f[i * 3 + 2] = c->z;
+ }
+ return b->numcorners;
}
projects / xonotic / darkplaces.git / blobdiff