SAF
Loading...
Searching...
No Matches
convhull_3d.c
Go to the documentation of this file.
1/*
2 Copyright (c) 2017-2018 Leo McCormack
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
42
43#include <stdlib.h>
44#include <stdio.h>
45#include <math.h>
46#include <string.h>
47#include <float.h>
48#include <ctype.h>
49#include <string.h>
50#include "convhull_3d.h"
51#include "../modules/saf_utilities/saf_utilities.h"
52#include "saf_externals.h"
53
54#define CONVHULL_ND_MAX_DIMENSIONS ( 5 )
55
56/* Force using CBLAS: */
57#ifndef CONVHULL_3D_USE_CBLAS
58# define CONVHULL_3D_USE_CBLAS
59#endif
60
61#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
62 #define CV_STRNCPY(a,b,c) strncpy_s(a,c+1,b,c);
63 #define CV_STRCAT(a,b) strcat_s(a,sizeof(b),b);
64#else
65 #define CV_STRNCPY(a,b,c) strncpy(a,b,c);
66 #define CV_STRCAT(a,b) strcat(a,b);
67#endif
68#ifdef CONVHULL_3D_USE_FLOAT_PRECISION
69 #define CH_FLT_MIN FLT_MIN
70 #define CH_FLT_MAX FLT_MAX
71 #define CH_NOISE_VAL 0.00001f
72#else
73 #define CH_FLT_MIN DBL_MIN
74 #define CH_FLT_MAX DBL_MAX
75 #define CH_NOISE_VAL 0.0000001
76#endif
77#ifndef MIN
78 #define MIN(a,b) (( (a) < (b) ) ? (a) : (b) )
79#endif
80#ifndef MAX
81 #define MAX(a,b) (( (a) > (b) ) ? (a) : (b) )
82#endif
83#define CH_MAX_NUM_FACES 50000
84
86typedef struct float_w_idx {
87 CH_FLOAT val;
88 int idx;
89}float_w_idx;
90
92typedef struct int_w_idx {
93 int val;
94 int idx;
95}int_w_idx;
96
97/* internal functions prototypes: */
98static int cmp_asc_float(const void*, const void*);
99static int cmp_desc_float(const void*, const void*);
100static int cmp_asc_int(const void*, const void*);
101static int cmp_desc_int(const void*, const void*);
102static void sort_float(CH_FLOAT*, CH_FLOAT*, int*, int, int);
103static void sort_int(int*, int*, int*, int, int);
104static ch_vec3 cross(ch_vec3*, ch_vec3*);
105static CH_FLOAT det_4x4(CH_FLOAT*);
106static void plane_3d(CH_FLOAT*, CH_FLOAT*, CH_FLOAT*);
107static void ismember(int*, int*, int*, int, int);
108
109/* internal functions definitions: */
110static int cmp_asc_float(const void *a,const void *b) {
111 struct float_w_idx *a1 = (struct float_w_idx*)a;
112 struct float_w_idx *a2 = (struct float_w_idx*)b;
113 if((*a1).val<(*a2).val)return -1;
114 else if((*a1).val>(*a2).val)return 1;
115 else return 0;
116}
117
118static int cmp_desc_float(const void *a,const void *b) {
119 struct float_w_idx *a1 = (struct float_w_idx*)a;
120 struct float_w_idx *a2 = (struct float_w_idx*)b;
121 if((*a1).val>(*a2).val)return -1;
122 else if((*a1).val<(*a2).val)return 1;
123 else return 0;
124}
125
126static int cmp_asc_int(const void *a,const void *b) {
127 struct int_w_idx *a1 = (struct int_w_idx*)a;
128 struct int_w_idx *a2 = (struct int_w_idx*)b;
129 if((*a1).val<(*a2).val)return -1;
130 else if((*a1).val>(*a2).val)return 1;
131 else return 0;
132}
133
134static int cmp_desc_int(const void *a,const void *b) {
135 struct int_w_idx *a1 = (struct int_w_idx*)a;
136 struct int_w_idx *a2 = (struct int_w_idx*)b;
137 if((*a1).val>(*a2).val)return -1;
138 else if((*a1).val<(*a2).val)return 1;
139 else return 0;
140}
141
142static void sort_float
143(
144 CH_FLOAT* in_vec, /* vector[len] to be sorted */
145 CH_FLOAT* out_vec, /* if NULL, then in_vec is sorted "in-place" */
146 int* new_idices, /* set to NULL if you don't need them */
147 int len, /* number of elements in vectors, must be consistent with the input data */
148 int descendFLAG /* !1:ascending, 1:descending */
149)
150{
151 int i;
152 struct float_w_idx *data;
153
154 data = (float_w_idx*)malloc(len*sizeof(float_w_idx));
155 for(i=0;i<len;i++) {
156 data[i].val=in_vec[i];
157 data[i].idx=i;
158 }
159 if(descendFLAG)
160 qsort(data,len,sizeof(data[0]),cmp_desc_float);
161 else
162 qsort(data,len,sizeof(data[0]),cmp_asc_float);
163 for(i=0;i<len;i++){
164 if (out_vec!=NULL)
165 out_vec[i] = data[i].val;
166 else
167 in_vec[i] = data[i].val; /* overwrite input vector */
168 if(new_idices!=NULL)
169 new_idices[i] = data[i].idx;
170 }
171 free(data);
172}
173
174static void sort_int
175(
176 int* in_vec, /* vector[len] to be sorted */
177 int* out_vec, /* if NULL, then in_vec is sorted "in-place" */
178 int* new_idices, /* set to NULL if you don't need them */
179 int len, /* number of elements in vectors, must be consistent with the input data */
180 int descendFLAG /* !1:ascending, 1:descending */
181)
182{
183 int i;
184 struct int_w_idx *data;
185
186 data = (int_w_idx*)malloc(len*sizeof(int_w_idx));
187 for(i=0;i<len;i++) {
188 data[i].val=in_vec[i];
189 data[i].idx=i;
190 }
191 if(descendFLAG)
192 qsort(data,len,sizeof(data[0]),cmp_desc_int);
193 else
194 qsort(data,len,sizeof(data[0]),cmp_asc_int);
195 for(i=0;i<len;i++){
196 if (out_vec!=NULL)
197 out_vec[i] = data[i].val;
198 else
199 in_vec[i] = data[i].val; /* overwrite input vector */
200 if(new_idices!=NULL)
201 new_idices[i] = data[i].idx;
202 }
203 free(data);
204}
205
206static ch_vec3 cross(ch_vec3* v1, ch_vec3* v2)
207{
208 ch_vec3 cross;
209 cross.x = v1->y * v2->z - v1->z * v2->y;
210 cross.y = v1->z * v2->x - v1->x * v2->z;
211 cross.z = v1->x * v2->y - v1->y * v2->x;
212 return cross;
213}
214
215/* calculates the determinent of a 4x4 matrix */
216static CH_FLOAT det_4x4(CH_FLOAT* m) {
217 return
218 m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12] -
219 m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12] +
220 m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12] -
221 m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13] +
222 m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13] -
223 m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13] +
224 m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14] -
225 m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14] +
226 m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14] -
227 m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15] +
228 m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15] -
229 m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15];
230}
231
232static CH_FLOAT det_NxN(CH_FLOAT* m, int d){
233#ifdef CONVHULL_3D_USE_FLOAT_PRECISION
234 return utility_sdet(NULL, (CH_FLOAT*)m, d);
235#else
236 return utility_ddet(NULL, (CH_FLOAT*)m, d);
237#endif
238}
239
240/* Calculates the coefficients of the equation of a PLANE in 3D.
241 * Original Copyright (c) 2014, George Papazafeiropoulos
242 * Distributed under the BSD (2-clause) license
243 */
244static void plane_3d
245(
246 CH_FLOAT* p,
247 CH_FLOAT* c,
248 CH_FLOAT* d
249)
250{
251 int i, j, k, l;
252 int r[3];
253 CH_FLOAT sign, det, norm_c;
254 CH_FLOAT pdiff[2][3], pdiff_s[2][2];
255
256 for(i=0; i<2; i++)
257 for(j=0; j<3; j++)
258 pdiff[i][j] = p[(i+1)*3+j] - p[i*3+j];
259 memset(c, 0, 3*sizeof(CH_FLOAT));
260 sign = 1.0;
261 for(i=0; i<3; i++)
262 r[i] = i;
263 for(i=0; i<3; i++){
264 for(j=0; j<2; j++){
265 for(k=0, l=0; k<3; k++){
266 if(r[k]!=i){
267 pdiff_s[j][l] = pdiff[j][k];
268 l++;
269 }
270 }
271 }
272 det = pdiff_s[0][0]*pdiff_s[1][1] - pdiff_s[1][0]*pdiff_s[0][1];
273 c[i] = sign * det;
274 sign *= -1.0;
275 }
276 norm_c = (CH_FLOAT)0.0;
277 for(i=0; i<3; i++)
278 norm_c += (pow(c[i], 2.0));
279 norm_c = sqrt(norm_c);
280 for(i=0; i<3; i++)
281 c[i] /= norm_c;
282 (*d) = (CH_FLOAT)0.0;
283 for(i=0; i<3; i++)
284 (*d) += -p[i] * c[i];
285}
286
287/* Calculates the coefficients of the equation of a PLANE in ND.
288 * Original Copyright (c) 2014, George Papazafeiropoulos
289 * Distributed under the BSD (2-clause) license
290 */
291static void plane_nd
292(
293 const int Nd,
294 CH_FLOAT* p,
295 CH_FLOAT* c,
296 CH_FLOAT* d
297)
298{
299 int i, j, k, l;
300 int r[CONVHULL_ND_MAX_DIMENSIONS];
301 CH_FLOAT sign, det, norm_c;
302 CH_FLOAT pdiff[CONVHULL_ND_MAX_DIMENSIONS-1][CONVHULL_ND_MAX_DIMENSIONS], pdiff_s[(CONVHULL_ND_MAX_DIMENSIONS-1)*(CONVHULL_ND_MAX_DIMENSIONS-1)];
303
304 for(i=0; i<Nd-1; i++)
305 for(j=0; j<Nd; j++)
306 pdiff[i][j] = p[(i+1)*Nd+j] - p[i*Nd+j];
307 memset(c, 0, Nd*sizeof(CH_FLOAT));
308 sign = 1.0;
309 for(i=0; i<Nd; i++)
310 r[i] = i;
311 for(i=0; i<Nd; i++){
312 for(j=0; j<Nd-1; j++){
313 for(k=0, l=0; k<Nd; k++){
314 if(r[k]!=i){
315 pdiff_s[j*(Nd-1)+l] = pdiff[j][k];
316 l++;
317 }
318 }
319 }
320 /* Determinant 1 dimension lower */
321 if(Nd==3)
322 det = pdiff_s[0*(Nd-1)+0]*pdiff_s[1*(Nd-1)+1] - pdiff_s[1*(Nd-1)+0]*pdiff_s[0*(Nd-1)+1];
323 else if(Nd==5)
324 det = det_4x4((CH_FLOAT*)pdiff_s);
325 else{
326 det = det_NxN((CH_FLOAT*)pdiff_s, Nd-1);
327 }
328 c[i] = sign * det;
329 sign *= -1.0;
330 }
331 norm_c = (CH_FLOAT)0.0;
332 for(i=0; i<Nd; i++)
333 norm_c += (pow(c[i], 2.0));
334 norm_c = sqrt(norm_c);
335 for(i=0; i<Nd; i++)
336 c[i] /= norm_c;
337 (*d) = (CH_FLOAT)0.0;
338 for(i=0; i<Nd; i++)
339 (*d) += -p[i] * c[i];
340}
341
342static void ismember
343(
344 int* pLeft, /* left vector; nLeftElements x 1 */
345 int* pRight, /* right vector; nRightElements x 1 */
346 int* pOut, /* 0, unless pRight elements are present in pLeft then 1; nLeftElements x 1 */
347 int nLeftElements, /* number of elements in pLeft */
348 int nRightElements /* number of elements in pRight */
349)
350{
351 int i, j;
352 memset(pOut, 0, nLeftElements*sizeof(int));
353 for(i=0; i< nLeftElements; i++)
354 for(j=0; j< nRightElements; j++)
355 if(pLeft[i] == pRight[j] )
356 pOut[i] = 1;
357}
358
359/* A C version of the 3D quickhull matlab implementation from here:
360 * https://www.mathworks.com/matlabcentral/fileexchange/48509-computational-geometry-toolbox?focused=3851550&tab=example
361 * (*out_faces) is returned as NULL, if triangulation fails *
362 * Original Copyright (c) 2014, George Papazafeiropoulos
363 * Distributed under the BSD (2-clause) license
364 * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, David P. Dobkin
365 * and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, July 30, 1993"
366 */
367void convhull_3d_build
368(
369 ch_vertex* const in_vertices,
370 const int nVert,
371 int** out_faces,
372 CH_FLOAT** out_cf,
373 CH_FLOAT** out_df,
374 int* nOut_faces
375)
376{
377 int i, j, k, l, h;
378 int nFaces, p, d;
379 int* aVec, *faces;
380 CH_FLOAT dfi, v, max_p, min_p;
381 CH_FLOAT* points, *cf, *cfi, *df, *p_s, *span;
382
383 if(nVert<=3 || in_vertices==NULL){
384 (*out_faces) = NULL;
385 (*nOut_faces) = 0;
386 if(out_cf!=NULL)
387 (*out_cf) = NULL;
388 if(out_df!=NULL)
389 (*out_df) = NULL;
390 return;
391 }
392
393 /* 3 dimensions */
394 d = 3;
395
396 /* Add noise to the points */
397 points = (CH_FLOAT*)malloc(nVert*(d+1)*sizeof(CH_FLOAT));
398 for(i=0; i<nVert; i++){
399 for(j=0; j<d; j++)
400 points[i*(d+1)+j] = in_vertices[i].v[j] + CH_NOISE_VAL*(CH_FLOAT)rand()/(CH_FLOAT)RAND_MAX; /* noise mitigates duplicates */
401 points[i*(d+1)+d] = 1.0f; /* add a last column of ones. Used only for determinant calculation */
402 }
403
404 /* Find the span */
405 span = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
406 for(j=0; j<d; j++){
407 max_p = -2.23e+13; min_p = 2.23e+13;
408 for(i=0; i<nVert; i++){
409 max_p = MAX(max_p, points[i*(d+1)+j]);
410 min_p = MIN(min_p, points[i*(d+1)+j]);
411 }
412 span[j] = max_p - min_p;
413 assert(span[j]>0.0000001f);
414 }
415
416 /* The initial convex hull is a simplex with (d+1) facets, where d is the number of dimensions */
417 nFaces = (d+1);
418 faces = (int*)calloc(nFaces*d, sizeof(int));
419 aVec = (int*)malloc(nFaces*sizeof(int));
420 for(i=0; i<nFaces; i++)
421 aVec[i] = i;
422
423 /* Each column of cf contains the coefficients of a plane */
424 cf = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
425 cfi = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
426 df = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
427 p_s = (CH_FLOAT*)malloc(d*d*sizeof(CH_FLOAT));
428 for(i=0; i<nFaces; i++){
429 /* Set the indices of the points defining the face */
430 for(j=0, k=0; j<(d+1); j++){
431 if(aVec[j]!=i){
432 faces[i*d+k] = aVec[j];
433 k++;
434 }
435 }
436
437 /* Calculate and store the plane coefficients of the face */
438 for(j=0; j<d; j++)
439 for(k=0; k<d; k++)
440 p_s[j*d+k] = points[(faces[i*d+j])*(d+1) + k];
441
442 /* Calculate and store the plane coefficients of the face */
443 plane_3d(p_s, cfi, &dfi);
444 for(j=0; j<d; j++)
445 cf[i*d+j] = cfi[j];
446 df[i] = dfi;
447 }
448 CH_FLOAT *A;
449 int *bVec, *fVec, *asfVec;
450 int face_tmp[2];
451
452 /* Check to make sure that faces are correctly oriented */
453 bVec = (int*)malloc((d+1)*sizeof(int));
454 for(i=0; i<d+1; i++)
455 bVec[i] = i;
456
457 /* A contains the coordinates of the points forming a simplex */
458 A = (CH_FLOAT*)calloc((d+1)*(d+1), sizeof(CH_FLOAT));
459 fVec = (int*)malloc((d+1)*sizeof(int));
460 asfVec = (int*)malloc((d+1)*sizeof(int));
461 for(k=0; k<(d+1); k++){
462 /* Get the point that is not on the current face (point p) */
463 for(i=0; i<d; i++)
464 fVec[i] = faces[k*d+i];
465 sort_int(fVec, NULL, NULL, d, 0); /* sort accending */
466 p=k;
467 for(i=0; i<d; i++)
468 for(j=0; j<(d+1); j++)
469 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
470 for(; i<(d+1); i++)
471 for(j=0; j<(d+1); j++)
472 A[i*(d+1)+j] = points[p*(d+1)+j];
473
474 /* det(A) determines the orientation of the face */
475 v = det_4x4(A);
476
477 /* Orient so that each point on the original simplex can't see the opposite face */
478 if(v<0){
479 /* Reverse the order of the last two vertices to change the volume */
480 for(j=0; j<2; j++)
481 face_tmp[j] = faces[k*d+d-j-1];
482 for(j=0; j<2; j++)
483 faces[k*d+d-j-1] = face_tmp[1-j];
484
485 /* Modify the plane coefficients of the properly oriented faces */
486 for(j=0; j<d; j++)
487 cf[k*d+j] = -cf[k*d+j];
488 df[k] = -df[k];
489 for(i=0; i<d; i++)
490 for(j=0; j<(d+1); j++)
491 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
492 for(; i<(d+1); i++)
493 for(j=0; j<(d+1); j++)
494 A[i*(d+1)+j] = points[p*(d+1)+j];
495 }
496 }
497
498 /* Coordinates of the center of the point set */
499 CH_FLOAT* meanp, *absdist, *reldist, *desReldist;
500 meanp = (CH_FLOAT*)calloc(d, sizeof(CH_FLOAT));
501 for(i=d+1; i<nVert; i++)
502 for(j=0; j<d; j++)
503 meanp[j] += points[i*(d+1)+j];
504 for(j=0; j<d; j++)
505 meanp[j] = meanp[j]/(CH_FLOAT)(nVert-d-1);
506
507 /* Absolute distance of points from the center */
508 absdist = (CH_FLOAT*)malloc((nVert-d-1)*d * sizeof(CH_FLOAT));
509 for(i=d+1, k=0; i<nVert; i++, k++)
510 for(j=0; j<d; j++)
511 absdist[k*d+j] = (points[i*(d+1)+j] - meanp[j])/span[j];
512
513 /* Relative distance of points from the center */
514 reldist = (CH_FLOAT*)calloc((nVert-d-1), sizeof(CH_FLOAT));
515 desReldist = (CH_FLOAT*)malloc((nVert-d-1) * sizeof(CH_FLOAT));
516 for(i=0; i<(nVert-d-1); i++)
517 for(j=0; j<d; j++)
518 reldist[i] += pow(absdist[i*d+j], 2.0);
519
520 /* Sort from maximum to minimum relative distance */
521 int num_pleft;
522 int* ind, *pleft;
523 ind = (int*)malloc((nVert-d-1) * sizeof(int));
524 pleft = (int*)malloc((nVert-d-1) * sizeof(int));
525 sort_float(reldist, desReldist, ind, (nVert-d-1), 1);
526
527 /* Initialize the vector of points left. The points with the larger relative
528 distance from the center are scanned first. */
529 num_pleft = (nVert-d-1);
530 for(i=0; i<num_pleft; i++)
531 pleft[i] = ind[i]+d+1;
532
533 /* Loop over all remaining points that are not deleted. Deletion of points
534 occurs every #iter2del# iterations of this while loop */
535 memset(A, 0, (d+1)*(d+1) * sizeof(CH_FLOAT));
536
537 /* The main loop for the quickhull algorithm */
538 CH_FLOAT detA;
539 CH_FLOAT* points_cf, *points_s;
540 int* visible_ind, *visible, *nonvisible_faces, *f0, *face_s, *u, *gVec, *horizon, *hVec, *pp, *hVec_mem_face;
541 int num_visible_ind, num_nonvisible_faces, n_newfaces, count, vis;
542 int f0_sum, u_len, start, num_p, index, horizon_size1;
543 int FUCKED;
544 FUCKED = 0;
545 u = horizon = NULL;
546 nFaces = d+1;
547 visible_ind = (int*)malloc(nFaces*sizeof(int));
548 points_cf = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
549 points_s = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
550 face_s = (int*)malloc(d*sizeof(int));
551 gVec = (int*)malloc(d*sizeof(int));
552 while( (num_pleft>0) ){
553 /* i is the first point of the points left */
554 i = pleft[0];
555
556 /* Delete the point selected */
557 for(j=0; j<num_pleft-1; j++)
558 pleft[j] = pleft[j+1];
559 num_pleft--;
560 if(num_pleft == 0)
561 free(pleft);
562 else
563 pleft = (int*)realloc(pleft, num_pleft*sizeof(int));
564
565 /* find visible faces */
566 for(j=0; j<d; j++)
567 points_s[j] = points[i*(d+1)+j];
568 points_cf = (CH_FLOAT*)realloc(points_cf, nFaces*sizeof(CH_FLOAT));
569 visible_ind = (int*)realloc(visible_ind, nFaces*sizeof(int));
570#ifdef CONVHULL_3D_USE_CBLAS
571 #ifdef CONVHULL_3D_USE_FLOAT_PRECISION
572 cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f,
573 points_s, d,
574 cf, d, 0.0f,
575 points_cf, nFaces);
576 #else
577 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0,
578 points_s, d,
579 cf, d, 0.0,
580 points_cf, nFaces);
581 #endif
582#else
583 for (j = 0; j < nFaces; j++) {
584 points_cf[j] = 0;
585 for (k = 0; k < d; k++)
586 points_cf[j] += points_s[k]*cf[j*d+k];
587 }
588#endif
589 num_visible_ind = 0;
590 for(j=0; j<nFaces; j++){
591 if(points_cf[j] + df[j] > 0.0){
592 num_visible_ind++; /* will sum to 0 if none are visible */
593 visible_ind[j] = 1;
594 }
595 else
596 visible_ind[j] = 0;
597 }
598 num_nonvisible_faces = nFaces - num_visible_ind;
599
600 /* proceed if there are any visible faces */
601 if(num_visible_ind!=0){
602 /* Find visible face indices */
603 visible = (int*)malloc(num_visible_ind*sizeof(int));
604 for(j=0, k=0; j<nFaces; j++){
605 if(visible_ind[j]==1){
606 visible[k]=j;
607 k++;
608 }
609 }
610
611 /* Find nonvisible faces */
612 nonvisible_faces = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
613 f0 = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
614 for(j=0, k=0; j<nFaces; j++){
615 if(visible_ind[j]==0){
616 for(l=0; l<d; l++)
617 nonvisible_faces[k*d+l]= faces[j*d+l];
618 k++;
619 }
620 }
621
622 /* Create horizon (count is the number of the edges of the horizon) */
623 count=0;
624 for(j=0; j<num_visible_ind; j++){
625 /* visible face */
626 vis = visible[j];
627 for(k=0; k<d; k++)
628 face_s[k] = faces[vis*d+k];
629 sort_int(face_s, NULL, NULL, d, 0);
630 ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces*d, d);
631 u_len = 0;
632
633 /* u are the nonvisible faces connected to the face v, if any */
634 for(k=0; k<num_nonvisible_faces; k++){
635 f0_sum = 0;
636 for(l=0; l<d; l++)
637 f0_sum += f0[k*d + l];
638 if(f0_sum == d-1){
639 u_len++;
640 if(u_len==1)
641 u = (int*)malloc(u_len*sizeof(int));
642 else
643 u = (int*)realloc(u, u_len*sizeof(int));
644 u[u_len-1] = k;
645 }
646 }
647 for(k=0; k<u_len; k++){
648 /* The boundary between the visible face v and the k(th) nonvisible face connected to the face v forms part of the horizon */
649 count++;
650 if(count==1)
651 horizon = (int*)malloc(count*(d-1)*sizeof(int));
652 else
653 horizon = (int*)realloc(horizon, count*(d-1)*sizeof(int));
654 for(l=0; l<d; l++)
655 gVec[l] = nonvisible_faces[u[k]*d+l];
656 for(l=0, h=0; l<d; l++){
657 if(f0[u[k]*d+l]){
658 horizon[(count-1)*(d-1)+h] = gVec[l];
659 h++;
660 }
661 }
662 }
663 if(u_len!=0)
664 free(u);
665 }
666 horizon_size1 = count;
667 for(j=0, l=0; j<nFaces; j++){
668 if(!visible_ind[j]){
669 /* Delete visible faces */
670 for(k=0; k<d; k++)
671 faces[l*d+k] = faces[j*d+k];
672
673 /* Delete the corresponding plane coefficients of the faces */
674 for(k=0; k<d; k++)
675 cf[l*d+k] = cf[j*d+k];
676 df[l] = df[j];
677 l++;
678 }
679 }
680
681 /* Update the number of faces */
682 nFaces = nFaces-num_visible_ind;
683 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
684 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
685 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
686
687 /* start is the first row of the new faces */
688 start=nFaces;
689
690 /* Add faces connecting horizon to the new point */
691 n_newfaces = horizon_size1;
692 for(j=0; j<n_newfaces; j++){
693 nFaces++;
694 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
695 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
696 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
697 for(k=0; k<d-1; k++)
698 faces[(nFaces-1)*d+k] = horizon[j*(d-1)+k];
699 faces[(nFaces-1)*d+(d-1)] = i;
700
701 /* Calculate and store appropriately the plane coefficients of the faces */
702 for(k=0; k<d; k++)
703 for(l=0; l<d; l++)
704 p_s[k*d+l] = points[(faces[(nFaces-1)*d+k])*(d+1) + l];
705 plane_3d(p_s, cfi, &dfi);
706 for(k=0; k<d; k++)
707 cf[(nFaces-1)*d+k] = cfi[k];
708 df[(nFaces-1)] = dfi;
709 if(nFaces > CH_MAX_NUM_FACES){
710 FUCKED = 1;
711 nFaces = 0;
712 break;
713 }
714 }
715
716 /* Orient each new face properly */
717 hVec = (int*)malloc( nFaces*sizeof(int));
718 hVec_mem_face = (int*)malloc( nFaces*sizeof(int));
719 for(j=0; j<nFaces; j++)
720 hVec[j] = j;
721 for(k=start; k<nFaces; k++){
722 for(j=0; j<d; j++)
723 face_s[j] = faces[k*d+j];
724 sort_int(face_s, NULL, NULL, d, 0);
725 ismember(hVec, face_s, hVec_mem_face, nFaces, d);
726 num_p = 0;
727 for(j=0; j<nFaces; j++)
728 if(!hVec_mem_face[j])
729 num_p++;
730 pp = (int*)malloc(num_p*sizeof(int));
731 for(j=0, l=0; j<nFaces; j++){
732 if(!hVec_mem_face[j]){
733 pp[l] = hVec[j];
734 l++;
735 }
736 }
737 index = 0;
738 detA = 0.0;
739
740 /* While new point is coplanar, choose another point */
741 while(detA==0.0){
742 for(j=0;j<d; j++)
743 for(l=0; l<d+1; l++)
744 A[j*(d+1)+l] = points[(faces[k*d+j])*(d+1) + l];
745 for(; j<d+1; j++)
746 for(l=0; l<d+1; l++)
747 A[j*(d+1)+l] = points[pp[index]*(d+1)+l];
748 index++;
749 detA = det_4x4(A);
750 }
751
752 /* Orient faces so that each point on the original simplex can't see the opposite face */
753 if (detA<0.0){
754 /* If orientation is improper, reverse the order to change the volume sign */
755 for(j=0; j<2; j++)
756 face_tmp[j] = faces[k*d+d-j-1];
757 for(j=0; j<2; j++)
758 faces[k*d+d-j-1] = face_tmp[1-j];
759
760 /* Modify the plane coefficients of the properly oriented faces */
761 for(j=0; j<d; j++)
762 cf[k*d+j] = -cf[k*d+j];
763 df[k] = -df[k];
764 for(l=0; l<d; l++)
765 for(j=0; j<d+1; j++)
766 A[l*(d+1)+j] = points[(faces[k*d+l])*(d+1) + j];
767 for(; l<d+1; l++)
768 for(j=0; j<d+1; j++)
769 A[l*(d+1)+j] = points[pp[index]*(d+1)+j];
770 }
771 free(pp);
772 }
773 if(horizon_size1>0)
774 free(horizon);
775 free(f0);
776 free(nonvisible_faces);
777 free(visible);
778 free(hVec);
779 free(hVec_mem_face);
780 }
781 if(FUCKED){
782 break;
783 }
784 }
785
786 /* output */
787 if(FUCKED){
788 (*out_faces) = NULL;
789 if(out_cf!=NULL)
790 (*out_cf) = NULL;
791 if(out_df!=NULL)
792 (*out_df) = NULL;
793 (*nOut_faces) = 0;
794 }
795 else{
796 (*out_faces) = (int*)malloc(nFaces*d*sizeof(int));
797 memcpy((*out_faces),faces, nFaces*d*sizeof(int));
798 (*nOut_faces) = nFaces;
799 if(out_cf!=NULL){
800 (*out_cf) = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
801 memcpy((*out_cf),cf, nFaces*d*sizeof(CH_FLOAT));
802 }
803 if(out_df!=NULL){
804 (*out_df) = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
805 memcpy((*out_df),df, nFaces*sizeof(CH_FLOAT));
806 }
807 }
808
809 /* clean-up */
810 free(visible_ind);
811 free(points_cf);
812 free(points_s);
813 free(face_s);
814 free(gVec);
815 free(meanp);
816 free(absdist);
817 free(reldist);
818 free(desReldist);
819 free(ind);
820 free(span);
821 free(points);
822 free(faces);
823 free(aVec);
824 free(cf);
825 free(cfi);
826 free(df);
827 free(p_s);
828 free(fVec);
829 free(asfVec);
830 free(bVec);
831 free(A);
832}
833
834/* A C version of the ND quickhull matlab implementation from here:
835 * https://www.mathworks.com/matlabcentral/fileexchange/48509-computational-geometry-toolbox?focused=3851550&tab=example
836 * (*out_faces) is returned as NULL, if triangulation fails *
837 * Original Copyright (c) 2014, George Papazafeiropoulos
838 * Distributed under the BSD (2-clause) license
839 * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, David P. Dobkin
840 * and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, July 30, 1993"
841 */
842void convhull_nd_build
843(
844 CH_FLOAT* const in_vertices,
845 const int nVert,
846 const int d,
847 int** out_faces,
848 CH_FLOAT** out_cf,
849 CH_FLOAT** out_df,
850 int* nOut_faces
851)
852{
853 int i, j, k, l, h;
854 int nFaces, p;
855 int* aVec, *faces;
856 CH_FLOAT dfi, v, max_p, min_p;
857 CH_FLOAT* points, *cf, *cfi, *df, *p_s, *span;
858
859 assert(d<=CONVHULL_ND_MAX_DIMENSIONS);
860
861 /* Solution not possible... */
862 if(nVert<=d || in_vertices==NULL){
863 (*out_faces) = NULL;
864 (*nOut_faces) = 0;
865 if(out_cf!=NULL)
866 (*out_cf) = NULL;
867 if(out_df!=NULL)
868 (*out_df) = NULL;
869 return;
870 }
871
872 /* Add noise to the points */
873 points = (CH_FLOAT*)malloc(nVert*(d+1)*sizeof(CH_FLOAT));
874 for(i=0; i<nVert; i++){
875 for(j=0; j<d; j++)
876 points[i*(d+1)+j] = in_vertices[i*d+j] + CH_NOISE_VAL*(CH_FLOAT)rand()/(CH_FLOAT)RAND_MAX;
877 points[i*(d+1)+d] = 1.0; /* add a last column of ones. Used only for determinant calculation */
878 }
879
880 /* Find the span */
881 span = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
882 for(j=0; j<d; j++){
883 max_p = -2.23e+13; min_p = 2.23e+13;
884 for(i=0; i<nVert; i++){
885 max_p = MAX(max_p, points[i*(d+1)+j]);
886 min_p = MIN(min_p, points[i*(d+1)+j]);
887 }
888 span[j] = max_p - min_p;
889 assert(span[j]>0.000000001);
890 }
891
892 /* The initial convex hull is a simplex with (d+1) facets, where d is the number of dimensions */
893 nFaces = (d+1);
894 faces = (int*)calloc(nFaces*d, sizeof(int));
895 aVec = (int*)malloc(nFaces*sizeof(int));
896 for(i=0; i<nFaces; i++)
897 aVec[i] = i;
898
899 /* Each column of cf contains the coefficients of a plane */
900 cf = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
901 cfi = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
902 df = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
903 p_s = (CH_FLOAT*)malloc(d*d*sizeof(CH_FLOAT));
904 for(i=0; i<nFaces; i++){
905 /* Set the indices of the points defining the face */
906 for(j=0, k=0; j<(d+1); j++){
907 if(aVec[j]!=i){
908 faces[i*d+k] = aVec[j];
909 k++;
910 }
911 }
912
913 /* Calculate and store the plane coefficients of the face */
914 for(j=0; j<d; j++)
915 for(k=0; k<d; k++)
916 p_s[j*d+k] = points[(faces[i*d+j])*(d+1) + k];
917
918 /* Calculate and store the plane coefficients of the face */
919 plane_nd(d, p_s, cfi, &dfi);
920 for(j=0; j<d; j++)
921 cf[i*d+j] = cfi[j];
922 df[i] = dfi;
923 }
924 CH_FLOAT *A;
925 int *bVec, *fVec;
926 int face_tmp[2];
927
928 /* Check to make sure that faces are correctly oriented */
929 bVec = (int*)malloc((d+1)*sizeof(int));
930 for(i=0; i<d+1; i++)
931 bVec[i] = i;
932
933 /* A contains the coordinates of the points forming a simplex */
934 A = (CH_FLOAT*)calloc((d+1)*(d+1), sizeof(CH_FLOAT));
935 fVec = (int*)malloc((d+1)*sizeof(int));
936 for(k=0; k<(d+1); k++){
937 /* Get the point that is not on the current face (point p) */
938 for(i=0; i<d; i++)
939 fVec[i] = faces[k*d+i];
940 sort_int(fVec, NULL, NULL, d, 0); /* sort accending */
941 p=k;
942 for(i=0; i<d; i++)
943 for(j=0; j<(d+1); j++)
944 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
945 for(; i<(d+1); i++)
946 for(j=0; j<(d+1); j++)
947 A[i*(d+1)+j] = points[p*(d+1)+j];
948
949 /* det(A) determines the orientation of the face */
950 if(d==3)
951 v = det_4x4(A);
952 else
953 v = det_NxN(A, d+1);
954
955 /* Orient so that each point on the original simplex can't see the opposite face */
956 if(v<0){
957 /* Reverse the order of the last two vertices to change the volume */
958 for(j=0; j<2; j++)
959 face_tmp[j] = faces[k*d+d-j-1];
960 for(j=0; j<2; j++)
961 faces[k*d+d-j-1] = face_tmp[1-j];
962
963 /* Modify the plane coefficients of the properly oriented faces */
964 for(j=0; j<d; j++)
965 cf[k*d+j] = -cf[k*d+j];
966 df[k] = -df[k];
967 for(i=0; i<d; i++)
968 for(j=0; j<(d+1); j++)
969 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
970 for(; i<(d+1); i++)
971 for(j=0; j<(d+1); j++)
972 A[i*(d+1)+j] = points[p*(d+1)+j];
973 }
974 }
975
976 /* Coordinates of the center of the point set */
977 CH_FLOAT* meanp, *reldist, *desReldist, *absdist;
978 meanp = (CH_FLOAT*)calloc(d, sizeof(CH_FLOAT));
979 for(i=d+1; i<nVert; i++)
980 for(j=0; j<d; j++)
981 meanp[j] += points[i*(d+1)+j];
982 for(j=0; j<d; j++)
983 meanp[j] = meanp[j]/(CH_FLOAT)(nVert-d-1);
984
985 /* Absolute distance of points from the center */
986 absdist = (CH_FLOAT*)malloc((nVert-d-1)*d * sizeof(CH_FLOAT));
987 for(i=d+1, k=0; i<nVert; i++, k++)
988 for(j=0; j<d; j++)
989 absdist[k*d+j] = (points[i*(d+1)+j] - meanp[j])/span[j];
990
991 /* Relative distance of points from the center */
992 reldist = (CH_FLOAT*)calloc((nVert-d-1), sizeof(CH_FLOAT));
993 desReldist = (CH_FLOAT*)malloc((nVert-d-1) * sizeof(CH_FLOAT));
994 for(i=0; i<(nVert-d-1); i++)
995 for(j=0; j<d; j++)
996 reldist[i] += pow(absdist[i*d+j], 2.0);
997
998 /* Sort from maximum to minimum relative distance */
999 int num_pleft;
1000 int* ind, *pleft;
1001 ind = (int*)malloc((nVert-d-1) * sizeof(int));
1002 pleft = (int*)malloc((nVert-d-1) * sizeof(int));
1003 sort_float(reldist, desReldist, ind, (nVert-d-1), 1);
1004
1005 /* Initialize the vector of points left. The points with the larger relative
1006 distance from the center are scanned first. */
1007 num_pleft = (nVert-d-1);
1008 for(i=0; i<num_pleft; i++)
1009 pleft[i] = ind[i]+d+1;
1010
1011 /* Loop over all remaining points that are not deleted. Deletion of points
1012 occurs every #iter2del# iterations of this while loop */
1013 memset(A, 0, (d+1)*(d+1) * sizeof(CH_FLOAT));
1014
1015 /* The main loop for the quickhull algorithm */
1016 CH_FLOAT detA;
1017 CH_FLOAT* points_cf, *points_s;
1018 int* visible_ind, *visible, *nonvisible_faces, *f0, *face_s, *u, *gVec, *horizon, *hVec, *pp, *hVec_mem_face;
1019 int num_visible_ind, num_nonvisible_faces, n_newfaces, count, vis;
1020 int f0_sum, u_len, start, num_p, index, horizon_size1;
1021 int FUCKED;
1022 FUCKED = 0;
1023 u = horizon = NULL;
1024 nFaces = d+1;
1025 visible_ind = (int*)malloc(nFaces*sizeof(int));
1026 points_cf = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
1027 points_s = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
1028 face_s = (int*)malloc(d*sizeof(int));
1029 gVec = (int*)malloc(d*sizeof(int));
1030 while( (num_pleft>0) ){
1031 /* i is the first point of the points left */
1032 i = pleft[0];
1033
1034 /* Delete the point selected */
1035 for(j=0; j<num_pleft-1; j++)
1036 pleft[j] = pleft[j+1];
1037 num_pleft--;
1038 if(num_pleft == 0)
1039 free(pleft);
1040 else
1041 pleft = (int*)realloc(pleft, num_pleft*sizeof(int));
1042
1043 /* find visible faces */
1044 for(j=0; j<d; j++)
1045 points_s[j] = points[i*(d+1)+j];
1046 points_cf = (CH_FLOAT*)realloc(points_cf,nFaces*sizeof(CH_FLOAT));
1047 visible_ind = (int*)realloc(visible_ind, nFaces*sizeof(int));
1048#ifdef CONVHULL_3D_USE_CBLAS
1049 #ifdef CONVHULL_3D_USE_FLOAT_PRECISION
1050 cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f,
1051 points_s, d,
1052 cf, d, 0.0f,
1053 points_cf, nFaces);
1054 #else
1055 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0,
1056 points_s, d,
1057 cf, d, 0.0,
1058 points_cf, nFaces);
1059 #endif
1060#else
1061 for (j = 0; j < nFaces; j++) {
1062 points_cf[j] = 0;
1063 for (k = 0; k < d; k++)
1064 points_cf[j] += points_s[k]*cf[j*d+k];
1065 }
1066#endif
1067 num_visible_ind = 0;
1068 for(j=0; j<nFaces; j++){
1069 if(points_cf[j] + df[j] > 0.0){
1070 num_visible_ind++; /* will sum to 0 if none are visible */
1071 visible_ind[j] = 1;
1072 }
1073 else
1074 visible_ind[j] = 0;
1075 }
1076 num_nonvisible_faces = nFaces - num_visible_ind;
1077
1078 /* proceed if there are any visible faces */
1079 if(num_visible_ind!=0){
1080 /* Find visible face indices */
1081 visible = (int*)malloc(num_visible_ind*sizeof(int));
1082 for(j=0, k=0; j<nFaces; j++){
1083 if(visible_ind[j]==1){
1084 visible[k]=j;
1085 k++;
1086 }
1087 }
1088
1089 /* Find nonvisible faces */
1090 nonvisible_faces = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
1091 f0 = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
1092 for(j=0, k=0; j<nFaces; j++){
1093 if(visible_ind[j]==0){
1094 for(l=0; l<d; l++)
1095 nonvisible_faces[k*d+l] = faces[j*d+l];
1096 k++;
1097 }
1098 }
1099
1100 /* Create horizon (count is the number of the edges of the horizon) */
1101 count=0;
1102 for(j=0; j<num_visible_ind; j++){
1103 /* visible face */
1104 vis = visible[j];
1105 for(k=0; k<d; k++)
1106 face_s[k] = faces[vis*d+k];
1107 sort_int(face_s, NULL, NULL, d, 0);
1108 ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces*d, d);
1109 u_len = 0;
1110
1111 /* u are the nonvisible faces connected to the face v, if any */
1112 for(k=0; k<num_nonvisible_faces; k++){
1113 f0_sum = 0;
1114 for(l=0; l<d; l++)
1115 f0_sum += f0[k*d + l];
1116 if(f0_sum == d-1){
1117 u_len++;
1118 if(u_len==1)
1119 u = (int*)malloc(u_len*sizeof(int));
1120 else
1121 u = (int*)realloc(u, u_len*sizeof(int));
1122 u[u_len-1] = k;
1123 }
1124 }
1125 for(k=0; k<u_len; k++){
1126 /* The boundary between the visible face v and the k(th) nonvisible face connected to the face v forms part of the horizon */
1127 count++;
1128 if(count==1)
1129 horizon = (int*)malloc(count*(d-1)*sizeof(int));
1130 else
1131 horizon = (int*)realloc(horizon, count*(d-1)*sizeof(int));
1132 for(l=0; l<d; l++)
1133 gVec[l] = nonvisible_faces[u[k]*d+l];
1134 for(l=0, h=0; l<d; l++){
1135 if(f0[u[k]*d+l]){
1136 horizon[(count-1)*(d-1)+h] = gVec[l];
1137 h++;
1138 }
1139 }
1140 }
1141 if(u_len!=0)
1142 free(u);
1143 }
1144 horizon_size1 = count;
1145 for(j=0, l=0; j<nFaces; j++){
1146 if(!visible_ind[j]){
1147 /* Delete visible faces */
1148 for(k=0; k<d; k++)
1149 faces[l*d+k] = faces[j*d+k];
1150
1151 /* Delete the corresponding plane coefficients of the faces */
1152 for(k=0; k<d; k++)
1153 cf[l*d+k] = cf[j*d+k];
1154 df[l] = df[j];
1155 l++;
1156 }
1157 }
1158
1159 /* Update the number of faces */
1160 nFaces = nFaces-num_visible_ind;
1161 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
1162 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
1163 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
1164
1165 /* start is the first row of the new faces */
1166 start=nFaces;
1167
1168 /* Add faces connecting horizon to the new point */
1169 n_newfaces = horizon_size1;
1170 for(j=0; j<n_newfaces; j++){
1171 nFaces++;
1172 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
1173 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
1174 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
1175 for(k=0; k<d-1; k++)
1176 faces[(nFaces-1)*d+k] = horizon[j*(d-1)+k];
1177 faces[(nFaces-1)*d+(d-1)] = i;
1178
1179 /* Calculate and store appropriately the plane coefficients of the faces */
1180 for(k=0; k<d; k++)
1181 for(l=0; l<d; l++)
1182 p_s[k*d+l] = points[(faces[(nFaces-1)*d+k])*(d+1) + l];
1183 plane_nd(d, p_s, cfi, &dfi);
1184 for(k=0; k<d; k++)
1185 cf[(nFaces-1)*d+k] = cfi[k];
1186 df[(nFaces-1)] = dfi;
1187 if(nFaces > CH_MAX_NUM_FACES){
1188 FUCKED = 1;
1189 nFaces = 0;
1190 break;
1191 }
1192 }
1193
1194 /* Orient each new face properly */
1195 hVec = (int*)malloc( nFaces*sizeof(int));
1196 hVec_mem_face = (int*)malloc( nFaces*sizeof(int));
1197 for(j=0; j<nFaces; j++)
1198 hVec[j] = j;
1199 for(k=start; k<nFaces; k++){
1200 for(j=0; j<d; j++)
1201 face_s[j] = faces[k*d+j];
1202 sort_int(face_s, NULL, NULL, d, 0);
1203 ismember(hVec, face_s, hVec_mem_face, nFaces, d);
1204 num_p = 0;
1205 for(j=0; j<nFaces; j++)
1206 if(!hVec_mem_face[j])
1207 num_p++;
1208 pp = (int*)malloc(num_p*sizeof(int));
1209 for(j=0, l=0; j<nFaces; j++){
1210 if(!hVec_mem_face[j]){
1211 pp[l] = hVec[j];
1212 l++;
1213 }
1214 }
1215 index = 0;
1216 detA = 0.0;
1217
1218 /* While new point is coplanar, choose another point */
1219 while(detA==0.0){
1220 for(j=0;j<d; j++)
1221 for(l=0; l<d+1; l++)
1222 A[j*(d+1)+l] = points[(faces[k*d+j])*(d+1) + l];
1223 for(; j<d+1; j++)
1224 for(l=0; l<d+1; l++)
1225 A[j*(d+1)+l] = points[pp[index]*(d+1)+l];
1226 index++;
1227 if(d==3)
1228 detA = det_4x4(A);
1229 else
1230 detA = det_NxN((CH_FLOAT*)A, d+1);
1231 }
1232
1233 /* Orient faces so that each point on the original simplex can't see the opposite face */
1234 if (detA<0.0){
1235 /* If orientation is improper, reverse the order to change the volume sign */
1236 for(j=0; j<2; j++)
1237 face_tmp[j] = faces[k*d+d-j-1];
1238 for(j=0; j<2; j++)
1239 faces[k*d+d-j-1] = face_tmp[1-j];
1240
1241 /* Modify the plane coefficients of the properly oriented faces */
1242 for(j=0; j<d; j++)
1243 cf[k*d+j] = -cf[k*d+j];
1244 df[k] = -df[k];
1245 for(l=0; l<d; l++)
1246 for(j=0; j<d+1; j++)
1247 A[l*(d+1)+j] = points[(faces[k*d+l])*(d+1) + j];
1248 for(; l<d+1; l++)
1249 for(j=0; j<d+1; j++)
1250 A[l*(d+1)+j] = points[pp[index]*(d+1)+j];
1251#ifndef NDEBUG
1252 /* Check */
1253 if(d==3)
1254 detA = det_4x4(A);
1255 else
1256 detA = det_NxN((CH_FLOAT*)A, d+1);
1257 assert(detA>0.0);
1258#endif
1259 }
1260 free(pp);
1261 }
1262 if(horizon_size1>0)
1263 free(horizon);
1264 free(f0);
1265 free(nonvisible_faces);
1266 free(visible);
1267 free(hVec);
1268 free(hVec_mem_face);
1269 }
1270 if(FUCKED){
1271 break;
1272 }
1273 }
1274
1275 /* output */
1276 if(FUCKED){
1277 (*out_faces) = NULL;
1278 if(out_cf!=NULL)
1279 (*out_cf) = NULL;
1280 if(out_df!=NULL)
1281 (*out_df) = NULL;
1282 (*nOut_faces) = 0;
1283 }
1284 else{
1285 (*out_faces) = (int*)malloc(nFaces*d*sizeof(int));
1286 memcpy((*out_faces),faces, nFaces*d*sizeof(int));
1287 (*nOut_faces) = nFaces;
1288 if(out_cf!=NULL){
1289 (*out_cf) = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
1290 memcpy((*out_cf), cf, nFaces*d*sizeof(CH_FLOAT));
1291 }
1292 if(out_df!=NULL){
1293 (*out_df) = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
1294 memcpy((*out_df), df, nFaces*sizeof(CH_FLOAT));
1295 }
1296 }
1297
1298 /* clean-up */
1299 free(visible_ind);
1300 free(points_cf);
1301 free(points_s);
1302 free(face_s);
1303 free(gVec);
1304 free(meanp);
1305 free(absdist);
1306 free(reldist);
1307 free(desReldist);
1308 free(ind);
1309 free(span);
1310 free(points);
1311 free(faces);
1312 free(aVec);
1313 free(cf);
1314 free(cfi);
1315 free(df);
1316 free(p_s);
1317 free(fVec);
1318 free(bVec);
1319 free(A);
1320}
1321
1322void convhull_3d_export_obj
1323(
1324 ch_vertex* const vertices,
1325 const int nVert,
1326 int* const faces,
1327 const int nFaces,
1328 const int keepOnlyUsedVerticesFLAG,
1329 char* const obj_filename
1330)
1331{
1332 int i, j;
1333 char path[256] = "\0";
1334 memcpy(path, obj_filename, strlen(obj_filename));
1335 FILE* obj_file;
1336#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1337 CV_STRCAT(path, ".obj");
1338 fopen_s(&obj_file, path, "wt");
1339#else
1340 obj_file = fopen(strcat(path, ".obj"), "wt");
1341#endif
1342 fprintf(obj_file, "o\n");
1343 CH_FLOAT scale;
1344 ch_vec3 v1, v2, normal;
1345
1346 /* export vertices */
1347 if(keepOnlyUsedVerticesFLAG){
1348 for (i = 0; i < nFaces; i++)
1349 for(j=0; j<3; j++)
1350 fprintf(obj_file, "v %f %f %f\n", vertices[faces[i*3+j]].x,
1351 vertices[faces[i*3+j]].y, vertices[faces[i*3+j]].z);
1352 }
1353 else {
1354 for (i = 0; i < nVert; i++)
1355 fprintf(obj_file, "v %f %f %f\n", vertices[i].x,
1356 vertices[i].y, vertices[i].z);
1357 }
1358
1359 /* export the face normals */
1360 for (i = 0; i < nFaces; i++){
1361 /* calculate cross product between v1-v0 and v2-v0 */
1362 v1 = vertices[faces[i*3+1]];
1363 v2 = vertices[faces[i*3+2]];
1364 v1.x -= vertices[faces[i*3]].x;
1365 v1.y -= vertices[faces[i*3]].y;
1366 v1.z -= vertices[faces[i*3]].z;
1367 v2.x -= vertices[faces[i*3]].x;
1368 v2.y -= vertices[faces[i*3]].y;
1369 v2.z -= vertices[faces[i*3]].z;
1370 normal = cross(&v1, &v2);
1371
1372 /* normalise to unit length */
1373 scale = 1.0/(sqrt(pow(normal.x, 2.0)+pow(normal.y, 2.0)+pow(normal.z, 2.0))+2.23e-9);
1374 normal.x *= scale;
1375 normal.y *= scale;
1376 normal.z *= scale;
1377 fprintf(obj_file, "vn %f %f %f\n", normal.x, normal.y, normal.z);
1378 }
1379
1380 /* export the face indices */
1381 if(keepOnlyUsedVerticesFLAG){
1382 for (i = 0; i < nFaces; i++){
1383 /* vertices are in same order as the faces, and normals are in order */
1384 fprintf(obj_file, "f %u//%u %u//%u %u//%u\n",
1385 i*3 + 1, i + 1,
1386 i*3+1 + 1, i + 1,
1387 i*3+2 + 1, i + 1);
1388 }
1389 }
1390 else {
1391 /* just normals are in order */
1392 for (i = 0; i < nFaces; i++){
1393 fprintf(obj_file, "f %u//%u %u//%u %u//%u\n",
1394 faces[i*3] + 1, i + 1,
1395 faces[i*3+1] + 1, i + 1,
1396 faces[i*3+2] + 1, i + 1);
1397 }
1398 }
1399 fclose(obj_file);
1400}
1401
1402void convhull_3d_export_m
1403(
1404 ch_vertex* const vertices,
1405 const int nVert,
1406 int* const faces,
1407 const int nFaces,
1408 char* const m_filename
1409)
1410{
1411 int i;
1412 char path[256] = { "\0" };
1413 memcpy(path, m_filename, strlen(m_filename));
1414 FILE* m_file;
1415#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1416 CV_STRCAT(path, ".m");
1417 fopen_s(&m_file, path, "wt");
1418#else
1419 m_file = fopen(strcat(path, ".m"), "wt");
1420#endif
1421
1422 /* save face indices and vertices for verification in matlab: */
1423 fprintf(m_file, "vertices = [\n");
1424 for (i = 0; i < nVert; i++)
1425 fprintf(m_file, "%f, %f, %f;\n", vertices[i].x, vertices[i].y, vertices[i].z);
1426 fprintf(m_file, "];\n\n\n");
1427 fprintf(m_file, "faces = [\n");
1428 for (i = 0; i < nFaces; i++) {
1429 fprintf(m_file, " %u, %u, %u;\n",
1430 faces[3*i+0]+1,
1431 faces[3*i+1]+1,
1432 faces[3*i+2]+1);
1433 }
1434 fprintf(m_file, "];\n\n\n");
1435 fclose(m_file);
1436}
1437
1438void extractVerticesFromObjFile(char* const obj_filename, ch_vertex** out_vertices, int* out_nVert)
1439{
1440 FILE* obj_file;
1441#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1442 CV_STRCAT(obj_filename, ".obj");
1443 fopen_s(&obj_file, obj_filename, "r");
1444#else
1445 obj_file = fopen(strcat(obj_filename, ".obj"), "r");
1446#endif
1447
1448 /* determine number of vertices */
1449 unsigned int nVert = 0;
1450 char line[256];
1451 while (fgets(line, sizeof(line), obj_file)) {
1452 char* vexists = strstr(line, "v ");
1453 if(vexists!=NULL)
1454 nVert++;
1455 }
1456 (*out_nVert) = nVert;
1457 (*out_vertices) = (ch_vertex*)malloc(nVert*sizeof(ch_vertex));
1458
1459 /* extract the vertices */
1460 rewind(obj_file);
1461 int i=0;
1462 int vertID, prev_char_isDigit, current_char_isDigit;
1463 char vert_char[256] = { 0 };
1464 while (fgets(line, sizeof(line), obj_file)) {
1465 char* vexists = strstr(line, "v ");
1466 if(vexists!=NULL){
1467 prev_char_isDigit = 0;
1468 vertID = -1;
1469 for(int j=0; j<(int)strlen(line)-1; j++){
1470 if(isdigit(line[j])||line[j]=='.'||line[j]=='-'||line[j]=='+'||line[j]=='E'||line[j]=='e'){
1471 vert_char[strlen(vert_char)] = line[j];
1472 current_char_isDigit = 1;
1473 }
1474 else
1475 current_char_isDigit = 0;
1476 if((prev_char_isDigit && !current_char_isDigit) || j ==(int)strlen(line)-2 ){
1477 vertID++;
1478 if(vertID>4){
1479 /* not a valid file */
1480 free((*out_vertices));
1481 (*out_vertices) = NULL;
1482 (*out_nVert) = 0;
1483 return;
1484 }
1485 (*out_vertices)[i].v[vertID] = atof(vert_char);
1486 memset(vert_char, 0, 256 * sizeof(char));
1487 }
1488 prev_char_isDigit = current_char_isDigit;
1489 }
1490 i++;
1491 }
1492 }
1493}
convhull_3d_build
void convhull_3d_build(ch_vertex *const in_vertices, const int nVert, int **out_faces, CH_FLOAT **out_cf, CH_FLOAT **out_df, int *nOut_faces)
Builds the 3-D convexhull using the quickhull algorithm [1].
Definition convhull_3d.c:368
convhull_3d_export_m
void convhull_3d_export_m(ch_vertex *const vertices, const int nVert, int *const faces, const int nFaces, char *const m_filename)
Exports the vertices, face indices, and face normals, as an '.m' file, for Matlab verification.
Definition convhull_3d.c:1403
extractVerticesFromObjFile
void extractVerticesFromObjFile(char *const obj_filename, ch_vertex **out_vertices, int *out_nVert)
Reads an '.obj' file and extracts only the vertices.
Definition convhull_3d.c:1438
convhull_3d_export_obj
void convhull_3d_export_obj(ch_vertex *const vertices, const int nVert, int *const faces, const int nFaces, const int keepOnlyUsedVerticesFLAG, char *const obj_filename)
Exports the vertices, face indices, and face normals, as an '.obj' file, ready for the GPU.
Definition convhull_3d.c:1323
convhull_nd_build
void convhull_nd_build(CH_FLOAT *const in_vertices, const int nVert, const int d, int **out_faces, CH_FLOAT **out_cf, CH_FLOAT **out_df, int *nOut_faces)
Builds the N-D convexhull using the quickhull algorithm [1].
Definition convhull_3d.c:843
convhull_3d.h
An implementation of the 3-D quickhull algorithm [1].
utility_ddet
double utility_ddet(void *const hWork, double *A, int N)
Determinant of a Matrix, double precision, i,e.
Definition saf_utility_veclib.c:4411
utility_sdet
float utility_sdet(void *const hWork, float *A, int N)
Determinant of a Matrix, single precision, i,e.
Definition saf_utility_veclib.c:4291
saf_externals.h
Include header for SAF externals.
start
static tick_t start
Start time for whole test program.
Definition saf_test.c:54
cmp_desc_float
static int cmp_desc_float(const void *a, const void *b)
Helper function for a sorting vector of floats using 'qsort' in decending order.
Definition saf_utility_sort.c:90
cmp_asc_int
static int cmp_asc_int(const void *a, const void *b)
Helper function for sorting a vector of integers using 'qsort' in ascending order.
Definition saf_utility_sort.c:54
cmp_desc_int
static int cmp_desc_int(const void *a, const void *b)
Helper function for a sorting vector of integers using 'qsort' in decending order.
Definition saf_utility_sort.c:66
cmp_asc_float
static int cmp_asc_float(const void *a, const void *b)
Helper function for a sorting vector of floats using 'qsort' in ascending order.
Definition saf_utility_sort.c:78
ch_vertex
vertex structure, used by convhull_3d
Definition convhull_3d.h:62
float_w_idx
Helper struct for qsort in convhull_3d_build()
Definition convhull_3d.c:86
int_w_idx
Helper struct for qsort in convhull_3d_build()
Definition convhull_3d.c:92