convex.c

   1 /*
   2 Copyright (c) 2022 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 <math.h>
  24 #include "convex.h"
  25
  26 void convex_builder_initialize(convex_builder_state_t* b, float epsilon)
  27 {
  28         b->numcorners = 0;
  29         b->numfaces = 0;
  30         b->epsilon = 0.0f;
  31 }
  32
  33 // this is a variant of QuickHull that relies on the caller to provide points
  34 // in a reasonable order - the result will be the same regardless of point order
  35 // but it's more efficient if the furthest points are provided first
  36 //
  37 // this could be a little more efficient if we kept track of edges during the
  38 // build, but I think it may be more numerically stable this way
  39 void convex_builder_add_point(convex_builder_state_t* b, float x, float y, float z)
  40 {
  41         int i, j, l;
  42         convex_corner_t corner;
  43         unsigned char removedcorner[CONVEX_MAX_CORNERS];
  44         unsigned char removedface[CONVEX_MAX_FACES];
  45
  46         // we can't add any new points after max generations is reached
  47         if (b->numcorners > CONVEX_MAX_CORNERS - 1 || b->numfaces > CONVEX_MAX_FACES - b->numcorners - 2)
  48                 return;
  49
  50         // make a corner struct with the same layout we expect to use for vector ops
  51         corner.x = x;
  52         corner.y = y;
  53         corner.z = z;
  54         corner.w = 1.0f;
  55
  56         float epsilon = b->epsilon;
  57
  58         // add the new corner to the bounding box
  59         if (b->numcorners == 0)
  60         {
  61                 b->extents[0][0] = corner.x;
  62                 b->extents[0][1] = corner.y;
  63                 b->extents[0][2] = corner.z;
  64                 b->extents[1][0] = corner.x;
  65                 b->extents[1][1] = corner.y;
  66                 b->extents[1][2] = corner.z;
  67         }
  68         else
  69         {
  70                 if (b->extents[0][0] > corner.x)
  71                         b->extents[0][0] = corner.x;
  72                 if (b->extents[0][1] > corner.y)
  73                         b->extents[0][1] = corner.y;
  74                 if (b->extents[0][2] > corner.z)
  75                         b->extents[0][2] = corner.z;
  76                 if (b->extents[1][0] < corner.x)
  77                         b->extents[1][0] = corner.x;
  78                 if (b->extents[1][1] < corner.y)
  79                         b->extents[1][1] = corner.y;
  80                 if (b->extents[1][2] < corner.z)
  81                         b->extents[1][2] = corner.z;
  82         }
  83
  84         if (b->numfaces > 0)
  85         {
  86                 // determine which faces will be inside the resulting solid
  87                 for (i = 0; i < b->numfaces; i++)
  88                 {
  89                         convex_face_t* f = b->faces + i;
  90                         // face will be removed if it places this corner outside the solid
  91                         removedface[i] = (f->x * corner.x + f->y * corner.y + f->z * corner.z + f->w * corner.w) > epsilon;
  92                 }
  93
  94                 // scan for removed faces
  95                 for (i = 0; i < b->numfaces; i++)
  96                         if (removedface[i])
  97                                 break;
  98
  99                 // exit early if point is completely inside the solid
 100                 if (i == b->numfaces)
 101                         return;
 102
 103                 // garbage collect the removed faces
 104                 for (j = i + 1; j < b->numfaces; j++)
 105                         if (!removedface[j])
 106                                 b->faces[i++] = b->faces[j];
 107                 b->numfaces = i;
 108         }
 109
 110         // iterate active corners to create replacement faces using the new corner
 111         for (i = 0; i < b->numcorners; i++)
 112         {
 113                 convex_corner_t ca = b->corners[i];
 114                 for (j = 0; j < b->numcorners; j++)
 115                 {
 116                         // using the same point twice would make a degenerate plane
 117                         if (i == j)
 118                                 continue;
 119                         convex_corner_t cb = b->corners[j];
 120                         // calculate the edge directions
 121                         convex_corner_t d, e;
 122                         convex_face_t face;
 123                         d.x = ca.x - cb.x;
 124                         d.y = ca.y - cb.y;
 125                         d.z = ca.z - cb.z;
 126                         d.w = 0.0f;
 127                         e.x = corner.x - cb.x;
 128                         e.y = corner.y - cb.y;
 129                         e.z = corner.z - cb.z;
 130                         e.w = 0.0f;
 131                         // cross product to produce a normal; this is not unit length,
 132                         // its length is the volume of the triangle *2
 133                         face.x = d.y * e.z - d.z * e.y;
 134                         face.y = d.z * e.x - d.x * e.z;
 135                         face.z = d.x * e.y - d.y * e.x;
 136                         float len2 = face.x * face.x + face.y * face.y + face.z * face.z;
 137                         if (len2 == 0.0f)
 138                         {
 139                                 // we can't do anything with a degenerate plane
 140                                 continue;
 141                         }
 142                         // normalize the plane normal
 143                         float inv = 1.0f / sqrt(len2);
 144                         face.x *= inv;
 145                         face.y *= inv;
 146                         face.z *= inv;
 147                         face.w = -(corner.x * face.x + corner.y * face.y + corner.z * face.z);
 148                         // flip the face if it's backwards (not facing center)
 149                         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)
 150                         {
 151                                 face.x *= -1.0f;
 152                                 face.y *= -1.0f;
 153                                 face.z *= -1.0f;
 154                                 face.w *= -1.0f;
 155                         }
 156                         // discard the proposed face if it slices through the solid
 157                         for (l = 0; l < b->numcorners; l++)
 158                         {
 159                                 convex_corner_t cl = b->corners[l];
 160                                 if (cl.x * face.x + cl.y * face.y + cl.z * face.z + face.w > epsilon)
 161                                         break;
 162                         }
 163                         if (l < b->numcorners)
 164                                 continue;
 165                         // add the new face
 166                         b->faces[b->numfaces++] = face;
 167                 }
 168         }
 169
 170         // discard any corners that are no longer on the surface of the solid
 171         for (i = 0; i < b->numcorners; i++)
 172         {
 173                 convex_corner_t ca = b->corners[i];
 174                 for (j = 0; j < b->numfaces; j++)
 175                 {
 176                         const convex_face_t *f = b->faces + j;
 177                         if (ca.x * f->x + ca.y * f->y + ca.z * f->z + ca.w * f->w > -epsilon)
 178                                 break;
 179                 }
 180                 // if we didn't find any face that uses this corner, remove the corner
 181                 removedcorner[i] = (j == b->numfaces);
 182         }
 183
 184         // scan for removed corners and remove them
 185         for (i = 0; i < b->numcorners; i++)
 186                 if (removedcorner[i])
 187                         break;
 188         for (j = i + 1;j < b->numcorners;j++)
 189                 if (!removedcorner[j])
 190                         b->corners[i++] = b->corners[j];
 191         b->numcorners = i;
 192
 193         // add the new corner
 194         b->corners[b->numcorners++] = corner;
 195 }
 196
 197 int convex_builder_get_planes4f(convex_builder_state_t* b, float* outplanes4f, int maxplanes, int positivew)
 198 {
 199         int i;
 200         int n = b->numfaces < maxplanes ? b->numfaces : maxplanes;
 201         if (positivew)
 202         {
 203                 for (i = 0; i < n; i++)
 204                 {
 205                         const convex_face_t* f = b->faces + i;
 206                         outplanes4f[i * 4 + 0] = f->x;
 207                         outplanes4f[i * 4 + 1] = f->y;
 208                         outplanes4f[i * 4 + 2] = f->z;
 209                         outplanes4f[i * 4 + 3] = f->w * -1.0f;
 210                 }
 211         }
 212         else
 213         {
 214                 for (i = 0; i < n; i++)
 215                 {
 216                         const convex_face_t* f = b->faces + i;
 217                         outplanes4f[i * 4 + 0] = f->x;
 218                         outplanes4f[i * 4 + 1] = f->y;
 219                         outplanes4f[i * 4 + 2] = f->z;
 220                         outplanes4f[i * 4 + 3] = f->w;
 221                 }
 222         }
 223         return b->numfaces;
 224 }
 225
 226 int convex_builder_get_points3f(convex_builder_state_t *b, float* outpoints3f, int maxpoints)
 227 {
 228         int i;
 229         int n = b->numcorners < maxpoints ? b->numcorners : maxpoints;
 230         for (i = 0; i < n; i++)
 231         {
 232                 const convex_corner_t* c = b->corners + i;
 233                 outpoints3f[i * 3 + 0] = c->x;
 234                 outpoints3f[i * 3 + 1] = c->y;
 235                 outpoints3f[i * 3 + 2] = c->z;
 236         }
 237         return b->numcorners;
 238 }