1/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
2 All rights reserved.
3
4
5 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
6
7 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
8
9 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
10
11 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission.
12
13 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
14 */
15
16#ifndef _CRT_SECURE_NO_WARNINGS
17#define _CRT_SECURE_NO_WARNINGS
18#endif
19
20#include "btConvexHullComputer.h"
21#include "vhacdVolume.h"
22#include <algorithm>
23#include <float.h>
24#include <math.h>
25#include <queue>
26#include <string.h>
27
28#ifdef _MSC_VER
29#pragma warning(disable:4458 4100)
30#endif
31
32
33namespace VHACD {
34/********************************************************/
35/* AABB-triangle overlap test code */
36/* by Tomas Akenine-M�ller */
37/* Function: int32_t triBoxOverlap(float boxcenter[3], */
38/* float boxhalfsize[3],float triverts[3][3]); */
39/* History: */
40/* 2001-03-05: released the code in its first version */
41/* 2001-06-18: changed the order of the tests, faster */
42/* */
43/* Acknowledgement: Many thanks to Pierre Terdiman for */
44/* suggestions and discussions on how to optimize code. */
45/* Thanks to David Hunt for finding a ">="-bug! */
46/********************************************************/
47
48#define X 0
49#define Y 1
50#define Z 2
51#define FINDMINMAX(x0, x1, x2, min, max) \
52 min = max = x0; \
53 if (x1 < min) \
54 min = x1; \
55 if (x1 > max) \
56 max = x1; \
57 if (x2 < min) \
58 min = x2; \
59 if (x2 > max) \
60 max = x2;
61
62#define AXISTEST_X01(a, b, fa, fb) \
63 p0 = a * v0[Y] - b * v0[Z]; \
64 p2 = a * v2[Y] - b * v2[Z]; \
65 if (p0 < p2) { \
66 min = p0; \
67 max = p2; \
68 } \
69 else { \
70 min = p2; \
71 max = p0; \
72 } \
73 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
74 if (min > rad || max < -rad) \
75 return 0;
76
77#define AXISTEST_X2(a, b, fa, fb) \
78 p0 = a * v0[Y] - b * v0[Z]; \
79 p1 = a * v1[Y] - b * v1[Z]; \
80 if (p0 < p1) { \
81 min = p0; \
82 max = p1; \
83 } \
84 else { \
85 min = p1; \
86 max = p0; \
87 } \
88 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
89 if (min > rad || max < -rad) \
90 return 0;
91
92#define AXISTEST_Y02(a, b, fa, fb) \
93 p0 = -a * v0[X] + b * v0[Z]; \
94 p2 = -a * v2[X] + b * v2[Z]; \
95 if (p0 < p2) { \
96 min = p0; \
97 max = p2; \
98 } \
99 else { \
100 min = p2; \
101 max = p0; \
102 } \
103 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
104 if (min > rad || max < -rad) \
105 return 0;
106
107#define AXISTEST_Y1(a, b, fa, fb) \
108 p0 = -a * v0[X] + b * v0[Z]; \
109 p1 = -a * v1[X] + b * v1[Z]; \
110 if (p0 < p1) { \
111 min = p0; \
112 max = p1; \
113 } \
114 else { \
115 min = p1; \
116 max = p0; \
117 } \
118 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
119 if (min > rad || max < -rad) \
120 return 0;
121
122#define AXISTEST_Z12(a, b, fa, fb) \
123 p1 = a * v1[X] - b * v1[Y]; \
124 p2 = a * v2[X] - b * v2[Y]; \
125 if (p2 < p1) { \
126 min = p2; \
127 max = p1; \
128 } \
129 else { \
130 min = p1; \
131 max = p2; \
132 } \
133 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
134 if (min > rad || max < -rad) \
135 return 0;
136
137#define AXISTEST_Z0(a, b, fa, fb) \
138 p0 = a * v0[X] - b * v0[Y]; \
139 p1 = a * v1[X] - b * v1[Y]; \
140 if (p0 < p1) { \
141 min = p0; \
142 max = p1; \
143 } \
144 else { \
145 min = p1; \
146 max = p0; \
147 } \
148 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
149 if (min > rad || max < -rad) \
150 return 0;
151
152int32_t PlaneBoxOverlap(const Vec3<double>& normal,
153 const Vec3<double>& vert,
154 const Vec3<double>& maxbox)
155{
156 int32_t q;
157 Vec3<double> vmin, vmax;
158 double v;
159 for (q = X; q <= Z; q++) {
160 v = vert[q];
161 if (normal[q] > 0.0) {
162 vmin[q] = -maxbox[q] - v;
163 vmax[q] = maxbox[q] - v;
164 }
165 else {
166 vmin[q] = maxbox[q] - v;
167 vmax[q] = -maxbox[q] - v;
168 }
169 }
170 if (normal * vmin > 0.0)
171 return 0;
172 if (normal * vmax >= 0.0)
173 return 1;
174 return 0;
175}
176
177int32_t TriBoxOverlap(const Vec3<double>& boxcenter,
178 const Vec3<double>& boxhalfsize,
179 const Vec3<double>& triver0,
180 const Vec3<double>& triver1,
181 const Vec3<double>& triver2)
182{
183 /* use separating axis theorem to test overlap between triangle and box */
184 /* need to test for overlap in these directions: */
185 /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
186 /* we do not even need to test these) */
187 /* 2) normal of the triangle */
188 /* 3) crossproduct(edge from tri, {x,y,z}-directin) */
189 /* this gives 3x3=9 more tests */
190
191 Vec3<double> v0, v1, v2;
192 double min, max, p0, p1, p2, rad, fex, fey, fez; // -NJMP- "d" local variable removed
193 Vec3<double> normal, e0, e1, e2;
194
195 /* This is the fastest branch on Sun */
196 /* move everything so that the boxcenter is in (0,0,0) */
197
198 v0 = triver0 - boxcenter;
199 v1 = triver1 - boxcenter;
200 v2 = triver2 - boxcenter;
201
202 /* compute triangle edges */
203 e0 = v1 - v0; /* tri edge 0 */
204 e1 = v2 - v1; /* tri edge 1 */
205 e2 = v0 - v2; /* tri edge 2 */
206
207 /* Bullet 3: */
208 /* test the 9 tests first (this was faster) */
209 fex = fabs(e0[X]);
210 fey = fabs(e0[Y]);
211 fez = fabs(e0[Z]);
212
213 AXISTEST_X01(e0[Z], e0[Y], fez, fey);
214 AXISTEST_Y02(e0[Z], e0[X], fez, fex);
215 AXISTEST_Z12(e0[Y], e0[X], fey, fex);
216
217 fex = fabs(e1[X]);
218 fey = fabs(e1[Y]);
219 fez = fabs(e1[Z]);
220
221 AXISTEST_X01(e1[Z], e1[Y], fez, fey);
222 AXISTEST_Y02(e1[Z], e1[X], fez, fex);
223 AXISTEST_Z0(e1[Y], e1[X], fey, fex);
224
225 fex = fabs(e2[X]);
226 fey = fabs(e2[Y]);
227 fez = fabs(e2[Z]);
228
229 AXISTEST_X2(e2[Z], e2[Y], fez, fey);
230 AXISTEST_Y1(e2[Z], e2[X], fez, fex);
231 AXISTEST_Z12(e2[Y], e2[X], fey, fex);
232
233 /* Bullet 1: */
234 /* first test overlap in the {x,y,z}-directions */
235 /* find min, max of the triangle each direction, and test for overlap in */
236 /* that direction -- this is equivalent to testing a minimal AABB around */
237 /* the triangle against the AABB */
238
239 /* test in X-direction */
240 FINDMINMAX(v0[X], v1[X], v2[X], min, max);
241 if (min > boxhalfsize[X] || max < -boxhalfsize[X])
242 return 0;
243
244 /* test in Y-direction */
245 FINDMINMAX(v0[Y], v1[Y], v2[Y], min, max);
246 if (min > boxhalfsize[Y] || max < -boxhalfsize[Y])
247 return 0;
248
249 /* test in Z-direction */
250 FINDMINMAX(v0[Z], v1[Z], v2[Z], min, max);
251 if (min > boxhalfsize[Z] || max < -boxhalfsize[Z])
252 return 0;
253
254 /* Bullet 2: */
255 /* test if the box intersects the plane of the triangle */
256 /* compute plane equation of triangle: normal*x+d=0 */
257 normal = e0 ^ e1;
258
259 if (!PlaneBoxOverlap(normal, v0, boxhalfsize))
260 return 0;
261 return 1; /* box and triangle overlaps */
262}
263
264// Slightly modified version of Stan Melax's code for 3x3 matrix diagonalization (Thanks Stan!)
265// source: http://www.melax.com/diag.html?attredirects=0
266void Diagonalize(const double (&A)[3][3], double (&Q)[3][3], double (&D)[3][3])
267{
268 // A must be a symmetric matrix.
269 // returns Q and D such that
270 // Diagonal matrix D = QT * A * Q; and A = Q*D*QT
271 const int32_t maxsteps = 24; // certainly wont need that many.
272 int32_t k0, k1, k2;
273 double o[3], m[3];
274 double q[4] = { 0.0, 0.0, 0.0, 1.0 };
275 double jr[4];
276 double sqw, sqx, sqy, sqz;
277 double tmp1, tmp2, mq;
278 double AQ[3][3];
279 double thet, sgn, t, c;
280 for (int32_t i = 0; i < maxsteps; ++i) {
281 // quat to matrix
282 sqx = q[0] * q[0];
283 sqy = q[1] * q[1];
284 sqz = q[2] * q[2];
285 sqw = q[3] * q[3];
286 Q[0][0] = (sqx - sqy - sqz + sqw);
287 Q[1][1] = (-sqx + sqy - sqz + sqw);
288 Q[2][2] = (-sqx - sqy + sqz + sqw);
289 tmp1 = q[0] * q[1];
290 tmp2 = q[2] * q[3];
291 Q[1][0] = 2.0 * (tmp1 + tmp2);
292 Q[0][1] = 2.0 * (tmp1 - tmp2);
293 tmp1 = q[0] * q[2];
294 tmp2 = q[1] * q[3];
295 Q[2][0] = 2.0 * (tmp1 - tmp2);
296 Q[0][2] = 2.0 * (tmp1 + tmp2);
297 tmp1 = q[1] * q[2];
298 tmp2 = q[0] * q[3];
299 Q[2][1] = 2.0 * (tmp1 + tmp2);
300 Q[1][2] = 2.0 * (tmp1 - tmp2);
301
302 // AQ = A * Q
303 AQ[0][0] = Q[0][0] * A[0][0] + Q[1][0] * A[0][1] + Q[2][0] * A[0][2];
304 AQ[0][1] = Q[0][1] * A[0][0] + Q[1][1] * A[0][1] + Q[2][1] * A[0][2];
305 AQ[0][2] = Q[0][2] * A[0][0] + Q[1][2] * A[0][1] + Q[2][2] * A[0][2];
306 AQ[1][0] = Q[0][0] * A[0][1] + Q[1][0] * A[1][1] + Q[2][0] * A[1][2];
307 AQ[1][1] = Q[0][1] * A[0][1] + Q[1][1] * A[1][1] + Q[2][1] * A[1][2];
308 AQ[1][2] = Q[0][2] * A[0][1] + Q[1][2] * A[1][1] + Q[2][2] * A[1][2];
309 AQ[2][0] = Q[0][0] * A[0][2] + Q[1][0] * A[1][2] + Q[2][0] * A[2][2];
310 AQ[2][1] = Q[0][1] * A[0][2] + Q[1][1] * A[1][2] + Q[2][1] * A[2][2];
311 AQ[2][2] = Q[0][2] * A[0][2] + Q[1][2] * A[1][2] + Q[2][2] * A[2][2];
312 // D = Qt * AQ
313 D[0][0] = AQ[0][0] * Q[0][0] + AQ[1][0] * Q[1][0] + AQ[2][0] * Q[2][0];
314 D[0][1] = AQ[0][0] * Q[0][1] + AQ[1][0] * Q[1][1] + AQ[2][0] * Q[2][1];
315 D[0][2] = AQ[0][0] * Q[0][2] + AQ[1][0] * Q[1][2] + AQ[2][0] * Q[2][2];
316 D[1][0] = AQ[0][1] * Q[0][0] + AQ[1][1] * Q[1][0] + AQ[2][1] * Q[2][0];
317 D[1][1] = AQ[0][1] * Q[0][1] + AQ[1][1] * Q[1][1] + AQ[2][1] * Q[2][1];
318 D[1][2] = AQ[0][1] * Q[0][2] + AQ[1][1] * Q[1][2] + AQ[2][1] * Q[2][2];
319 D[2][0] = AQ[0][2] * Q[0][0] + AQ[1][2] * Q[1][0] + AQ[2][2] * Q[2][0];
320 D[2][1] = AQ[0][2] * Q[0][1] + AQ[1][2] * Q[1][1] + AQ[2][2] * Q[2][1];
321 D[2][2] = AQ[0][2] * Q[0][2] + AQ[1][2] * Q[1][2] + AQ[2][2] * Q[2][2];
322 o[0] = D[1][2];
323 o[1] = D[0][2];
324 o[2] = D[0][1];
325 m[0] = fabs(o[0]);
326 m[1] = fabs(o[1]);
327 m[2] = fabs(o[2]);
328
329 k0 = (m[0] > m[1] && m[0] > m[2]) ? 0 : (m[1] > m[2]) ? 1 : 2; // index of largest element of offdiag
330 k1 = (k0 + 1) % 3;
331 k2 = (k0 + 2) % 3;
332 if (o[k0] == 0.0) {
333 break; // diagonal already
334 }
335 thet = (D[k2][k2] - D[k1][k1]) / (2.0 * o[k0]);
336 sgn = (thet > 0.0) ? 1.0 : -1.0;
337 thet *= sgn; // make it positive
338 t = sgn / (thet + ((thet < 1.E6) ? sqrt(thet * thet + 1.0) : thet)); // sign(T)/(|T|+sqrt(T^2+1))
339 c = 1.0 / sqrt(t * t + 1.0); // c= 1/(t^2+1) , t=s/c
340 if (c == 1.0) {
341 break; // no room for improvement - reached machine precision.
342 }
343 jr[0] = jr[1] = jr[2] = jr[3] = 0.0;
344 jr[k0] = sgn * sqrt((1.0 - c) / 2.0); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
345 jr[k0] *= -1.0; // since our quat-to-matrix convention was for v*M instead of M*v
346 jr[3] = sqrt(1.0 - jr[k0] * jr[k0]);
347 if (jr[3] == 1.0) {
348 break; // reached limits of floating point precision
349 }
350 q[0] = (q[3] * jr[0] + q[0] * jr[3] + q[1] * jr[2] - q[2] * jr[1]);
351 q[1] = (q[3] * jr[1] - q[0] * jr[2] + q[1] * jr[3] + q[2] * jr[0]);
352 q[2] = (q[3] * jr[2] + q[0] * jr[1] - q[1] * jr[0] + q[2] * jr[3]);
353 q[3] = (q[3] * jr[3] - q[0] * jr[0] - q[1] * jr[1] - q[2] * jr[2]);
354 mq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
355 q[0] /= mq;
356 q[1] /= mq;
357 q[2] /= mq;
358 q[3] /= mq;
359 }
360}
361const double TetrahedronSet::EPS = 0.0000000000001;
362VoxelSet::VoxelSet()
363{
364 m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
365 m_minBBVoxels[0] = m_minBBVoxels[1] = m_minBBVoxels[2] = 0;
366 m_maxBBVoxels[0] = m_maxBBVoxels[1] = m_maxBBVoxels[2] = 1;
367 m_minBBPts[0] = m_minBBPts[1] = m_minBBPts[2] = 0;
368 m_maxBBPts[0] = m_maxBBPts[1] = m_maxBBPts[2] = 1;
369 m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0;
370 m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0;
371 m_scale = 1.0;
372 m_unitVolume = 1.0;
373 m_numVoxelsOnSurface = 0;
374 m_numVoxelsInsideSurface = 0;
375 memset(m_Q, 0, sizeof(double) * 9);
376 memset(m_D, 0, sizeof(double) * 9);
377}
378VoxelSet::~VoxelSet(void)
379{
380}
381void VoxelSet::ComputeBB()
382{
383 const size_t nVoxels = m_voxels.Size();
384 if (nVoxels == 0)
385 return;
386 for (int32_t h = 0; h < 3; ++h) {
387 m_minBBVoxels[h] = m_voxels[0].m_coord[h];
388 m_maxBBVoxels[h] = m_voxels[0].m_coord[h];
389 }
390 Vec3<double> bary(0.0);
391 for (size_t p = 0; p < nVoxels; ++p) {
392 for (int32_t h = 0; h < 3; ++h) {
393 bary[h] += m_voxels[p].m_coord[h];
394 if (m_minBBVoxels[h] > m_voxels[p].m_coord[h])
395 m_minBBVoxels[h] = m_voxels[p].m_coord[h];
396 if (m_maxBBVoxels[h] < m_voxels[p].m_coord[h])
397 m_maxBBVoxels[h] = m_voxels[p].m_coord[h];
398 }
399 }
400 bary /= (double)nVoxels;
401 for (int32_t h = 0; h < 3; ++h) {
402 m_minBBPts[h] = m_minBBVoxels[h] * m_scale + m_minBB[h];
403 m_maxBBPts[h] = m_maxBBVoxels[h] * m_scale + m_minBB[h];
404 m_barycenter[h] = (short)(bary[h] + 0.5);
405 }
406}
407void VoxelSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const
408{
409 const size_t CLUSTER_SIZE = 65536;
410 const size_t nVoxels = m_voxels.Size();
411 if (nVoxels == 0)
412 return;
413
414 SArray<Vec3<double> > cpoints;
415
416 Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE];
417 size_t p = 0;
418 size_t s = 0;
419 short i, j, k;
420 while (p < nVoxels) {
421 size_t q = 0;
422 while (q < CLUSTER_SIZE && p < nVoxels) {
423 if (m_voxels[p].m_data == PRIMITIVE_ON_SURFACE) {
424 ++s;
425 if (s == sampling) {
426 s = 0;
427 i = m_voxels[p].m_coord[0];
428 j = m_voxels[p].m_coord[1];
429 k = m_voxels[p].m_coord[2];
430 Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
431 Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
432 Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
433 Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
434 Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
435 Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
436 Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
437 Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
438 points[q++] = p0 + m_minBB;
439 points[q++] = p1 + m_minBB;
440 points[q++] = p2 + m_minBB;
441 points[q++] = p3 + m_minBB;
442 points[q++] = p4 + m_minBB;
443 points[q++] = p5 + m_minBB;
444 points[q++] = p6 + m_minBB;
445 points[q++] = p7 + m_minBB;
446 }
447 }
448 ++p;
449 }
450 btConvexHullComputer ch;
451 ch.compute((double*)points, 3 * sizeof(double), (int32_t)q, -1.0, -1.0);
452 for (int32_t v = 0; v < ch.vertices.size(); v++) {
453 cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
454 }
455 }
456 delete[] points;
457
458 points = cpoints.Data();
459 btConvexHullComputer ch;
460 ch.compute((double*)points, 3 * sizeof(double), (int32_t)cpoints.Size(), -1.0, -1.0);
461 meshCH.ResizePoints(0);
462 meshCH.ResizeTriangles(0);
463 for (int32_t v = 0; v < ch.vertices.size(); v++) {
464 meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
465 }
466 const int32_t nt = ch.faces.size();
467 for (int32_t t = 0; t < nt; ++t) {
468 const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
469 int32_t a = sourceEdge->getSourceVertex();
470 int32_t b = sourceEdge->getTargetVertex();
471 const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
472 int32_t c = edge->getTargetVertex();
473 while (c != a) {
474 meshCH.AddTriangle(Vec3<int32_t>(a, b, c));
475 edge = edge->getNextEdgeOfFace();
476 b = c;
477 c = edge->getTargetVertex();
478 }
479 }
480}
481void VoxelSet::GetPoints(const Voxel& voxel,
482 Vec3<double>* const pts) const
483{
484 short i = voxel.m_coord[0];
485 short j = voxel.m_coord[1];
486 short k = voxel.m_coord[2];
487 pts[0][0] = (i - 0.5) * m_scale + m_minBB[0];
488 pts[1][0] = (i + 0.5) * m_scale + m_minBB[0];
489 pts[2][0] = (i + 0.5) * m_scale + m_minBB[0];
490 pts[3][0] = (i - 0.5) * m_scale + m_minBB[0];
491 pts[4][0] = (i - 0.5) * m_scale + m_minBB[0];
492 pts[5][0] = (i + 0.5) * m_scale + m_minBB[0];
493 pts[6][0] = (i + 0.5) * m_scale + m_minBB[0];
494 pts[7][0] = (i - 0.5) * m_scale + m_minBB[0];
495 pts[0][1] = (j - 0.5) * m_scale + m_minBB[1];
496 pts[1][1] = (j - 0.5) * m_scale + m_minBB[1];
497 pts[2][1] = (j + 0.5) * m_scale + m_minBB[1];
498 pts[3][1] = (j + 0.5) * m_scale + m_minBB[1];
499 pts[4][1] = (j - 0.5) * m_scale + m_minBB[1];
500 pts[5][1] = (j - 0.5) * m_scale + m_minBB[1];
501 pts[6][1] = (j + 0.5) * m_scale + m_minBB[1];
502 pts[7][1] = (j + 0.5) * m_scale + m_minBB[1];
503 pts[0][2] = (k - 0.5) * m_scale + m_minBB[2];
504 pts[1][2] = (k - 0.5) * m_scale + m_minBB[2];
505 pts[2][2] = (k - 0.5) * m_scale + m_minBB[2];
506 pts[3][2] = (k - 0.5) * m_scale + m_minBB[2];
507 pts[4][2] = (k + 0.5) * m_scale + m_minBB[2];
508 pts[5][2] = (k + 0.5) * m_scale + m_minBB[2];
509 pts[6][2] = (k + 0.5) * m_scale + m_minBB[2];
510 pts[7][2] = (k + 0.5) * m_scale + m_minBB[2];
511}
512void VoxelSet::Intersect(const Plane& plane,
513 SArray<Vec3<double> >* const positivePts,
514 SArray<Vec3<double> >* const negativePts,
515 const size_t sampling) const
516{
517 const size_t nVoxels = m_voxels.Size();
518 if (nVoxels == 0)
519 return;
520 const double d0 = m_scale;
521 double d;
522 Vec3<double> pts[8];
523 Vec3<double> pt;
524 Voxel voxel;
525 size_t sp = 0;
526 size_t sn = 0;
527 for (size_t v = 0; v < nVoxels; ++v) {
528 voxel = m_voxels[v];
529 pt = GetPoint(voxel);
530 d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
531 // if (d >= 0.0 && d <= d0) positivePts->PushBack(pt);
532 // else if (d < 0.0 && -d <= d0) negativePts->PushBack(pt);
533 if (d >= 0.0) {
534 if (d <= d0) {
535 GetPoints(voxel, pts);
536 for (int32_t k = 0; k < 8; ++k) {
537 positivePts->PushBack(pts[k]);
538 }
539 }
540 else {
541 if (++sp == sampling) {
542 // positivePts->PushBack(pt);
543 GetPoints(voxel, pts);
544 for (int32_t k = 0; k < 8; ++k) {
545 positivePts->PushBack(pts[k]);
546 }
547 sp = 0;
548 }
549 }
550 }
551 else {
552 if (-d <= d0) {
553 GetPoints(voxel, pts);
554 for (int32_t k = 0; k < 8; ++k) {
555 negativePts->PushBack(pts[k]);
556 }
557 }
558 else {
559 if (++sn == sampling) {
560 // negativePts->PushBack(pt);
561 GetPoints(voxel, pts);
562 for (int32_t k = 0; k < 8; ++k) {
563 negativePts->PushBack(pts[k]);
564 }
565 sn = 0;
566 }
567 }
568 }
569 }
570}
571void VoxelSet::ComputeExteriorPoints(const Plane& plane,
572 const Mesh& mesh,
573 SArray<Vec3<double> >* const exteriorPts) const
574{
575 const size_t nVoxels = m_voxels.Size();
576 if (nVoxels == 0)
577 return;
578 double d;
579 Vec3<double> pt;
580 Vec3<double> pts[8];
581 Voxel voxel;
582 for (size_t v = 0; v < nVoxels; ++v) {
583 voxel = m_voxels[v];
584 pt = GetPoint(voxel);
585 d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
586 if (d >= 0.0) {
587 if (!mesh.IsInside(pt)) {
588 GetPoints(voxel, pts);
589 for (int32_t k = 0; k < 8; ++k) {
590 exteriorPts->PushBack(pts[k]);
591 }
592 }
593 }
594 }
595}
596void VoxelSet::ComputeClippedVolumes(const Plane& plane,
597 double& positiveVolume,
598 double& negativeVolume) const
599{
600 negativeVolume = 0.0;
601 positiveVolume = 0.0;
602 const size_t nVoxels = m_voxels.Size();
603 if (nVoxels == 0)
604 return;
605 double d;
606 Vec3<double> pt;
607 size_t nPositiveVoxels = 0;
608 for (size_t v = 0; v < nVoxels; ++v) {
609 pt = GetPoint(m_voxels[v]);
610 d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
611 nPositiveVoxels += (d >= 0.0);
612 }
613 size_t nNegativeVoxels = nVoxels - nPositiveVoxels;
614 positiveVolume = m_unitVolume * nPositiveVoxels;
615 negativeVolume = m_unitVolume * nNegativeVoxels;
616}
617void VoxelSet::SelectOnSurface(PrimitiveSet* const onSurfP) const
618{
619 VoxelSet* const onSurf = (VoxelSet*)onSurfP;
620 const size_t nVoxels = m_voxels.Size();
621 if (nVoxels == 0)
622 return;
623
624 for (int32_t h = 0; h < 3; ++h) {
625 onSurf->m_minBB[h] = m_minBB[h];
626 }
627 onSurf->m_voxels.Resize(0);
628 onSurf->m_scale = m_scale;
629 onSurf->m_unitVolume = m_unitVolume;
630 onSurf->m_numVoxelsOnSurface = 0;
631 onSurf->m_numVoxelsInsideSurface = 0;
632 Voxel voxel;
633 for (size_t v = 0; v < nVoxels; ++v) {
634 voxel = m_voxels[v];
635 if (voxel.m_data == PRIMITIVE_ON_SURFACE) {
636 onSurf->m_voxels.PushBack(voxel);
637 ++onSurf->m_numVoxelsOnSurface;
638 }
639 }
640}
641void VoxelSet::Clip(const Plane& plane,
642 PrimitiveSet* const positivePartP,
643 PrimitiveSet* const negativePartP) const
644{
645 VoxelSet* const positivePart = (VoxelSet*)positivePartP;
646 VoxelSet* const negativePart = (VoxelSet*)negativePartP;
647 const size_t nVoxels = m_voxels.Size();
648 if (nVoxels == 0)
649 return;
650
651 for (int32_t h = 0; h < 3; ++h) {
652 negativePart->m_minBB[h] = positivePart->m_minBB[h] = m_minBB[h];
653 }
654 positivePart->m_voxels.Resize(0);
655 negativePart->m_voxels.Resize(0);
656 positivePart->m_voxels.Allocate(nVoxels);
657 negativePart->m_voxels.Allocate(nVoxels);
658 negativePart->m_scale = positivePart->m_scale = m_scale;
659 negativePart->m_unitVolume = positivePart->m_unitVolume = m_unitVolume;
660 negativePart->m_numVoxelsOnSurface = positivePart->m_numVoxelsOnSurface = 0;
661 negativePart->m_numVoxelsInsideSurface = positivePart->m_numVoxelsInsideSurface = 0;
662
663 double d;
664 Vec3<double> pt;
665 Voxel voxel;
666 const double d0 = m_scale;
667 for (size_t v = 0; v < nVoxels; ++v) {
668 voxel = m_voxels[v];
669 pt = GetPoint(voxel);
670 d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
671 if (d >= 0.0) {
672 if (voxel.m_data == PRIMITIVE_ON_SURFACE || d <= d0) {
673 voxel.m_data = PRIMITIVE_ON_SURFACE;
674 positivePart->m_voxels.PushBack(voxel);
675 ++positivePart->m_numVoxelsOnSurface;
676 }
677 else {
678 positivePart->m_voxels.PushBack(voxel);
679 ++positivePart->m_numVoxelsInsideSurface;
680 }
681 }
682 else {
683 if (voxel.m_data == PRIMITIVE_ON_SURFACE || -d <= d0) {
684 voxel.m_data = PRIMITIVE_ON_SURFACE;
685 negativePart->m_voxels.PushBack(voxel);
686 ++negativePart->m_numVoxelsOnSurface;
687 }
688 else {
689 negativePart->m_voxels.PushBack(voxel);
690 ++negativePart->m_numVoxelsInsideSurface;
691 }
692 }
693 }
694}
695void VoxelSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const
696{
697 const size_t nVoxels = m_voxels.Size();
698 if (nVoxels == 0)
699 return;
700 Voxel voxel;
701 Vec3<double> pts[8];
702 for (size_t v = 0; v < nVoxels; ++v) {
703 voxel = m_voxels[v];
704 if (voxel.m_data == value) {
705 GetPoints(voxel, pts);
706 int32_t s = (int32_t)mesh.GetNPoints();
707 for (int32_t k = 0; k < 8; ++k) {
708 mesh.AddPoint(pts[k]);
709 }
710 mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 2, s + 1));
711 mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 3, s + 2));
712 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 5, s + 6));
713 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 6, s + 7));
714 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 6, s + 2));
715 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 2, s + 3));
716 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 1, s + 5));
717 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 0, s + 1));
718 mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 5, s + 1));
719 mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 1, s + 2));
720 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 0, s + 4));
721 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 3, s + 0));
722 }
723 }
724}
725void VoxelSet::ComputePrincipalAxes()
726{
727 const size_t nVoxels = m_voxels.Size();
728 if (nVoxels == 0)
729 return;
730 m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0;
731 for (size_t v = 0; v < nVoxels; ++v) {
732 Voxel& voxel = m_voxels[v];
733 m_barycenterPCA[0] += voxel.m_coord[0];
734 m_barycenterPCA[1] += voxel.m_coord[1];
735 m_barycenterPCA[2] += voxel.m_coord[2];
736 }
737 m_barycenterPCA /= (double)nVoxels;
738
739 double covMat[3][3] = { { 0.0, 0.0, 0.0 },
740 { 0.0, 0.0, 0.0 },
741 { 0.0, 0.0, 0.0 } };
742 double x, y, z;
743 for (size_t v = 0; v < nVoxels; ++v) {
744 Voxel& voxel = m_voxels[v];
745 x = voxel.m_coord[0] - m_barycenter[0];
746 y = voxel.m_coord[1] - m_barycenter[1];
747 z = voxel.m_coord[2] - m_barycenter[2];
748 covMat[0][0] += x * x;
749 covMat[1][1] += y * y;
750 covMat[2][2] += z * z;
751 covMat[0][1] += x * y;
752 covMat[0][2] += x * z;
753 covMat[1][2] += y * z;
754 }
755 covMat[0][0] /= nVoxels;
756 covMat[1][1] /= nVoxels;
757 covMat[2][2] /= nVoxels;
758 covMat[0][1] /= nVoxels;
759 covMat[0][2] /= nVoxels;
760 covMat[1][2] /= nVoxels;
761 covMat[1][0] = covMat[0][1];
762 covMat[2][0] = covMat[0][2];
763 covMat[2][1] = covMat[1][2];
764 Diagonalize(covMat, m_Q, m_D);
765}
766Volume::Volume()
767{
768 m_dim[0] = m_dim[1] = m_dim[2] = 0;
769 m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
770 m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0;
771 m_numVoxelsOnSurface = 0;
772 m_numVoxelsInsideSurface = 0;
773 m_numVoxelsOutsideSurface = 0;
774 m_scale = 1.0;
775 m_data = 0;
776}
777Volume::~Volume(void)
778{
779 delete[] m_data;
780}
781void Volume::Allocate()
782{
783 delete[] m_data;
784 size_t size = m_dim[0] * m_dim[1] * m_dim[2];
785 m_data = new unsigned char[size];
786 memset(m_data, PRIMITIVE_UNDEFINED, sizeof(unsigned char) * size);
787}
788void Volume::Free()
789{
790 delete[] m_data;
791 m_data = 0;
792}
793void Volume::FillOutsideSurface(const size_t i0,
794 const size_t j0,
795 const size_t k0,
796 const size_t i1,
797 const size_t j1,
798 const size_t k1)
799{
800 const short neighbours[6][3] = { { 1, 0, 0 },
801 { 0, 1, 0 },
802 { 0, 0, 1 },
803 { -1, 0, 0 },
804 { 0, -1, 0 },
805 { 0, 0, -1 } };
806 std::queue<Vec3<short> > fifo;
807 Vec3<short> current;
808 short a, b, c;
809 for (size_t i = i0; i < i1; ++i) {
810 for (size_t j = j0; j < j1; ++j) {
811 for (size_t k = k0; k < k1; ++k) {
812
813 if (GetVoxel(i, j, k) == PRIMITIVE_UNDEFINED) {
814 current[0] = (short)i;
815 current[1] = (short)j;
816 current[2] = (short)k;
817 fifo.push(current);
818 GetVoxel(current[0], current[1], current[2]) = PRIMITIVE_OUTSIDE_SURFACE;
819 ++m_numVoxelsOutsideSurface;
820 while (fifo.size() > 0) {
821 current = fifo.front();
822 fifo.pop();
823 for (int32_t h = 0; h < 6; ++h) {
824 a = current[0] + neighbours[h][0];
825 b = current[1] + neighbours[h][1];
826 c = current[2] + neighbours[h][2];
827 if (a < 0 || a >= (int32_t)m_dim[0] || b < 0 || b >= (int32_t)m_dim[1] || c < 0 || c >= (int32_t)m_dim[2]) {
828 continue;
829 }
830 unsigned char& v = GetVoxel(a, b, c);
831 if (v == PRIMITIVE_UNDEFINED) {
832 v = PRIMITIVE_OUTSIDE_SURFACE;
833 ++m_numVoxelsOutsideSurface;
834 fifo.push(Vec3<short>(a, b, c));
835 }
836 }
837 }
838 }
839 }
840 }
841 }
842}
843void Volume::FillInsideSurface()
844{
845 const size_t i0 = m_dim[0];
846 const size_t j0 = m_dim[1];
847 const size_t k0 = m_dim[2];
848 for (size_t i = 0; i < i0; ++i) {
849 for (size_t j = 0; j < j0; ++j) {
850 for (size_t k = 0; k < k0; ++k) {
851 unsigned char& v = GetVoxel(i, j, k);
852 if (v == PRIMITIVE_UNDEFINED) {
853 v = PRIMITIVE_INSIDE_SURFACE;
854 ++m_numVoxelsInsideSurface;
855 }
856 }
857 }
858 }
859}
860void Volume::Convert(Mesh& mesh, const VOXEL_VALUE value) const
861{
862 const size_t i0 = m_dim[0];
863 const size_t j0 = m_dim[1];
864 const size_t k0 = m_dim[2];
865 for (size_t i = 0; i < i0; ++i) {
866 for (size_t j = 0; j < j0; ++j) {
867 for (size_t k = 0; k < k0; ++k) {
868 const unsigned char& voxel = GetVoxel(i, j, k);
869 if (voxel == value) {
870 Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
871 Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
872 Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
873 Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
874 Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
875 Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
876 Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
877 Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
878 int32_t s = (int32_t)mesh.GetNPoints();
879 mesh.AddPoint(p0 + m_minBB);
880 mesh.AddPoint(p1 + m_minBB);
881 mesh.AddPoint(p2 + m_minBB);
882 mesh.AddPoint(p3 + m_minBB);
883 mesh.AddPoint(p4 + m_minBB);
884 mesh.AddPoint(p5 + m_minBB);
885 mesh.AddPoint(p6 + m_minBB);
886 mesh.AddPoint(p7 + m_minBB);
887 mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 2, s + 1));
888 mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 3, s + 2));
889 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 5, s + 6));
890 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 6, s + 7));
891 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 6, s + 2));
892 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 2, s + 3));
893 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 1, s + 5));
894 mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 0, s + 1));
895 mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 5, s + 1));
896 mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 1, s + 2));
897 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 0, s + 4));
898 mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 3, s + 0));
899 }
900 }
901 }
902 }
903}
904void Volume::Convert(VoxelSet& vset) const
905{
906 for (int32_t h = 0; h < 3; ++h) {
907 vset.m_minBB[h] = m_minBB[h];
908 }
909 vset.m_voxels.Allocate(m_numVoxelsInsideSurface + m_numVoxelsOnSurface);
910 vset.m_scale = m_scale;
911 vset.m_unitVolume = m_scale * m_scale * m_scale;
912 const short i0 = (short)m_dim[0];
913 const short j0 = (short)m_dim[1];
914 const short k0 = (short)m_dim[2];
915 Voxel voxel;
916 vset.m_numVoxelsOnSurface = 0;
917 vset.m_numVoxelsInsideSurface = 0;
918 for (short i = 0; i < i0; ++i) {
919 for (short j = 0; j < j0; ++j) {
920 for (short k = 0; k < k0; ++k) {
921 const unsigned char& value = GetVoxel(i, j, k);
922 if (value == PRIMITIVE_INSIDE_SURFACE) {
923 voxel.m_coord[0] = i;
924 voxel.m_coord[1] = j;
925 voxel.m_coord[2] = k;
926 voxel.m_data = PRIMITIVE_INSIDE_SURFACE;
927 vset.m_voxels.PushBack(voxel);
928 ++vset.m_numVoxelsInsideSurface;
929 }
930 else if (value == PRIMITIVE_ON_SURFACE) {
931 voxel.m_coord[0] = i;
932 voxel.m_coord[1] = j;
933 voxel.m_coord[2] = k;
934 voxel.m_data = PRIMITIVE_ON_SURFACE;
935 vset.m_voxels.PushBack(voxel);
936 ++vset.m_numVoxelsOnSurface;
937 }
938 }
939 }
940 }
941}
942
943void Volume::Convert(TetrahedronSet& tset) const
944{
945 tset.m_tetrahedra.Allocate(5 * (m_numVoxelsInsideSurface + m_numVoxelsOnSurface));
946 tset.m_scale = m_scale;
947 const short i0 = (short)m_dim[0];
948 const short j0 = (short)m_dim[1];
949 const short k0 = (short)m_dim[2];
950 tset.m_numTetrahedraOnSurface = 0;
951 tset.m_numTetrahedraInsideSurface = 0;
952 Tetrahedron tetrahedron;
953 for (short i = 0; i < i0; ++i) {
954 for (short j = 0; j < j0; ++j) {
955 for (short k = 0; k < k0; ++k) {
956 const unsigned char& value = GetVoxel(i, j, k);
957 if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
958 tetrahedron.m_data = value;
959 Vec3<double> p1((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
960 Vec3<double> p2((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
961 Vec3<double> p3((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
962 Vec3<double> p4((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
963 Vec3<double> p5((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
964 Vec3<double> p6((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
965 Vec3<double> p7((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
966 Vec3<double> p8((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
967
968 tetrahedron.m_pts[0] = p2;
969 tetrahedron.m_pts[1] = p4;
970 tetrahedron.m_pts[2] = p7;
971 tetrahedron.m_pts[3] = p5;
972 tset.m_tetrahedra.PushBack(tetrahedron);
973
974 tetrahedron.m_pts[0] = p6;
975 tetrahedron.m_pts[1] = p2;
976 tetrahedron.m_pts[2] = p7;
977 tetrahedron.m_pts[3] = p5;
978 tset.m_tetrahedra.PushBack(tetrahedron);
979
980 tetrahedron.m_pts[0] = p3;
981 tetrahedron.m_pts[1] = p4;
982 tetrahedron.m_pts[2] = p7;
983 tetrahedron.m_pts[3] = p2;
984 tset.m_tetrahedra.PushBack(tetrahedron);
985
986 tetrahedron.m_pts[0] = p1;
987 tetrahedron.m_pts[1] = p4;
988 tetrahedron.m_pts[2] = p2;
989 tetrahedron.m_pts[3] = p5;
990 tset.m_tetrahedra.PushBack(tetrahedron);
991
992 tetrahedron.m_pts[0] = p8;
993 tetrahedron.m_pts[1] = p5;
994 tetrahedron.m_pts[2] = p7;
995 tetrahedron.m_pts[3] = p4;
996 tset.m_tetrahedra.PushBack(tetrahedron);
997 if (value == PRIMITIVE_INSIDE_SURFACE) {
998 tset.m_numTetrahedraInsideSurface += 5;
999 }
1000 else {
1001 tset.m_numTetrahedraOnSurface += 5;
1002 }
1003 }
1004 }
1005 }
1006 }
1007}
1008
1009void Volume::AlignToPrincipalAxes(double (&rot)[3][3]) const
1010{
1011 const short i0 = (short)m_dim[0];
1012 const short j0 = (short)m_dim[1];
1013 const short k0 = (short)m_dim[2];
1014 Vec3<double> barycenter(0.0);
1015 size_t nVoxels = 0;
1016 for (short i = 0; i < i0; ++i) {
1017 for (short j = 0; j < j0; ++j) {
1018 for (short k = 0; k < k0; ++k) {
1019 const unsigned char& value = GetVoxel(i, j, k);
1020 if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
1021 barycenter[0] += i;
1022 barycenter[1] += j;
1023 barycenter[2] += k;
1024 ++nVoxels;
1025 }
1026 }
1027 }
1028 }
1029 barycenter /= (double)nVoxels;
1030
1031 double covMat[3][3] = { { 0.0, 0.0, 0.0 },
1032 { 0.0, 0.0, 0.0 },
1033 { 0.0, 0.0, 0.0 } };
1034 double x, y, z;
1035 for (short i = 0; i < i0; ++i) {
1036 for (short j = 0; j < j0; ++j) {
1037 for (short k = 0; k < k0; ++k) {
1038 const unsigned char& value = GetVoxel(i, j, k);
1039 if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
1040 x = i - barycenter[0];
1041 y = j - barycenter[1];
1042 z = k - barycenter[2];
1043 covMat[0][0] += x * x;
1044 covMat[1][1] += y * y;
1045 covMat[2][2] += z * z;
1046 covMat[0][1] += x * y;
1047 covMat[0][2] += x * z;
1048 covMat[1][2] += y * z;
1049 }
1050 }
1051 }
1052 }
1053 covMat[1][0] = covMat[0][1];
1054 covMat[2][0] = covMat[0][2];
1055 covMat[2][1] = covMat[1][2];
1056 double D[3][3];
1057 Diagonalize(covMat, rot, D);
1058}
1059TetrahedronSet::TetrahedronSet()
1060{
1061 m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
1062 m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0;
1063 m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0.0;
1064 m_scale = 1.0;
1065 m_numTetrahedraOnSurface = 0;
1066 m_numTetrahedraInsideSurface = 0;
1067 memset(m_Q, 0, sizeof(double) * 9);
1068 memset(m_D, 0, sizeof(double) * 9);
1069}
1070TetrahedronSet::~TetrahedronSet(void)
1071{
1072}
1073void TetrahedronSet::ComputeBB()
1074{
1075 const size_t nTetrahedra = m_tetrahedra.Size();
1076 if (nTetrahedra == 0)
1077 return;
1078
1079 for (int32_t h = 0; h < 3; ++h) {
1080 m_minBB[h] = m_maxBB[h] = m_tetrahedra[0].m_pts[0][h];
1081 m_barycenter[h] = 0.0;
1082 }
1083 for (size_t p = 0; p < nTetrahedra; ++p) {
1084 for (int32_t i = 0; i < 4; ++i) {
1085 for (int32_t h = 0; h < 3; ++h) {
1086 if (m_minBB[h] > m_tetrahedra[p].m_pts[i][h])
1087 m_minBB[h] = m_tetrahedra[p].m_pts[i][h];
1088 if (m_maxBB[h] < m_tetrahedra[p].m_pts[i][h])
1089 m_maxBB[h] = m_tetrahedra[p].m_pts[i][h];
1090 m_barycenter[h] += m_tetrahedra[p].m_pts[i][h];
1091 }
1092 }
1093 }
1094 m_barycenter /= (double)(4 * nTetrahedra);
1095}
1096void TetrahedronSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const
1097{
1098 const size_t CLUSTER_SIZE = 65536;
1099 const size_t nTetrahedra = m_tetrahedra.Size();
1100 if (nTetrahedra == 0)
1101 return;
1102
1103 SArray<Vec3<double> > cpoints;
1104
1105 Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE];
1106 size_t p = 0;
1107 while (p < nTetrahedra) {
1108 size_t q = 0;
1109 size_t s = 0;
1110 while (q < CLUSTER_SIZE && p < nTetrahedra) {
1111 if (m_tetrahedra[p].m_data == PRIMITIVE_ON_SURFACE) {
1112 ++s;
1113 if (s == sampling) {
1114 s = 0;
1115 for (int32_t a = 0; a < 4; ++a) {
1116 points[q++] = m_tetrahedra[p].m_pts[a];
1117 for (int32_t xx = 0; xx < 3; ++xx) {
1118 assert(m_tetrahedra[p].m_pts[a][xx] + EPS >= m_minBB[xx]);
1119 assert(m_tetrahedra[p].m_pts[a][xx] <= m_maxBB[xx] + EPS);
1120 }
1121 }
1122 }
1123 }
1124 ++p;
1125 }
1126 btConvexHullComputer ch;
1127 ch.compute((double*)points, 3 * sizeof(double), (int32_t)q, -1.0, -1.0);
1128 for (int32_t v = 0; v < ch.vertices.size(); v++) {
1129 cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
1130 }
1131 }
1132 delete[] points;
1133
1134 points = cpoints.Data();
1135 btConvexHullComputer ch;
1136 ch.compute((double*)points, 3 * sizeof(double), (int32_t)cpoints.Size(), -1.0, -1.0);
1137 meshCH.ResizePoints(0);
1138 meshCH.ResizeTriangles(0);
1139 for (int32_t v = 0; v < ch.vertices.size(); v++) {
1140 meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
1141 }
1142 const int32_t nt = ch.faces.size();
1143 for (int32_t t = 0; t < nt; ++t) {
1144 const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
1145 int32_t a = sourceEdge->getSourceVertex();
1146 int32_t b = sourceEdge->getTargetVertex();
1147 const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
1148 int32_t c = edge->getTargetVertex();
1149 while (c != a) {
1150 meshCH.AddTriangle(Vec3<int32_t>(a, b, c));
1151 edge = edge->getNextEdgeOfFace();
1152 b = c;
1153 c = edge->getTargetVertex();
1154 }
1155 }
1156}
1157inline bool TetrahedronSet::Add(Tetrahedron& tetrahedron)
1158{
1159 double v = ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]);
1160
1161 const double EPS = 0.0000000001;
1162 if (fabs(v) < EPS) {
1163 return false;
1164 }
1165 else if (v < 0.0) {
1166 Vec3<double> tmp = tetrahedron.m_pts[0];
1167 tetrahedron.m_pts[0] = tetrahedron.m_pts[1];
1168 tetrahedron.m_pts[1] = tmp;
1169 }
1170
1171 for (int32_t a = 0; a < 4; ++a) {
1172 for (int32_t xx = 0; xx < 3; ++xx) {
1173 assert(tetrahedron.m_pts[a][xx] + EPS >= m_minBB[xx]);
1174 assert(tetrahedron.m_pts[a][xx] <= m_maxBB[xx] + EPS);
1175 }
1176 }
1177 m_tetrahedra.PushBack(tetrahedron);
1178 return true;
1179}
1180
1181void TetrahedronSet::AddClippedTetrahedra(const Vec3<double> (&pts)[10], const int32_t nPts)
1182{
1183 const int32_t tetF[4][3] = { { 0, 1, 2 }, { 2, 1, 3 }, { 3, 1, 0 }, { 3, 0, 2 } };
1184 if (nPts < 4) {
1185 return;
1186 }
1187 else if (nPts == 4) {
1188 Tetrahedron tetrahedron;
1189 tetrahedron.m_data = PRIMITIVE_ON_SURFACE;
1190 tetrahedron.m_pts[0] = pts[0];
1191 tetrahedron.m_pts[1] = pts[1];
1192 tetrahedron.m_pts[2] = pts[2];
1193 tetrahedron.m_pts[3] = pts[3];
1194 if (Add(tetrahedron)) {
1195 ++m_numTetrahedraOnSurface;
1196 }
1197 }
1198 else if (nPts == 5) {
1199 const int32_t tet[15][4] = {
1200 { 0, 1, 2, 3 }, { 1, 2, 3, 4 }, { 0, 2, 3, 4 }, { 0, 1, 3, 4 }, { 0, 1, 2, 4 },
1201 };
1202 const int32_t rem[5] = { 4, 0, 1, 2, 3 };
1203 double maxVol = 0.0;
1204 int32_t h0 = -1;
1205 Tetrahedron tetrahedron0;
1206 tetrahedron0.m_data = PRIMITIVE_ON_SURFACE;
1207 for (int32_t h = 0; h < 5; ++h) {
1208 double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]);
1209 if (v > maxVol) {
1210 h0 = h;
1211 tetrahedron0.m_pts[0] = pts[tet[h][0]];
1212 tetrahedron0.m_pts[1] = pts[tet[h][1]];
1213 tetrahedron0.m_pts[2] = pts[tet[h][2]];
1214 tetrahedron0.m_pts[3] = pts[tet[h][3]];
1215 maxVol = v;
1216 }
1217 else if (-v > maxVol) {
1218 h0 = h;
1219 tetrahedron0.m_pts[0] = pts[tet[h][1]];
1220 tetrahedron0.m_pts[1] = pts[tet[h][0]];
1221 tetrahedron0.m_pts[2] = pts[tet[h][2]];
1222 tetrahedron0.m_pts[3] = pts[tet[h][3]];
1223 maxVol = -v;
1224 }
1225 }
1226 if (h0 == -1)
1227 return;
1228 if (Add(tetrahedron0)) {
1229 ++m_numTetrahedraOnSurface;
1230 }
1231 else {
1232 return;
1233 }
1234 int32_t a = rem[h0];
1235 maxVol = 0.0;
1236 int32_t h1 = -1;
1237 Tetrahedron tetrahedron1;
1238 tetrahedron1.m_data = PRIMITIVE_ON_SURFACE;
1239 for (int32_t h = 0; h < 4; ++h) {
1240 double v = ComputeVolume4(pts[a], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
1241 if (v > maxVol) {
1242 h1 = h;
1243 tetrahedron1.m_pts[0] = pts[a];
1244 tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
1245 tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
1246 tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
1247 maxVol = v;
1248 }
1249 }
1250 if (h1 == -1 && Add(tetrahedron1)) {
1251 ++m_numTetrahedraOnSurface;
1252 }
1253 }
1254 else if (nPts == 6) {
1255
1256 const int32_t tet[15][4] = { { 2, 3, 4, 5 }, { 1, 3, 4, 5 }, { 1, 2, 4, 5 }, { 1, 2, 3, 5 }, { 1, 2, 3, 4 },
1257 { 0, 3, 4, 5 }, { 0, 2, 4, 5 }, { 0, 2, 3, 5 }, { 0, 2, 3, 4 }, { 0, 1, 4, 5 },
1258 { 0, 1, 3, 5 }, { 0, 1, 3, 4 }, { 0, 1, 2, 5 }, { 0, 1, 2, 4 }, { 0, 1, 2, 3 } };
1259 const int32_t rem[15][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 }, { 0, 5 },
1260 { 1, 2 }, { 1, 3 }, { 1, 4 }, { 1, 5 }, { 2, 3 },
1261 { 2, 4 }, { 2, 5 }, { 3, 4 }, { 3, 5 }, { 4, 5 } };
1262 double maxVol = 0.0;
1263 int32_t h0 = -1;
1264 Tetrahedron tetrahedron0;
1265 tetrahedron0.m_data = PRIMITIVE_ON_SURFACE;
1266 for (int32_t h = 0; h < 15; ++h) {
1267 double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]);
1268 if (v > maxVol) {
1269 h0 = h;
1270 tetrahedron0.m_pts[0] = pts[tet[h][0]];
1271 tetrahedron0.m_pts[1] = pts[tet[h][1]];
1272 tetrahedron0.m_pts[2] = pts[tet[h][2]];
1273 tetrahedron0.m_pts[3] = pts[tet[h][3]];
1274 maxVol = v;
1275 }
1276 else if (-v > maxVol) {
1277 h0 = h;
1278 tetrahedron0.m_pts[0] = pts[tet[h][1]];
1279 tetrahedron0.m_pts[1] = pts[tet[h][0]];
1280 tetrahedron0.m_pts[2] = pts[tet[h][2]];
1281 tetrahedron0.m_pts[3] = pts[tet[h][3]];
1282 maxVol = -v;
1283 }
1284 }
1285 if (h0 == -1)
1286 return;
1287 if (Add(tetrahedron0)) {
1288 ++m_numTetrahedraOnSurface;
1289 }
1290 else {
1291 return;
1292 }
1293
1294 int32_t a0 = rem[h0][0];
1295 int32_t a1 = rem[h0][1];
1296 int32_t h1 = -1;
1297 Tetrahedron tetrahedron1;
1298 tetrahedron1.m_data = PRIMITIVE_ON_SURFACE;
1299 maxVol = 0.0;
1300 for (int32_t h = 0; h < 4; ++h) {
1301 double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
1302 if (v > maxVol) {
1303 h1 = h;
1304 tetrahedron1.m_pts[0] = pts[a0];
1305 tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
1306 tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
1307 tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
1308 maxVol = v;
1309 }
1310 }
1311 if (h1 != -1 && Add(tetrahedron1)) {
1312 ++m_numTetrahedraOnSurface;
1313 }
1314 else {
1315 h1 = -1;
1316 }
1317 maxVol = 0.0;
1318 int32_t h2 = -1;
1319 Tetrahedron tetrahedron2;
1320 tetrahedron2.m_data = PRIMITIVE_ON_SURFACE;
1321 for (int32_t h = 0; h < 4; ++h) {
1322 double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
1323 if (h == h1)
1324 continue;
1325 if (v > maxVol) {
1326 h2 = h;
1327 tetrahedron2.m_pts[0] = pts[a1];
1328 tetrahedron2.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
1329 tetrahedron2.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
1330 tetrahedron2.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
1331 maxVol = v;
1332 }
1333 }
1334 if (h1 != -1) {
1335 for (int32_t h = 0; h < 4; ++h) {
1336 double v = ComputeVolume4(pts[a1], tetrahedron1.m_pts[tetF[h][0]], tetrahedron1.m_pts[tetF[h][1]], tetrahedron1.m_pts[tetF[h][2]]);
1337 if (h == 1)
1338 continue;
1339 if (v > maxVol) {
1340 h2 = h;
1341 tetrahedron2.m_pts[0] = pts[a1];
1342 tetrahedron2.m_pts[1] = tetrahedron1.m_pts[tetF[h][0]];
1343 tetrahedron2.m_pts[2] = tetrahedron1.m_pts[tetF[h][1]];
1344 tetrahedron2.m_pts[3] = tetrahedron1.m_pts[tetF[h][2]];
1345 maxVol = v;
1346 }
1347 }
1348 }
1349 if (h2 != -1 && Add(tetrahedron2)) {
1350 ++m_numTetrahedraOnSurface;
1351 }
1352 }
1353 else {
1354 assert(0);
1355 }
1356}
1357
1358void TetrahedronSet::Intersect(const Plane& plane,
1359 SArray<Vec3<double> >* const positivePts,
1360 SArray<Vec3<double> >* const negativePts,
1361 const size_t sampling) const
1362{
1363 const size_t nTetrahedra = m_tetrahedra.Size();
1364 if (nTetrahedra == 0)
1365 return;
1366}
1367void TetrahedronSet::ComputeExteriorPoints(const Plane& plane,
1368 const Mesh& mesh,
1369 SArray<Vec3<double> >* const exteriorPts) const
1370{
1371}
1372void TetrahedronSet::ComputeClippedVolumes(const Plane& plane,
1373 double& positiveVolume,
1374 double& negativeVolume) const
1375{
1376 const size_t nTetrahedra = m_tetrahedra.Size();
1377 if (nTetrahedra == 0)
1378 return;
1379}
1380
1381void TetrahedronSet::SelectOnSurface(PrimitiveSet* const onSurfP) const
1382{
1383 TetrahedronSet* const onSurf = (TetrahedronSet*)onSurfP;
1384 const size_t nTetrahedra = m_tetrahedra.Size();
1385 if (nTetrahedra == 0)
1386 return;
1387 onSurf->m_tetrahedra.Resize(0);
1388 onSurf->m_scale = m_scale;
1389 onSurf->m_numTetrahedraOnSurface = 0;
1390 onSurf->m_numTetrahedraInsideSurface = 0;
1391 onSurf->m_barycenter = m_barycenter;
1392 onSurf->m_minBB = m_minBB;
1393 onSurf->m_maxBB = m_maxBB;
1394 for (int32_t i = 0; i < 3; ++i) {
1395 for (int32_t j = 0; j < 3; ++j) {
1396 onSurf->m_Q[i][j] = m_Q[i][j];
1397 onSurf->m_D[i][j] = m_D[i][j];
1398 }
1399 }
1400 Tetrahedron tetrahedron;
1401 for (size_t v = 0; v < nTetrahedra; ++v) {
1402 tetrahedron = m_tetrahedra[v];
1403 if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
1404 onSurf->m_tetrahedra.PushBack(tetrahedron);
1405 ++onSurf->m_numTetrahedraOnSurface;
1406 }
1407 }
1408}
1409void TetrahedronSet::Clip(const Plane& plane,
1410 PrimitiveSet* const positivePartP,
1411 PrimitiveSet* const negativePartP) const
1412{
1413 TetrahedronSet* const positivePart = (TetrahedronSet*)positivePartP;
1414 TetrahedronSet* const negativePart = (TetrahedronSet*)negativePartP;
1415 const size_t nTetrahedra = m_tetrahedra.Size();
1416 if (nTetrahedra == 0)
1417 return;
1418 positivePart->m_tetrahedra.Resize(0);
1419 negativePart->m_tetrahedra.Resize(0);
1420 positivePart->m_tetrahedra.Allocate(nTetrahedra);
1421 negativePart->m_tetrahedra.Allocate(nTetrahedra);
1422 negativePart->m_scale = positivePart->m_scale = m_scale;
1423 negativePart->m_numTetrahedraOnSurface = positivePart->m_numTetrahedraOnSurface = 0;
1424 negativePart->m_numTetrahedraInsideSurface = positivePart->m_numTetrahedraInsideSurface = 0;
1425 negativePart->m_barycenter = m_barycenter;
1426 positivePart->m_barycenter = m_barycenter;
1427 negativePart->m_minBB = m_minBB;
1428 positivePart->m_minBB = m_minBB;
1429 negativePart->m_maxBB = m_maxBB;
1430 positivePart->m_maxBB = m_maxBB;
1431 for (int32_t i = 0; i < 3; ++i) {
1432 for (int32_t j = 0; j < 3; ++j) {
1433 negativePart->m_Q[i][j] = positivePart->m_Q[i][j] = m_Q[i][j];
1434 negativePart->m_D[i][j] = positivePart->m_D[i][j] = m_D[i][j];
1435 }
1436 }
1437
1438 Tetrahedron tetrahedron;
1439 double delta, alpha;
1440 int32_t sign[4];
1441 int32_t npos, nneg;
1442 Vec3<double> posPts[10];
1443 Vec3<double> negPts[10];
1444 Vec3<double> P0, P1, M;
1445 const Vec3<double> n(plane.m_a, plane.m_b, plane.m_c);
1446 const int32_t edges[6][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 1, 2 }, { 1, 3 }, { 2, 3 } };
1447 double dist;
1448 for (size_t v = 0; v < nTetrahedra; ++v) {
1449 tetrahedron = m_tetrahedra[v];
1450 npos = nneg = 0;
1451 for (int32_t i = 0; i < 4; ++i) {
1452 dist = plane.m_a * tetrahedron.m_pts[i][0] + plane.m_b * tetrahedron.m_pts[i][1] + plane.m_c * tetrahedron.m_pts[i][2] + plane.m_d;
1453 if (dist > 0.0) {
1454 sign[i] = 1;
1455 posPts[npos] = tetrahedron.m_pts[i];
1456 ++npos;
1457 }
1458 else {
1459 sign[i] = -1;
1460 negPts[nneg] = tetrahedron.m_pts[i];
1461 ++nneg;
1462 }
1463 }
1464
1465 if (npos == 4) {
1466 positivePart->Add(tetrahedron);
1467 if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
1468 ++positivePart->m_numTetrahedraOnSurface;
1469 }
1470 else {
1471 ++positivePart->m_numTetrahedraInsideSurface;
1472 }
1473 }
1474 else if (nneg == 4) {
1475 negativePart->Add(tetrahedron);
1476 if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
1477 ++negativePart->m_numTetrahedraOnSurface;
1478 }
1479 else {
1480 ++negativePart->m_numTetrahedraInsideSurface;
1481 }
1482 }
1483 else {
1484 int32_t nnew = 0;
1485 for (int32_t j = 0; j < 6; ++j) {
1486 if (sign[edges[j][0]] * sign[edges[j][1]] == -1) {
1487 P0 = tetrahedron.m_pts[edges[j][0]];
1488 P1 = tetrahedron.m_pts[edges[j][1]];
1489 delta = (P0 - P1) * n;
1490 alpha = -(plane.m_d + (n * P1)) / delta;
1491 assert(alpha >= 0.0 && alpha <= 1.0);
1492 M = alpha * P0 + (1 - alpha) * P1;
1493 for (int32_t xx = 0; xx < 3; ++xx) {
1494 assert(M[xx] + EPS >= m_minBB[xx]);
1495 assert(M[xx] <= m_maxBB[xx] + EPS);
1496 }
1497 posPts[npos++] = M;
1498 negPts[nneg++] = M;
1499 ++nnew;
1500 }
1501 }
1502 negativePart->AddClippedTetrahedra(negPts, nneg);
1503 positivePart->AddClippedTetrahedra(posPts, npos);
1504 }
1505 }
1506}
1507void TetrahedronSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const
1508{
1509 const size_t nTetrahedra = m_tetrahedra.Size();
1510 if (nTetrahedra == 0)
1511 return;
1512 for (size_t v = 0; v < nTetrahedra; ++v) {
1513 const Tetrahedron& tetrahedron = m_tetrahedra[v];
1514 if (tetrahedron.m_data == value) {
1515 int32_t s = (int32_t)mesh.GetNPoints();
1516 mesh.AddPoint(tetrahedron.m_pts[0]);
1517 mesh.AddPoint(tetrahedron.m_pts[1]);
1518 mesh.AddPoint(tetrahedron.m_pts[2]);
1519 mesh.AddPoint(tetrahedron.m_pts[3]);
1520 mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 1, s + 2));
1521 mesh.AddTriangle(Vec3<int32_t>(s + 2, s + 1, s + 3));
1522 mesh.AddTriangle(Vec3<int32_t>(s + 3, s + 1, s + 0));
1523 mesh.AddTriangle(Vec3<int32_t>(s + 3, s + 0, s + 2));
1524 }
1525 }
1526}
1527const double TetrahedronSet::ComputeVolume() const
1528{
1529 const size_t nTetrahedra = m_tetrahedra.Size();
1530 if (nTetrahedra == 0)
1531 return 0.0;
1532 double volume = 0.0;
1533 for (size_t v = 0; v < nTetrahedra; ++v) {
1534 const Tetrahedron& tetrahedron = m_tetrahedra[v];
1535 volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]));
1536 }
1537 return volume / 6.0;
1538}
1539const double TetrahedronSet::ComputeMaxVolumeError() const
1540{
1541 const size_t nTetrahedra = m_tetrahedra.Size();
1542 if (nTetrahedra == 0)
1543 return 0.0;
1544 double volume = 0.0;
1545 for (size_t v = 0; v < nTetrahedra; ++v) {
1546 const Tetrahedron& tetrahedron = m_tetrahedra[v];
1547 if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
1548 volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]));
1549 }
1550 }
1551 return volume / 6.0;
1552}
1553void TetrahedronSet::RevertAlignToPrincipalAxes()
1554{
1555 const size_t nTetrahedra = m_tetrahedra.Size();
1556 if (nTetrahedra == 0)
1557 return;
1558 double x, y, z;
1559 for (size_t v = 0; v < nTetrahedra; ++v) {
1560 Tetrahedron& tetrahedron = m_tetrahedra[v];
1561 for (int32_t i = 0; i < 4; ++i) {
1562 x = tetrahedron.m_pts[i][0] - m_barycenter[0];
1563 y = tetrahedron.m_pts[i][1] - m_barycenter[1];
1564 z = tetrahedron.m_pts[i][2] - m_barycenter[2];
1565 tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[0][1] * y + m_Q[0][2] * z + m_barycenter[0];
1566 tetrahedron.m_pts[i][1] = m_Q[1][0] * x + m_Q[1][1] * y + m_Q[1][2] * z + m_barycenter[1];
1567 tetrahedron.m_pts[i][2] = m_Q[2][0] * x + m_Q[2][1] * y + m_Q[2][2] * z + m_barycenter[2];
1568 }
1569 }
1570 ComputeBB();
1571}
1572void TetrahedronSet::ComputePrincipalAxes()
1573{
1574 const size_t nTetrahedra = m_tetrahedra.Size();
1575 if (nTetrahedra == 0)
1576 return;
1577 double covMat[3][3] = { { 0.0, 0.0, 0.0 },
1578 { 0.0, 0.0, 0.0 },
1579 { 0.0, 0.0, 0.0 } };
1580 double x, y, z;
1581 for (size_t v = 0; v < nTetrahedra; ++v) {
1582 Tetrahedron& tetrahedron = m_tetrahedra[v];
1583 for (int32_t i = 0; i < 4; ++i) {
1584 x = tetrahedron.m_pts[i][0] - m_barycenter[0];
1585 y = tetrahedron.m_pts[i][1] - m_barycenter[1];
1586 z = tetrahedron.m_pts[i][2] - m_barycenter[2];
1587 covMat[0][0] += x * x;
1588 covMat[1][1] += y * y;
1589 covMat[2][2] += z * z;
1590 covMat[0][1] += x * y;
1591 covMat[0][2] += x * z;
1592 covMat[1][2] += y * z;
1593 }
1594 }
1595 double n = nTetrahedra * 4.0;
1596 covMat[0][0] /= n;
1597 covMat[1][1] /= n;
1598 covMat[2][2] /= n;
1599 covMat[0][1] /= n;
1600 covMat[0][2] /= n;
1601 covMat[1][2] /= n;
1602 covMat[1][0] = covMat[0][1];
1603 covMat[2][0] = covMat[0][2];
1604 covMat[2][1] = covMat[1][2];
1605 Diagonalize(covMat, m_Q, m_D);
1606}
1607void TetrahedronSet::AlignToPrincipalAxes()
1608{
1609 const size_t nTetrahedra = m_tetrahedra.Size();
1610 if (nTetrahedra == 0)
1611 return;
1612 double x, y, z;
1613 for (size_t v = 0; v < nTetrahedra; ++v) {
1614 Tetrahedron& tetrahedron = m_tetrahedra[v];
1615 for (int32_t i = 0; i < 4; ++i) {
1616 x = tetrahedron.m_pts[i][0] - m_barycenter[0];
1617 y = tetrahedron.m_pts[i][1] - m_barycenter[1];
1618 z = tetrahedron.m_pts[i][2] - m_barycenter[2];
1619 tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[1][0] * y + m_Q[2][0] * z + m_barycenter[0];
1620 tetrahedron.m_pts[i][1] = m_Q[0][1] * x + m_Q[1][1] * y + m_Q[2][1] * z + m_barycenter[1];
1621 tetrahedron.m_pts[i][2] = m_Q[0][2] * x + m_Q[1][2] * y + m_Q[2][2] * z + m_barycenter[2];
1622 }
1623 }
1624 ComputeBB();
1625}
1626}
1627