convex.c

   1 /*
   2 Copyright (c) 2021 Ashley Rose Hale (LadyHavoc)
   3
   4 Permission is hereby granted, free of charge, to any person obtaining a copy
   5 of this software and associated documentation files (the "Software"), to deal
   6 in the Software without restriction, including without limitation the rights
   7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   8 copies of the Software, and to permit persons to whom the Software is
   9 furnished to do so, subject to the following conditions:
  10
  11 The above copyright notice and this permission notice shall be included in
  12 all copies or substantial portions of the Software.
  13
  14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  20 THE SOFTWARE.
  21 */
  22
  23 #include "convex.h"
  24
  25 typedef struct convex_builder_state_s
  26 {
  27         // planes chosen to describe the volume
  28         int numplanes;
  29         float planes[64][4];
  30
  31         // corners of the solid described by the planes chosen so far
  32         int numcorners;
  33         float corners[128][3];
  34
  35         // provided point cloud which we are trying to find an optimal fit for
  36         int numpoints;
  37         const float* points3f;
  38
  39         // we consider points to be equivalent if they are within this distance
  40         float epsilon;
  41 }
  42 convex_builder_state_t;
  43
  44 float convex_normal_distance(const float *normal3f, int numpoints, const float* points3f)
  45 {
  46         int i;
  47         float d;
  48         float best = 0;
  49         best = points3f[0] * normal3f[0] + points3f[1] * normal3f[1] + points3f[2] * normal3f[2];
  50         for (i = 1; i < numpoints; i++)
  51         {
  52                 d = points3f[i * 3 + 0] * normal3f[0] + points3f[i * 3 + 1] * normal3f[1] + points3f[i * 3 + 2] * normal3f[2];
  53                 if (best < d)
  54                         best = d;
  55         }
  56         return best;
  57 }
  58
  59 void convex_builder_initialize_for_point_cloud(convex_builder_state_t* b, int numpoints, const float* points3f)
  60 {
  61         int i, j, k, l;
  62         float aabb[2][3], e;
  63
  64         // we'll be continuing to read the points provided by the caller
  65         b->numpoints = numpoints;
  66         b->points3f = points3f;
  67
  68         // figure out the bounding box first as a starting point, this can be a
  69         // reasonable fit on its own, but more importantly it ensures we never
  70         // produce an unbounded solid
  71         aabb[0][0] = aabb[1][0] = points3f[0];
  72         aabb[0][1] = aabb[1][1] = points3f[1];
  73         aabb[0][2] = aabb[1][2] = points3f[2];
  74         b->epsilon = 0.0f;
  75         for (i = 0; i < numpoints; i++)
  76         {
  77                 for (j = 0; j < 3; j++)
  78                 {
  79                         e = fabs(points3f[i * 3 + j]) * (1.0f / 1048576.0f);
  80                         if (b->epsilon < e)
  81                                 b->epsilon = e;
  82                         if (aabb[0][j] > points3f[i * 3 + j])
  83                                 aabb[0][j] = points3f[i * 3 + j];
  84                         if (aabb[0][j] < points3f[i * 3 + j])
  85                                 aabb[0][j] = points3f[i * 3 + j];
  86                 }
  87         }
  88         b->numplanes = 6;
  89         for (i = 0; i < 6; i++)
  90                 for (j = 0;j < 4;j++)
  91                         b->planes[i][j] = 0;
  92         for (i = 0;i < 3;i++)
  93         {
  94                 b->planes[i * 2 + 0][i] = 1;
  95                 b->planes[i * 2 + 0][3] = aabb[1][i];
  96                 b->planes[i * 2 + 1][i] = -1;
  97                 b->planes[i * 2 + 1][3] = -aabb[0][i];
  98         }
  99
 100         // create the corners of the box
 101         b->numcorners = 8;
 102         for (i = 0; i < 2; i++)
 103         {
 104                 for (j = 0; j < 2; j++)
 105                 {
 106                         for (k = 0; k < 2; k++)
 107                         {
 108                                 b->corners[i * 4 + j * 2 + k][0] = aabb[i][0];
 109                                 b->corners[i * 4 + j * 2 + k][1] = aabb[j][1];
 110                                 b->corners[i * 4 + j * 2 + k][2] = aabb[k][2];
 111                         }
 112                 }
 113         }
 114 }
 115
 116
 117 void convex_builder_pick_best_planes(convex_builder_state_t* b, int maxplanes)
 118 {
 119         int i, j, k, l;
 120         int numplanes = 0;
 121         float planes[64][4];
 122         float aabb[2][3], ca[3], cb[3], cn[3], plane[2][4], p[3][3], d[2];
 123         float volume = 0, clen2, inv;
 124
 125         // iterate all possible planes we could construct from the
 126         // provided points
 127         for (i = 0; i < b->numpoints - 2; i++)
 128         {
 129                 for (j = i + 1; j < b->numpoints - 1; j++)
 130                 {
 131                         for (k = j + 1; k < b->numpoints; k++)
 132                         {
 133                                 // for each unique triplet of points [i,j,k] we visit only the
 134                                 // canonical ordering i<j<k, so we have to produce two opposite
 135                                 // planes; it would be worse to visit all orderings of [i,j,k]
 136                                 // because that would produce 6 planes using 6 cross products,
 137                                 // this way we produce two planes using one cross product.
 138
 139                                 // calculate the edge directions
 140                                 for (l = 0; l < 3; l++)
 141                                 {
 142                                         p[0][l] = b->points3f[i * 3 + l];
 143                                         p[1][l] = b->points3f[j * 3 + l];
 144                                         p[2][l] = b->points3f[k * 3 + l];
 145                                         ca[l] = p[1][l] - p[0][l];
 146                                         cb[l] = p[2][l] - p[0][l];
 147                                 }
 148                                 // cross product to produce a normal; this is not unit length,
 149                                 // its length is the volume of the triangle *2
 150                                 cn[0] = ca[1] * cb[2] - ca[2] * cb[1];
 151                                 cn[1] = ca[2] * cb[0] - ca[0] * cb[2];
 152                                 cn[2] = ca[0] * cb[1] - ca[1] * cb[0];
 153                                 clen2 = cn[0] * cn[0] + cn[1] * cn[1] + cn[2] * cn[2];
 154                                 if (clen2 == 0.0f)
 155                                 {
 156                                         // we can't do anything with a degenerate plane
 157                                         continue;
 158                                 }
 159                                 // normalize the plane normal
 160                                 inv = 1.0f / sqrt(clen2);
 161                                 for (l = 0; l < 3; l++)
 162                                 {
 163                                         plane[0][l] = cn[l] * inv;
 164                                         plane[1][l] = plane[0][l] * -1.0f;
 165                                 }
 166                                 // calculate the plane distance of the point triplet
 167                                 plane[0][3] = convex_normal_distance(plane[0], 3, p);
 168                                 plane[1][3] = plane[0][3] * -1.0f;
 169                                 for (l = 0; l < 2; l++)
 170                                 {
 171                                         // reject the plane if it puts any points outside of the solid
 172                                         d[l] = convex_normal_distance(plane[l], b->numpoints, b->points3f);
 173                                         if (d[l] - plane[l][3] > b->epsilon)
 174                                                 continue;
 175                                         // measure how much this plane carves the volume
 176                                         TODO;
 177                                 }
 178                         }
 179                 }
 180         }
 181 }
 182
 183 void convex_planes_for_point_cloud(int* outnumplanes, float* outplanes4f, int maxplanes, int numpoints, float* points3f)
 184 {
 185         // 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.
 186         convex_builder_state_t b;
 187
 188         // return early if there are no points, rather than crash
 189         *outnumplanes = 0;
 190         if (numpoints < 1)
 191                 return;
 192
 193         // first we create a box from the points
 194         convex_builder_initialize_for_point_cloud(&b, numpoints, points3f);
 195
 196         // optimize the convex solid as best we can
 197         convex_builder_pick_best_planes(&b, maxplanes);
 198
 199 }