2 Copyright (c) 2022 Ashley Rose Hale (LadyHavoc)
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:
11 The above copyright notice and this permission notice shall be included in
12 all copies or substantial portions of the Software.
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
26 void convex_builder_initialize(convex_builder_state_t* b, float epsilon)
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
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)
42 convex_corner_t corner;
43 unsigned char removedcorner[CONVEX_MAX_CORNERS];
44 unsigned char removedface[CONVEX_MAX_FACES];
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)
50 // make a corner struct with the same layout we expect to use for vector ops
56 float epsilon = b->epsilon;
58 // add the new corner to the bounding box
59 if (b->numcorners == 0)
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;
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;
86 // determine which faces will be inside the resulting solid
87 for (i = 0; i < b->numfaces; i++)
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;
94 // scan for removed faces
95 for (i = 0; i < b->numfaces; i++)
99 // exit early if point is completely inside the solid
100 if (i == b->numfaces)
103 // garbage collect the removed faces
104 for (j = i + 1; j < b->numfaces; j++)
106 b->faces[i++] = b->faces[j];
110 // iterate active corners to create replacement faces using the new corner
111 for (i = 0; i < b->numcorners; i++)
113 convex_corner_t ca = b->corners[i];
114 for (j = 0; j < b->numcorners; j++)
116 // using the same point twice would make a degenerate plane
119 convex_corner_t cb = b->corners[j];
120 // calculate the edge directions
121 convex_corner_t d, e;
127 e.x = corner.x - cb.x;
128 e.y = corner.y - cb.y;
129 e.z = corner.z - cb.z;
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;
139 // we can't do anything with a degenerate plane
142 // normalize the plane normal
143 float inv = 1.0f / sqrt(len2);
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)
156 // discard the proposed face if it slices through the solid
157 for (l = 0; l < b->numcorners; l++)
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)
163 if (l < b->numcorners)
166 b->faces[b->numfaces++] = face;
170 // discard any corners that are no longer on the surface of the solid
171 for (i = 0; i < b->numcorners; i++)
173 convex_corner_t ca = b->corners[i];
174 for (j = 0; j < b->numfaces; j++)
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)
180 // if we didn't find any face that uses this corner, remove the corner
181 removedcorner[i] = (j == b->numfaces);
184 // scan for removed corners and remove them
185 for (i = 0; i < b->numcorners; i++)
186 if (removedcorner[i])
188 for (j = i + 1;j < b->numcorners;j++)
189 if (!removedcorner[j])
190 b->corners[i++] = b->corners[j];
193 // add the new corner
194 b->corners[b->numcorners++] = corner;
197 int convex_builder_get_planes4f(convex_builder_state_t* b, float* outplanes4f, int maxplanes, int positivew)
200 int n = b->numfaces < maxplanes ? b->numfaces : maxplanes;
203 for (i = 0; i < n; i++)
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;
214 for (i = 0; i < n; i++)
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;
226 int convex_builder_get_points3f(convex_builder_state_t *b, float* outpoints3f, int maxpoints)
229 int n = b->numcorners < maxpoints ? b->numcorners : maxpoints;
230 for (i = 0; i < n; i++)
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;
237 return b->numcorners;