Added convex.[ch] to compute brush shapes from point clouds for use in experimental...
[xonotic/darkplaces.git] / 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 }