1 #include "btMiniSDF.h"
2
3 //
4 //Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
5 //example:
6 //GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
7 //The MIT License (MIT)
8 //
9 //Copyright (c) 2017 Dan Koschier
10 //
11
12 #include <limits.h>
13 #include <string.h> //memcpy
14
15 struct btSdfDataStream
16 {
17 const char* m_data;
18 int m_size;
19
20 int m_currentOffset;
21
btSdfDataStreambtSdfDataStream22 btSdfDataStream(const char* data, int size)
23 : m_data(data),
24 m_size(size),
25 m_currentOffset(0)
26 {
27 }
28
29 template <class T>
readbtSdfDataStream30 bool read(T& val)
31 {
32 int bytes = sizeof(T);
33 if (m_currentOffset + bytes <= m_size)
34 {
35 char* dest = (char*)&val;
36 memcpy(dest, &m_data[m_currentOffset], bytes);
37 m_currentOffset += bytes;
38 return true;
39 }
40 btAssert(0);
41 return false;
42 }
43 };
44
load(const char * data,int size)45 bool btMiniSDF::load(const char* data, int size)
46 {
47 int fileSize = -1;
48
49 btSdfDataStream ds(data, size);
50 {
51 double buf[6];
52 ds.read(buf);
53 m_domain.m_min[0] = buf[0];
54 m_domain.m_min[1] = buf[1];
55 m_domain.m_min[2] = buf[2];
56 m_domain.m_min[3] = 0;
57 m_domain.m_max[0] = buf[3];
58 m_domain.m_max[1] = buf[4];
59 m_domain.m_max[2] = buf[5];
60 m_domain.m_max[3] = 0;
61 }
62 {
63 unsigned int buf2[3];
64 ds.read(buf2);
65 m_resolution[0] = buf2[0];
66 m_resolution[1] = buf2[1];
67 m_resolution[2] = buf2[2];
68 }
69 {
70 double buf[3];
71 ds.read(buf);
72 m_cell_size[0] = buf[0];
73 m_cell_size[1] = buf[1];
74 m_cell_size[2] = buf[2];
75 }
76 {
77 double buf[3];
78 ds.read(buf);
79 m_inv_cell_size[0] = buf[0];
80 m_inv_cell_size[1] = buf[1];
81 m_inv_cell_size[2] = buf[2];
82 }
83 {
84 unsigned long long int cells;
85 ds.read(cells);
86 m_n_cells = cells;
87 }
88 {
89 unsigned long long int fields;
90 ds.read(fields);
91 m_n_fields = fields;
92 }
93
94 unsigned long long int nodes0;
95 std::size_t n_nodes0;
96 ds.read(nodes0);
97 n_nodes0 = nodes0;
98 if (n_nodes0 > 1024 * 1024 * 1024)
99 {
100 return m_isValid;
101 }
102 m_nodes.resize(n_nodes0);
103 for (unsigned int i = 0; i < n_nodes0; i++)
104 {
105 unsigned long long int n_nodes1;
106 ds.read(n_nodes1);
107 btAlignedObjectArray<double>& nodes = m_nodes[i];
108 nodes.resize(n_nodes1);
109 for (int j = 0; j < nodes.size(); j++)
110 {
111 double& node = nodes[j];
112 ds.read(node);
113 }
114 }
115
116 unsigned long long int n_cells0;
117 ds.read(n_cells0);
118 m_cells.resize(n_cells0);
119 for (int i = 0; i < n_cells0; i++)
120 {
121 unsigned long long int n_cells1;
122 btAlignedObjectArray<btCell32>& cells = m_cells[i];
123 ds.read(n_cells1);
124 cells.resize(n_cells1);
125 for (int j = 0; j < n_cells1; j++)
126 {
127 btCell32& cell = cells[j];
128 ds.read(cell);
129 }
130 }
131
132 {
133 unsigned long long int n_cell_maps0;
134 ds.read(n_cell_maps0);
135
136 m_cell_map.resize(n_cell_maps0);
137 for (int i = 0; i < n_cell_maps0; i++)
138 {
139 unsigned long long int n_cell_maps1;
140 btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i];
141 ds.read(n_cell_maps1);
142 cell_maps.resize(n_cell_maps1);
143 for (int j = 0; j < n_cell_maps1; j++)
144 {
145 unsigned int& cell_map = cell_maps[j];
146 ds.read(cell_map);
147 }
148 }
149 }
150
151 m_isValid = (ds.m_currentOffset == ds.m_size);
152 return m_isValid;
153 }
154
multiToSingleIndex(btMultiIndex const & ijk) const155 unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const
156 {
157 return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0];
158 }
159
160 btAlignedBox3d
subdomain(btMultiIndex const & ijk) const161 btMiniSDF::subdomain(btMultiIndex const& ijk) const
162 {
163 btAssert(m_isValid);
164 btVector3 tmp;
165 tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0];
166 tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1];
167 tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2];
168
169 btVector3 origin = m_domain.min() + tmp;
170
171 btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size);
172 return box;
173 }
174
175 btMultiIndex
singleToMultiIndex(unsigned int l) const176 btMiniSDF::singleToMultiIndex(unsigned int l) const
177 {
178 btAssert(m_isValid);
179 unsigned int n01 = m_resolution[0] * m_resolution[1];
180 unsigned int k = l / n01;
181 unsigned int temp = l % n01;
182 unsigned int j = temp / m_resolution[0];
183 unsigned int i = temp % m_resolution[0];
184 btMultiIndex mi;
185 mi.ijk[0] = i;
186 mi.ijk[1] = j;
187 mi.ijk[2] = k;
188 return mi;
189 }
190
191 btAlignedBox3d
subdomain(unsigned int l) const192 btMiniSDF::subdomain(unsigned int l) const
193 {
194 btAssert(m_isValid);
195 return subdomain(singleToMultiIndex(l));
196 }
197
198 btShapeMatrix
shape_function_(btVector3 const & xi,btShapeGradients * gradient) const199 btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const
200 {
201 btAssert(m_isValid);
202 btShapeMatrix res;
203
204 btScalar x = xi[0];
205 btScalar y = xi[1];
206 btScalar z = xi[2];
207
208 btScalar x2 = x * x;
209 btScalar y2 = y * y;
210 btScalar z2 = z * z;
211
212 btScalar _1mx = 1.0 - x;
213 btScalar _1my = 1.0 - y;
214 btScalar _1mz = 1.0 - z;
215
216 btScalar _1px = 1.0 + x;
217 btScalar _1py = 1.0 + y;
218 btScalar _1pz = 1.0 + z;
219
220 btScalar _1m3x = 1.0 - 3.0 * x;
221 btScalar _1m3y = 1.0 - 3.0 * y;
222 btScalar _1m3z = 1.0 - 3.0 * z;
223
224 btScalar _1p3x = 1.0 + 3.0 * x;
225 btScalar _1p3y = 1.0 + 3.0 * y;
226 btScalar _1p3z = 1.0 + 3.0 * z;
227
228 btScalar _1mxt1my = _1mx * _1my;
229 btScalar _1mxt1py = _1mx * _1py;
230 btScalar _1pxt1my = _1px * _1my;
231 btScalar _1pxt1py = _1px * _1py;
232
233 btScalar _1mxt1mz = _1mx * _1mz;
234 btScalar _1mxt1pz = _1mx * _1pz;
235 btScalar _1pxt1mz = _1px * _1mz;
236 btScalar _1pxt1pz = _1px * _1pz;
237
238 btScalar _1myt1mz = _1my * _1mz;
239 btScalar _1myt1pz = _1my * _1pz;
240 btScalar _1pyt1mz = _1py * _1mz;
241 btScalar _1pyt1pz = _1py * _1pz;
242
243 btScalar _1mx2 = 1.0 - x2;
244 btScalar _1my2 = 1.0 - y2;
245 btScalar _1mz2 = 1.0 - z2;
246
247 // Corner nodes.
248 btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0);
249 res[0] = fac * _1mxt1my * _1mz;
250 res[1] = fac * _1pxt1my * _1mz;
251 res[2] = fac * _1mxt1py * _1mz;
252 res[3] = fac * _1pxt1py * _1mz;
253 res[4] = fac * _1mxt1my * _1pz;
254 res[5] = fac * _1pxt1my * _1pz;
255 res[6] = fac * _1mxt1py * _1pz;
256 res[7] = fac * _1pxt1py * _1pz;
257
258 // Edge nodes.
259
260 fac = 9.0 / 64.0 * _1mx2;
261 btScalar fact1m3x = fac * _1m3x;
262 btScalar fact1p3x = fac * _1p3x;
263 res[8] = fact1m3x * _1myt1mz;
264 res[9] = fact1p3x * _1myt1mz;
265 res[10] = fact1m3x * _1myt1pz;
266 res[11] = fact1p3x * _1myt1pz;
267 res[12] = fact1m3x * _1pyt1mz;
268 res[13] = fact1p3x * _1pyt1mz;
269 res[14] = fact1m3x * _1pyt1pz;
270 res[15] = fact1p3x * _1pyt1pz;
271
272 fac = 9.0 / 64.0 * _1my2;
273 btScalar fact1m3y = fac * _1m3y;
274 btScalar fact1p3y = fac * _1p3y;
275 res[16] = fact1m3y * _1mxt1mz;
276 res[17] = fact1p3y * _1mxt1mz;
277 res[18] = fact1m3y * _1pxt1mz;
278 res[19] = fact1p3y * _1pxt1mz;
279 res[20] = fact1m3y * _1mxt1pz;
280 res[21] = fact1p3y * _1mxt1pz;
281 res[22] = fact1m3y * _1pxt1pz;
282 res[23] = fact1p3y * _1pxt1pz;
283
284 fac = 9.0 / 64.0 * _1mz2;
285 btScalar fact1m3z = fac * _1m3z;
286 btScalar fact1p3z = fac * _1p3z;
287 res[24] = fact1m3z * _1mxt1my;
288 res[25] = fact1p3z * _1mxt1my;
289 res[26] = fact1m3z * _1mxt1py;
290 res[27] = fact1p3z * _1mxt1py;
291 res[28] = fact1m3z * _1pxt1my;
292 res[29] = fact1p3z * _1pxt1my;
293 res[30] = fact1m3z * _1pxt1py;
294 res[31] = fact1p3z * _1pxt1py;
295
296 if (gradient)
297 {
298 btShapeGradients& dN = *gradient;
299
300 btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0;
301 btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0;
302 btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0;
303 btScalar _18x = 18.0 * x;
304 btScalar _18y = 18.0 * y;
305 btScalar _18z = 18.0 * z;
306
307 btScalar _3m9x2 = 3.0 - 9.0 * x2;
308 btScalar _3m9y2 = 3.0 - 9.0 * y2;
309 btScalar _3m9z2 = 3.0 - 9.0 * z2;
310
311 btScalar _2x = 2.0 * x;
312 btScalar _2y = 2.0 * y;
313 btScalar _2z = 2.0 * z;
314
315 btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19;
316 btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19;
317 btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19;
318 btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19;
319 btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19;
320 btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19;
321
322 dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz;
323 dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19;
324 dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19;
325 dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz;
326 dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19;
327 dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19;
328 dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz;
329 dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19;
330 dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19;
331 dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz;
332 dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19;
333 dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19;
334 dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz;
335 dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19;
336 dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19;
337 dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz;
338 dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19;
339 dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19;
340 dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz;
341 dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19;
342 dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19;
343 dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz;
344 dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19;
345 dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19;
346
347 dN.topRowsDivide(8, 64.0);
348
349 btScalar _m3m9x2m2x = -_3m9x2 - _2x;
350 btScalar _p3m9x2m2x = _3m9x2 - _2x;
351 btScalar _1mx2t1m3x = _1mx2 * _1m3x;
352 btScalar _1mx2t1p3x = _1mx2 * _1p3x;
353 dN(8, 0) = _m3m9x2m2x * _1myt1mz,
354 dN(8, 1) = -_1mx2t1m3x * _1mz,
355 dN(8, 2) = -_1mx2t1m3x * _1my;
356 dN(9, 0) = _p3m9x2m2x * _1myt1mz,
357 dN(9, 1) = -_1mx2t1p3x * _1mz,
358 dN(9, 2) = -_1mx2t1p3x * _1my;
359 dN(10, 0) = _m3m9x2m2x * _1myt1pz,
360 dN(10, 1) = -_1mx2t1m3x * _1pz,
361 dN(10, 2) = _1mx2t1m3x * _1my;
362 dN(11, 0) = _p3m9x2m2x * _1myt1pz,
363 dN(11, 1) = -_1mx2t1p3x * _1pz,
364 dN(11, 2) = _1mx2t1p3x * _1my;
365 dN(12, 0) = _m3m9x2m2x * _1pyt1mz,
366 dN(12, 1) = _1mx2t1m3x * _1mz,
367 dN(12, 2) = -_1mx2t1m3x * _1py;
368 dN(13, 0) = _p3m9x2m2x * _1pyt1mz,
369 dN(13, 1) = _1mx2t1p3x * _1mz,
370 dN(13, 2) = -_1mx2t1p3x * _1py;
371 dN(14, 0) = _m3m9x2m2x * _1pyt1pz,
372 dN(14, 1) = _1mx2t1m3x * _1pz,
373 dN(14, 2) = _1mx2t1m3x * _1py;
374 dN(15, 0) = _p3m9x2m2x * _1pyt1pz,
375 dN(15, 1) = _1mx2t1p3x * _1pz,
376 dN(15, 2) = _1mx2t1p3x * _1py;
377
378 btScalar _m3m9y2m2y = -_3m9y2 - _2y;
379 btScalar _p3m9y2m2y = _3m9y2 - _2y;
380 btScalar _1my2t1m3y = _1my2 * _1m3y;
381 btScalar _1my2t1p3y = _1my2 * _1p3y;
382 dN(16, 0) = -_1my2t1m3y * _1mz,
383 dN(16, 1) = _m3m9y2m2y * _1mxt1mz,
384 dN(16, 2) = -_1my2t1m3y * _1mx;
385 dN(17, 0) = -_1my2t1p3y * _1mz,
386 dN(17, 1) = _p3m9y2m2y * _1mxt1mz,
387 dN(17, 2) = -_1my2t1p3y * _1mx;
388 dN(18, 0) = _1my2t1m3y * _1mz,
389 dN(18, 1) = _m3m9y2m2y * _1pxt1mz,
390 dN(18, 2) = -_1my2t1m3y * _1px;
391 dN(19, 0) = _1my2t1p3y * _1mz,
392 dN(19, 1) = _p3m9y2m2y * _1pxt1mz,
393 dN(19, 2) = -_1my2t1p3y * _1px;
394 dN(20, 0) = -_1my2t1m3y * _1pz,
395 dN(20, 1) = _m3m9y2m2y * _1mxt1pz,
396 dN(20, 2) = _1my2t1m3y * _1mx;
397 dN(21, 0) = -_1my2t1p3y * _1pz,
398 dN(21, 1) = _p3m9y2m2y * _1mxt1pz,
399 dN(21, 2) = _1my2t1p3y * _1mx;
400 dN(22, 0) = _1my2t1m3y * _1pz,
401 dN(22, 1) = _m3m9y2m2y * _1pxt1pz,
402 dN(22, 2) = _1my2t1m3y * _1px;
403 dN(23, 0) = _1my2t1p3y * _1pz,
404 dN(23, 1) = _p3m9y2m2y * _1pxt1pz,
405 dN(23, 2) = _1my2t1p3y * _1px;
406
407 btScalar _m3m9z2m2z = -_3m9z2 - _2z;
408 btScalar _p3m9z2m2z = _3m9z2 - _2z;
409 btScalar _1mz2t1m3z = _1mz2 * _1m3z;
410 btScalar _1mz2t1p3z = _1mz2 * _1p3z;
411 dN(24, 0) = -_1mz2t1m3z * _1my,
412 dN(24, 1) = -_1mz2t1m3z * _1mx,
413 dN(24, 2) = _m3m9z2m2z * _1mxt1my;
414 dN(25, 0) = -_1mz2t1p3z * _1my,
415 dN(25, 1) = -_1mz2t1p3z * _1mx,
416 dN(25, 2) = _p3m9z2m2z * _1mxt1my;
417 dN(26, 0) = -_1mz2t1m3z * _1py,
418 dN(26, 1) = _1mz2t1m3z * _1mx,
419 dN(26, 2) = _m3m9z2m2z * _1mxt1py;
420 dN(27, 0) = -_1mz2t1p3z * _1py,
421 dN(27, 1) = _1mz2t1p3z * _1mx,
422 dN(27, 2) = _p3m9z2m2z * _1mxt1py;
423 dN(28, 0) = _1mz2t1m3z * _1my,
424 dN(28, 1) = -_1mz2t1m3z * _1px,
425 dN(28, 2) = _m3m9z2m2z * _1pxt1my;
426 dN(29, 0) = _1mz2t1p3z * _1my,
427 dN(29, 1) = -_1mz2t1p3z * _1px,
428 dN(29, 2) = _p3m9z2m2z * _1pxt1my;
429 dN(30, 0) = _1mz2t1m3z * _1py,
430 dN(30, 1) = _1mz2t1m3z * _1px,
431 dN(30, 2) = _m3m9z2m2z * _1pxt1py;
432 dN(31, 0) = _1mz2t1p3z * _1py,
433 dN(31, 1) = _1mz2t1p3z * _1px,
434 dN(31, 2) = _p3m9z2m2z * _1pxt1py;
435
436 dN.bottomRowsMul(32u - 8u, 9.0 / 64.0);
437 }
438
439 return res;
440 }
441
interpolate(unsigned int field_id,double & dist,btVector3 const & x,btVector3 * gradient) const442 bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x,
443 btVector3* gradient) const
444 {
445 btAssert(m_isValid);
446 if (!m_isValid)
447 return false;
448
449 if (!m_domain.contains(x))
450 return false;
451
452 btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size)); //.cast<unsigned int>().eval();
453 unsigned int mi[3] = {(unsigned int)tmpmi[0], (unsigned int)tmpmi[1], (unsigned int)tmpmi[2]};
454 if (mi[0] >= m_resolution[0])
455 mi[0] = m_resolution[0] - 1;
456 if (mi[1] >= m_resolution[1])
457 mi[1] = m_resolution[1] - 1;
458 if (mi[2] >= m_resolution[2])
459 mi[2] = m_resolution[2] - 1;
460 btMultiIndex mui;
461 mui.ijk[0] = mi[0];
462 mui.ijk[1] = mi[1];
463 mui.ijk[2] = mi[2];
464 int i = multiToSingleIndex(mui);
465 unsigned int i_ = m_cell_map[field_id][i];
466 if (i_ == UINT_MAX)
467 return false;
468
469 btAlignedBox3d sd = subdomain(i);
470 i = i_;
471 btVector3 d = sd.m_max - sd.m_min; //.diagonal().eval();
472
473 btVector3 denom = (sd.max() - sd.min());
474 btVector3 c0 = btVector3(2.0, 2.0, 2.0) / denom;
475 btVector3 c1 = (sd.max() + sd.min()) / denom;
476 btVector3 xi = (c0 * x - c1);
477
478 btCell32 const& cell = m_cells[field_id][i];
479 if (!gradient)
480 {
481 //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0));
482 double phi = 0.0;
483 btShapeMatrix N = shape_function_(xi, 0);
484 for (unsigned int j = 0u; j < 32u; ++j)
485 {
486 unsigned int v = cell.m_cells[j];
487 double c = m_nodes[field_id][v];
488 if (c == DBL_MAX)
489 {
490 return false;
491 ;
492 }
493 phi += c * N[j];
494 }
495
496 dist = phi;
497 return true;
498 }
499
500 btShapeGradients dN;
501 btShapeMatrix N = shape_function_(xi, &dN);
502
503 double phi = 0.0;
504 gradient->setZero();
505 for (unsigned int j = 0u; j < 32u; ++j)
506 {
507 unsigned int v = cell.m_cells[j];
508 double c = m_nodes[field_id][v];
509 if (c == DBL_MAX)
510 {
511 gradient->setZero();
512 return false;
513 }
514 phi += c * N[j];
515 (*gradient)[0] += c * dN(j, 0);
516 (*gradient)[1] += c * dN(j, 1);
517 (*gradient)[2] += c * dN(j, 2);
518 }
519 (*gradient) *= c0;
520 dist = phi;
521 return true;
522 }
523