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
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, cnt;
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 /* cnt is equal to the points having been selected without deletion of
538 nonvisible points (i.e. points inside the current convex hull) */
539 cnt=0;
540
541 /* The main loop for the quickhull algorithm */
542 CH_FLOAT detA;
543 CH_FLOAT* points_cf, *points_s;
544 int* visible_ind, *visible, *nonvisible_faces, *f0, *face_s, *u, *gVec, *horizon, *hVec, *pp, *hVec_mem_face;
545 int num_visible_ind, num_nonvisible_faces, n_newfaces, count, vis;
546 int f0_sum, u_len, start, num_p, index, horizon_size1;
547 int FUCKED;
548 FUCKED = 0;
549 u = horizon = NULL;
550 nFaces = d+1;
551 visible_ind = (int*)malloc(nFaces*sizeof(int));
552 points_cf = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
553 points_s = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
554 face_s = (int*)malloc(d*sizeof(int));
555 gVec = (int*)malloc(d*sizeof(int));
556 while( (num_pleft>0) ){
557 /* i is the first point of the points left */
558 i = pleft[0];
559
560 /* Delete the point selected */
561 for(j=0; j<num_pleft-1; j++)
562 pleft[j] = pleft[j+1];
563 num_pleft--;
564 if(num_pleft == 0)
565 free(pleft);
566 else
567 pleft = (int*)realloc(pleft, num_pleft*sizeof(int));
568
569 /* Update point selection counter */
570 cnt++;
571
572 /* find visible faces */
573 for(j=0; j<d; j++)
574 points_s[j] = points[i*(d+1)+j];
575 points_cf = (CH_FLOAT*)realloc(points_cf, nFaces*sizeof(CH_FLOAT));
576 visible_ind = (int*)realloc(visible_ind, nFaces*sizeof(int));
577#ifdef CONVHULL_3D_USE_CBLAS
578 #ifdef CONVHULL_3D_USE_FLOAT_PRECISION
579 cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f,
580 points_s, d,
581 cf, d, 0.0f,
582 points_cf, nFaces);
583 #else
584 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0,
585 points_s, d,
586 cf, d, 0.0,
587 points_cf, nFaces);
588 #endif
589#else
590 for (j = 0; j < nFaces; j++) {
591 points_cf[j] = 0;
592 for (k = 0; k < d; k++)
593 points_cf[j] += points_s[k]*cf[j*d+k];
594 }
595#endif
596 num_visible_ind = 0;
597 for(j=0; j<nFaces; j++){
598 if(points_cf[j] + df[j] > 0.0){
599 num_visible_ind++; /* will sum to 0 if none are visible */
600 visible_ind[j] = 1;
601 }
602 else
603 visible_ind[j] = 0;
604 }
605 num_nonvisible_faces = nFaces - num_visible_ind;
606
607 /* proceed if there are any visible faces */
608 if(num_visible_ind!=0){
609 /* Find visible face indices */
610 visible = (int*)malloc(num_visible_ind*sizeof(int));
611 for(j=0, k=0; j<nFaces; j++){
612 if(visible_ind[j]==1){
613 visible[k]=j;
614 k++;
615 }
616 }
617
618 /* Find nonvisible faces */
619 nonvisible_faces = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
620 f0 = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
621 for(j=0, k=0; j<nFaces; j++){
622 if(visible_ind[j]==0){
623 for(l=0; l<d; l++)
624 nonvisible_faces[k*d+l]= faces[j*d+l];
625 k++;
626 }
627 }
628
629 /* Create horizon (count is the number of the edges of the horizon) */
630 count=0;
631 for(j=0; j<num_visible_ind; j++){
632 /* visible face */
633 vis = visible[j];
634 for(k=0; k<d; k++)
635 face_s[k] = faces[vis*d+k];
636 sort_int(face_s, NULL, NULL, d, 0);
637 ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces*d, d);
638 u_len = 0;
639
640 /* u are the nonvisible faces connected to the face v, if any */
641 for(k=0; k<num_nonvisible_faces; k++){
642 f0_sum = 0;
643 for(l=0; l<d; l++)
644 f0_sum += f0[k*d + l];
645 if(f0_sum == d-1){
646 u_len++;
647 if(u_len==1)
648 u = (int*)malloc(u_len*sizeof(int));
649 else
650 u = (int*)realloc(u, u_len*sizeof(int));
651 u[u_len-1] = k;
652 }
653 }
654 for(k=0; k<u_len; k++){
655 /* The boundary between the visible face v and the k(th) nonvisible face connected to the face v forms part of the horizon */
656 count++;
657 if(count==1)
658 horizon = (int*)malloc(count*(d-1)*sizeof(int));
659 else
660 horizon = (int*)realloc(horizon, count*(d-1)*sizeof(int));
661 for(l=0; l<d; l++)
662 gVec[l] = nonvisible_faces[u[k]*d+l];
663 for(l=0, h=0; l<d; l++){
664 if(f0[u[k]*d+l]){
665 horizon[(count-1)*(d-1)+h] = gVec[l];
666 h++;
667 }
668 }
669 }
670 if(u_len!=0)
671 free(u);
672 }
673 horizon_size1 = count;
674 for(j=0, l=0; j<nFaces; j++){
675 if(!visible_ind[j]){
676 /* Delete visible faces */
677 for(k=0; k<d; k++)
678 faces[l*d+k] = faces[j*d+k];
679
680 /* Delete the corresponding plane coefficients of the faces */
681 for(k=0; k<d; k++)
682 cf[l*d+k] = cf[j*d+k];
683 df[l] = df[j];
684 l++;
685 }
686 }
687
688 /* Update the number of faces */
689 nFaces = nFaces-num_visible_ind;
690 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
691 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
692 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
693
694 /* start is the first row of the new faces */
695 start=nFaces;
696
697 /* Add faces connecting horizon to the new point */
698 n_newfaces = horizon_size1;
699 for(j=0; j<n_newfaces; j++){
700 nFaces++;
701 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
702 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
703 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
704 for(k=0; k<d-1; k++)
705 faces[(nFaces-1)*d+k] = horizon[j*(d-1)+k];
706 faces[(nFaces-1)*d+(d-1)] = i;
707
708 /* Calculate and store appropriately the plane coefficients of the faces */
709 for(k=0; k<d; k++)
710 for(l=0; l<d; l++)
711 p_s[k*d+l] = points[(faces[(nFaces-1)*d+k])*(d+1) + l];
712 plane_3d(p_s, cfi, &dfi);
713 for(k=0; k<d; k++)
714 cf[(nFaces-1)*d+k] = cfi[k];
715 df[(nFaces-1)] = dfi;
716 if(nFaces > CH_MAX_NUM_FACES){
717 FUCKED = 1;
718 nFaces = 0;
719 break;
720 }
721 }
722
723 /* Orient each new face properly */
724 hVec = (int*)malloc( nFaces*sizeof(int));
725 hVec_mem_face = (int*)malloc( nFaces*sizeof(int));
726 for(j=0; j<nFaces; j++)
727 hVec[j] = j;
728 for(k=start; k<nFaces; k++){
729 for(j=0; j<d; j++)
730 face_s[j] = faces[k*d+j];
731 sort_int(face_s, NULL, NULL, d, 0);
732 ismember(hVec, face_s, hVec_mem_face, nFaces, d);
733 num_p = 0;
734 for(j=0; j<nFaces; j++)
735 if(!hVec_mem_face[j])
736 num_p++;
737 pp = (int*)malloc(num_p*sizeof(int));
738 for(j=0, l=0; j<nFaces; j++){
739 if(!hVec_mem_face[j]){
740 pp[l] = hVec[j];
741 l++;
742 }
743 }
744 index = 0;
745 detA = 0.0;
746
747 /* While new point is coplanar, choose another point */
748 while(detA==0.0){
749 for(j=0;j<d; j++)
750 for(l=0; l<d+1; l++)
751 A[j*(d+1)+l] = points[(faces[k*d+j])*(d+1) + l];
752 for(; j<d+1; j++)
753 for(l=0; l<d+1; l++)
754 A[j*(d+1)+l] = points[pp[index]*(d+1)+l];
755 index++;
756 detA = det_4x4(A);
757 }
758
759 /* Orient faces so that each point on the original simplex can't see the opposite face */
760 if (detA<0.0){
761 /* If orientation is improper, reverse the order to change the volume sign */
762 for(j=0; j<2; j++)
763 face_tmp[j] = faces[k*d+d-j-1];
764 for(j=0; j<2; j++)
765 faces[k*d+d-j-1] = face_tmp[1-j];
766
767 /* Modify the plane coefficients of the properly oriented faces */
768 for(j=0; j<d; j++)
769 cf[k*d+j] = -cf[k*d+j];
770 df[k] = -df[k];
771 for(l=0; l<d; l++)
772 for(j=0; j<d+1; j++)
773 A[l*(d+1)+j] = points[(faces[k*d+l])*(d+1) + j];
774 for(; l<d+1; l++)
775 for(j=0; j<d+1; j++)
776 A[l*(d+1)+j] = points[pp[index]*(d+1)+j];
777 }
778 free(pp);
779 }
780 if(horizon_size1>0)
781 free(horizon);
782 free(f0);
783 free(nonvisible_faces);
784 free(visible);
785 free(hVec);
786 free(hVec_mem_face);
787 }
788 if(FUCKED){
789 break;
790 }
791 }
792
793 /* output */
794 if(FUCKED){
795 (*out_faces) = NULL;
796 if(out_cf!=NULL)
797 (*out_cf) = NULL;
798 if(out_df!=NULL)
799 (*out_df) = NULL;
800 (*nOut_faces) = 0;
801 }
802 else{
803 (*out_faces) = (int*)malloc(nFaces*d*sizeof(int));
804 memcpy((*out_faces),faces, nFaces*d*sizeof(int));
805 (*nOut_faces) = nFaces;
806 if(out_cf!=NULL){
807 (*out_cf) = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
808 memcpy((*out_cf),cf, nFaces*d*sizeof(CH_FLOAT));
809 }
810 if(out_df!=NULL){
811 (*out_df) = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
812 memcpy((*out_df),df, nFaces*sizeof(CH_FLOAT));
813 }
814 }
815
816 /* clean-up */
817 free(visible_ind);
818 free(points_cf);
819 free(points_s);
820 free(face_s);
821 free(gVec);
822 free(meanp);
823 free(absdist);
824 free(reldist);
825 free(desReldist);
826 free(ind);
827 free(span);
828 free(points);
829 free(faces);
830 free(aVec);
831 free(cf);
832 free(cfi);
833 free(df);
834 free(p_s);
835 free(fVec);
836 free(asfVec);
837 free(bVec);
838 free(A);
839}
840
841/* A C version of the ND quickhull matlab implementation from here:
842 * https://www.mathworks.com/matlabcentral/fileexchange/48509-computational-geometry-toolbox?focused=3851550&tab=example
843 * (*out_faces) is returned as NULL, if triangulation fails *
844 * Original Copyright (c) 2014, George Papazafeiropoulos
845 * Distributed under the BSD (2-clause) license
846 * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, David P. Dobkin
847 * and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, July 30, 1993"
848 */
849void convhull_nd_build
850(
851 CH_FLOAT* const in_vertices,
852 const int nVert,
853 const int d,
854 int** out_faces,
855 CH_FLOAT** out_cf,
856 CH_FLOAT** out_df,
857 int* nOut_faces
858)
859{
860 int i, j, k, l, h;
861 int nFaces, p;
862 int* aVec, *faces;
863 CH_FLOAT dfi, v, max_p, min_p;
864 CH_FLOAT* points, *cf, *cfi, *df, *p_s, *span;
865
866 assert(d<=CONVHULL_ND_MAX_DIMENSIONS);
867
868 /* Solution not possible... */
869 if(nVert<=d || in_vertices==NULL){
870 (*out_faces) = NULL;
871 (*nOut_faces) = 0;
872 if(out_cf!=NULL)
873 (*out_cf) = NULL;
874 if(out_df!=NULL)
875 (*out_df) = NULL;
876 return;
877 }
878
879 /* Add noise to the points */
880 points = (CH_FLOAT*)malloc(nVert*(d+1)*sizeof(CH_FLOAT));
881 for(i=0; i<nVert; i++){
882 for(j=0; j<d; j++)
883 points[i*(d+1)+j] = in_vertices[i*d+j] + CH_NOISE_VAL*(CH_FLOAT)rand()/(CH_FLOAT)RAND_MAX;
884 points[i*(d+1)+d] = 1.0; /* add a last column of ones. Used only for determinant calculation */
885 }
886
887 /* Find the span */
888 span = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
889 for(j=0; j<d; j++){
890 max_p = -2.23e+13; min_p = 2.23e+13;
891 for(i=0; i<nVert; i++){
892 max_p = MAX(max_p, points[i*(d+1)+j]);
893 min_p = MIN(min_p, points[i*(d+1)+j]);
894 }
895 span[j] = max_p - min_p;
896 assert(span[j]>0.000000001);
897 }
898
899 /* The initial convex hull is a simplex with (d+1) facets, where d is the number of dimensions */
900 nFaces = (d+1);
901 faces = (int*)calloc(nFaces*d, sizeof(int));
902 aVec = (int*)malloc(nFaces*sizeof(int));
903 for(i=0; i<nFaces; i++)
904 aVec[i] = i;
905
906 /* Each column of cf contains the coefficients of a plane */
907 cf = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
908 cfi = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
909 df = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
910 p_s = (CH_FLOAT*)malloc(d*d*sizeof(CH_FLOAT));
911 for(i=0; i<nFaces; i++){
912 /* Set the indices of the points defining the face */
913 for(j=0, k=0; j<(d+1); j++){
914 if(aVec[j]!=i){
915 faces[i*d+k] = aVec[j];
916 k++;
917 }
918 }
919
920 /* Calculate and store the plane coefficients of the face */
921 for(j=0; j<d; j++)
922 for(k=0; k<d; k++)
923 p_s[j*d+k] = points[(faces[i*d+j])*(d+1) + k];
924
925 /* Calculate and store the plane coefficients of the face */
926 plane_nd(d, p_s, cfi, &dfi);
927 for(j=0; j<d; j++)
928 cf[i*d+j] = cfi[j];
929 df[i] = dfi;
930 }
931 CH_FLOAT *A;
932 int *bVec, *fVec;
933 int face_tmp[2];
934
935 /* Check to make sure that faces are correctly oriented */
936 bVec = (int*)malloc((d+1)*sizeof(int));
937 for(i=0; i<d+1; i++)
938 bVec[i] = i;
939
940 /* A contains the coordinates of the points forming a simplex */
941 A = (CH_FLOAT*)calloc((d+1)*(d+1), sizeof(CH_FLOAT));
942 fVec = (int*)malloc((d+1)*sizeof(int));
943 for(k=0; k<(d+1); k++){
944 /* Get the point that is not on the current face (point p) */
945 for(i=0; i<d; i++)
946 fVec[i] = faces[k*d+i];
947 sort_int(fVec, NULL, NULL, d, 0); /* sort accending */
948 p=k;
949 for(i=0; i<d; i++)
950 for(j=0; j<(d+1); j++)
951 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
952 for(; i<(d+1); i++)
953 for(j=0; j<(d+1); j++)
954 A[i*(d+1)+j] = points[p*(d+1)+j];
955
956 /* det(A) determines the orientation of the face */
957 if(d==3)
958 v = det_4x4(A);
959 else
960 v = det_NxN(A, d+1);
961
962 /* Orient so that each point on the original simplex can't see the opposite face */
963 if(v<0){
964 /* Reverse the order of the last two vertices to change the volume */
965 for(j=0; j<2; j++)
966 face_tmp[j] = faces[k*d+d-j-1];
967 for(j=0; j<2; j++)
968 faces[k*d+d-j-1] = face_tmp[1-j];
969
970 /* Modify the plane coefficients of the properly oriented faces */
971 for(j=0; j<d; j++)
972 cf[k*d+j] = -cf[k*d+j];
973 df[k] = -df[k];
974 for(i=0; i<d; i++)
975 for(j=0; j<(d+1); j++)
976 A[i*(d+1)+j] = points[(faces[k*d+i])*(d+1) + j];
977 for(; i<(d+1); i++)
978 for(j=0; j<(d+1); j++)
979 A[i*(d+1)+j] = points[p*(d+1)+j];
980 }
981 }
982
983 /* Coordinates of the center of the point set */
984 CH_FLOAT* meanp, *reldist, *desReldist, *absdist;
985 meanp = (CH_FLOAT*)calloc(d, sizeof(CH_FLOAT));
986 for(i=d+1; i<nVert; i++)
987 for(j=0; j<d; j++)
988 meanp[j] += points[i*(d+1)+j];
989 for(j=0; j<d; j++)
990 meanp[j] = meanp[j]/(CH_FLOAT)(nVert-d-1);
991
992 /* Absolute distance of points from the center */
993 absdist = (CH_FLOAT*)malloc((nVert-d-1)*d * sizeof(CH_FLOAT));
994 for(i=d+1, k=0; i<nVert; i++, k++)
995 for(j=0; j<d; j++)
996 absdist[k*d+j] = (points[i*(d+1)+j] - meanp[j])/span[j];
997
998 /* Relative distance of points from the center */
999 reldist = (CH_FLOAT*)calloc((nVert-d-1), sizeof(CH_FLOAT));
1000 desReldist = (CH_FLOAT*)malloc((nVert-d-1) * sizeof(CH_FLOAT));
1001 for(i=0; i<(nVert-d-1); i++)
1002 for(j=0; j<d; j++)
1003 reldist[i] += pow(absdist[i*d+j], 2.0);
1004
1005 /* Sort from maximum to minimum relative distance */
1006 int num_pleft, cnt;
1007 int* ind, *pleft;
1008 ind = (int*)malloc((nVert-d-1) * sizeof(int));
1009 pleft = (int*)malloc((nVert-d-1) * sizeof(int));
1010 sort_float(reldist, desReldist, ind, (nVert-d-1), 1);
1011
1012 /* Initialize the vector of points left. The points with the larger relative
1013 distance from the center are scanned first. */
1014 num_pleft = (nVert-d-1);
1015 for(i=0; i<num_pleft; i++)
1016 pleft[i] = ind[i]+d+1;
1017
1018 /* Loop over all remaining points that are not deleted. Deletion of points
1019 occurs every #iter2del# iterations of this while loop */
1020 memset(A, 0, (d+1)*(d+1) * sizeof(CH_FLOAT));
1021
1022 /* cnt is equal to the points having been selected without deletion of
1023 nonvisible points (i.e. points inside the current convex hull) */
1024 cnt=0;
1025
1026 /* The main loop for the quickhull algorithm */
1027 CH_FLOAT detA;
1028 CH_FLOAT* points_cf, *points_s;
1029 int* visible_ind, *visible, *nonvisible_faces, *f0, *face_s, *u, *gVec, *horizon, *hVec, *pp, *hVec_mem_face;
1030 int num_visible_ind, num_nonvisible_faces, n_newfaces, count, vis;
1031 int f0_sum, u_len, start, num_p, index, horizon_size1;
1032 int FUCKED;
1033 FUCKED = 0;
1034 u = horizon = NULL;
1035 nFaces = d+1;
1036 visible_ind = (int*)malloc(nFaces*sizeof(int));
1037 points_cf = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
1038 points_s = (CH_FLOAT*)malloc(d*sizeof(CH_FLOAT));
1039 face_s = (int*)malloc(d*sizeof(int));
1040 gVec = (int*)malloc(d*sizeof(int));
1041 while( (num_pleft>0) ){
1042 /* i is the first point of the points left */
1043 i = pleft[0];
1044
1045 /* Delete the point selected */
1046 for(j=0; j<num_pleft-1; j++)
1047 pleft[j] = pleft[j+1];
1048 num_pleft--;
1049 if(num_pleft == 0)
1050 free(pleft);
1051 else
1052 pleft = (int*)realloc(pleft, num_pleft*sizeof(int));
1053
1054 /* Update point selection counter */
1055 cnt++;
1056
1057 /* find visible faces */
1058 for(j=0; j<d; j++)
1059 points_s[j] = points[i*(d+1)+j];
1060 points_cf = (CH_FLOAT*)realloc(points_cf,nFaces*sizeof(CH_FLOAT));
1061 visible_ind = (int*)realloc(visible_ind, nFaces*sizeof(int));
1062#ifdef CONVHULL_3D_USE_CBLAS
1063 #ifdef CONVHULL_3D_USE_FLOAT_PRECISION
1064 cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f,
1065 points_s, d,
1066 cf, d, 0.0f,
1067 points_cf, nFaces);
1068 #else
1069 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0,
1070 points_s, d,
1071 cf, d, 0.0,
1072 points_cf, nFaces);
1073 #endif
1074#else
1075 for (j = 0; j < nFaces; j++) {
1076 points_cf[j] = 0;
1077 for (k = 0; k < d; k++)
1078 points_cf[j] += points_s[k]*cf[j*d+k];
1079 }
1080#endif
1081 num_visible_ind = 0;
1082 for(j=0; j<nFaces; j++){
1083 if(points_cf[j] + df[j] > 0.0){
1084 num_visible_ind++; /* will sum to 0 if none are visible */
1085 visible_ind[j] = 1;
1086 }
1087 else
1088 visible_ind[j] = 0;
1089 }
1090 num_nonvisible_faces = nFaces - num_visible_ind;
1091
1092 /* proceed if there are any visible faces */
1093 if(num_visible_ind!=0){
1094 /* Find visible face indices */
1095 visible = (int*)malloc(num_visible_ind*sizeof(int));
1096 for(j=0, k=0; j<nFaces; j++){
1097 if(visible_ind[j]==1){
1098 visible[k]=j;
1099 k++;
1100 }
1101 }
1102
1103 /* Find nonvisible faces */
1104 nonvisible_faces = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
1105 f0 = (int*)malloc(num_nonvisible_faces*d*sizeof(int));
1106 for(j=0, k=0; j<nFaces; j++){
1107 if(visible_ind[j]==0){
1108 for(l=0; l<d; l++)
1109 nonvisible_faces[k*d+l] = faces[j*d+l];
1110 k++;
1111 }
1112 }
1113
1114 /* Create horizon (count is the number of the edges of the horizon) */
1115 count=0;
1116 for(j=0; j<num_visible_ind; j++){
1117 /* visible face */
1118 vis = visible[j];
1119 for(k=0; k<d; k++)
1120 face_s[k] = faces[vis*d+k];
1121 sort_int(face_s, NULL, NULL, d, 0);
1122 ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces*d, d);
1123 u_len = 0;
1124
1125 /* u are the nonvisible faces connected to the face v, if any */
1126 for(k=0; k<num_nonvisible_faces; k++){
1127 f0_sum = 0;
1128 for(l=0; l<d; l++)
1129 f0_sum += f0[k*d + l];
1130 if(f0_sum == d-1){
1131 u_len++;
1132 if(u_len==1)
1133 u = (int*)malloc(u_len*sizeof(int));
1134 else
1135 u = (int*)realloc(u, u_len*sizeof(int));
1136 u[u_len-1] = k;
1137 }
1138 }
1139 for(k=0; k<u_len; k++){
1140 /* The boundary between the visible face v and the k(th) nonvisible face connected to the face v forms part of the horizon */
1141 count++;
1142 if(count==1)
1143 horizon = (int*)malloc(count*(d-1)*sizeof(int));
1144 else
1145 horizon = (int*)realloc(horizon, count*(d-1)*sizeof(int));
1146 for(l=0; l<d; l++)
1147 gVec[l] = nonvisible_faces[u[k]*d+l];
1148 for(l=0, h=0; l<d; l++){
1149 if(f0[u[k]*d+l]){
1150 horizon[(count-1)*(d-1)+h] = gVec[l];
1151 h++;
1152 }
1153 }
1154 }
1155 if(u_len!=0)
1156 free(u);
1157 }
1158 horizon_size1 = count;
1159 for(j=0, l=0; j<nFaces; j++){
1160 if(!visible_ind[j]){
1161 /* Delete visible faces */
1162 for(k=0; k<d; k++)
1163 faces[l*d+k] = faces[j*d+k];
1164
1165 /* Delete the corresponding plane coefficients of the faces */
1166 for(k=0; k<d; k++)
1167 cf[l*d+k] = cf[j*d+k];
1168 df[l] = df[j];
1169 l++;
1170 }
1171 }
1172
1173 /* Update the number of faces */
1174 nFaces = nFaces-num_visible_ind;
1175 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
1176 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
1177 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
1178
1179 /* start is the first row of the new faces */
1180 start=nFaces;
1181
1182 /* Add faces connecting horizon to the new point */
1183 n_newfaces = horizon_size1;
1184 for(j=0; j<n_newfaces; j++){
1185 nFaces++;
1186 faces = (int*)realloc(faces, nFaces*d*sizeof(int));
1187 cf = (CH_FLOAT*)realloc(cf, nFaces*d*sizeof(CH_FLOAT));
1188 df = (CH_FLOAT*)realloc(df, nFaces*sizeof(CH_FLOAT));
1189 for(k=0; k<d-1; k++)
1190 faces[(nFaces-1)*d+k] = horizon[j*(d-1)+k];
1191 faces[(nFaces-1)*d+(d-1)] = i;
1192
1193 /* Calculate and store appropriately the plane coefficients of the faces */
1194 for(k=0; k<d; k++)
1195 for(l=0; l<d; l++)
1196 p_s[k*d+l] = points[(faces[(nFaces-1)*d+k])*(d+1) + l];
1197 plane_nd(d, p_s, cfi, &dfi);
1198 for(k=0; k<d; k++)
1199 cf[(nFaces-1)*d+k] = cfi[k];
1200 df[(nFaces-1)] = dfi;
1201 if(nFaces > CH_MAX_NUM_FACES){
1202 FUCKED = 1;
1203 nFaces = 0;
1204 break;
1205 }
1206 }
1207
1208 /* Orient each new face properly */
1209 hVec = (int*)malloc( nFaces*sizeof(int));
1210 hVec_mem_face = (int*)malloc( nFaces*sizeof(int));
1211 for(j=0; j<nFaces; j++)
1212 hVec[j] = j;
1213 for(k=start; k<nFaces; k++){
1214 for(j=0; j<d; j++)
1215 face_s[j] = faces[k*d+j];
1216 sort_int(face_s, NULL, NULL, d, 0);
1217 ismember(hVec, face_s, hVec_mem_face, nFaces, d);
1218 num_p = 0;
1219 for(j=0; j<nFaces; j++)
1220 if(!hVec_mem_face[j])
1221 num_p++;
1222 pp = (int*)malloc(num_p*sizeof(int));
1223 for(j=0, l=0; j<nFaces; j++){
1224 if(!hVec_mem_face[j]){
1225 pp[l] = hVec[j];
1226 l++;
1227 }
1228 }
1229 index = 0;
1230 detA = 0.0;
1231
1232 /* While new point is coplanar, choose another point */
1233 while(detA==0.0){
1234 for(j=0;j<d; j++)
1235 for(l=0; l<d+1; l++)
1236 A[j*(d+1)+l] = points[(faces[k*d+j])*(d+1) + l];
1237 for(; j<d+1; j++)
1238 for(l=0; l<d+1; l++)
1239 A[j*(d+1)+l] = points[pp[index]*(d+1)+l];
1240 index++;
1241 if(d==3)
1242 detA = det_4x4(A);
1243 else
1244 detA = det_NxN((CH_FLOAT*)A, d+1);
1245 }
1246
1247 /* Orient faces so that each point on the original simplex can't see the opposite face */
1248 if (detA<0.0){
1249 /* If orientation is improper, reverse the order to change the volume sign */
1250 for(j=0; j<2; j++)
1251 face_tmp[j] = faces[k*d+d-j-1];
1252 for(j=0; j<2; j++)
1253 faces[k*d+d-j-1] = face_tmp[1-j];
1254
1255 /* Modify the plane coefficients of the properly oriented faces */
1256 for(j=0; j<d; j++)
1257 cf[k*d+j] = -cf[k*d+j];
1258 df[k] = -df[k];
1259 for(l=0; l<d; l++)
1260 for(j=0; j<d+1; j++)
1261 A[l*(d+1)+j] = points[(faces[k*d+l])*(d+1) + j];
1262 for(; l<d+1; l++)
1263 for(j=0; j<d+1; j++)
1264 A[l*(d+1)+j] = points[pp[index]*(d+1)+j];
1265#ifndef NDEBUG
1266 /* Check */
1267 if(d==3)
1268 detA = det_4x4(A);
1269 else
1270 detA = det_NxN((CH_FLOAT*)A, d+1);
1271 assert(detA>0.0);
1272#endif
1273 }
1274 free(pp);
1275 }
1276 if(horizon_size1>0)
1277 free(horizon);
1278 free(f0);
1279 free(nonvisible_faces);
1280 free(visible);
1281 free(hVec);
1282 free(hVec_mem_face);
1283 }
1284 if(FUCKED){
1285 break;
1286 }
1287 }
1288
1289 /* output */
1290 if(FUCKED){
1291 (*out_faces) = NULL;
1292 if(out_cf!=NULL)
1293 (*out_cf) = NULL;
1294 if(out_df!=NULL)
1295 (*out_df) = NULL;
1296 (*nOut_faces) = 0;
1297 }
1298 else{
1299 (*out_faces) = (int*)malloc(nFaces*d*sizeof(int));
1300 memcpy((*out_faces),faces, nFaces*d*sizeof(int));
1301 (*nOut_faces) = nFaces;
1302 if(out_cf!=NULL){
1303 (*out_cf) = (CH_FLOAT*)malloc(nFaces*d*sizeof(CH_FLOAT));
1304 memcpy((*out_cf), cf, nFaces*d*sizeof(CH_FLOAT));
1305 }
1306 if(out_df!=NULL){
1307 (*out_df) = (CH_FLOAT*)malloc(nFaces*sizeof(CH_FLOAT));
1308 memcpy((*out_df), df, nFaces*sizeof(CH_FLOAT));
1309 }
1310 }
1311
1312 /* clean-up */
1313 free(visible_ind);
1314 free(points_cf);
1315 free(points_s);
1316 free(face_s);
1317 free(gVec);
1318 free(meanp);
1319 free(absdist);
1320 free(reldist);
1321 free(desReldist);
1322 free(ind);
1323 free(span);
1324 free(points);
1325 free(faces);
1326 free(aVec);
1327 free(cf);
1328 free(cfi);
1329 free(df);
1330 free(p_s);
1331 free(fVec);
1332 free(bVec);
1333 free(A);
1334}
1335
1336void convhull_3d_export_obj
1337(
1338 ch_vertex* const vertices,
1339 const int nVert,
1340 int* const faces,
1341 const int nFaces,
1342 const int keepOnlyUsedVerticesFLAG,
1343 char* const obj_filename
1344)
1345{
1346 int i, j;
1347 char path[256] = "\0";
1348 memcpy(path, obj_filename, strlen(obj_filename));
1349 FILE* obj_file;
1350#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1351 CV_STRCAT(path, ".obj");
1352 fopen_s(&obj_file, path, "wt");
1353#else
1354 obj_file = fopen(strcat(path, ".obj"), "wt");
1355#endif
1356 fprintf(obj_file, "o\n");
1357 CH_FLOAT scale;
1358 ch_vec3 v1, v2, normal;
1359
1360 /* export vertices */
1361 if(keepOnlyUsedVerticesFLAG){
1362 for (i = 0; i < nFaces; i++)
1363 for(j=0; j<3; j++)
1364 fprintf(obj_file, "v %f %f %f\n", vertices[faces[i*3+j]].x,
1365 vertices[faces[i*3+j]].y, vertices[faces[i*3+j]].z);
1366 }
1367 else {
1368 for (i = 0; i < nVert; i++)
1369 fprintf(obj_file, "v %f %f %f\n", vertices[i].x,
1370 vertices[i].y, vertices[i].z);
1371 }
1372
1373 /* export the face normals */
1374 for (i = 0; i < nFaces; i++){
1375 /* calculate cross product between v1-v0 and v2-v0 */
1376 v1 = vertices[faces[i*3+1]];
1377 v2 = vertices[faces[i*3+2]];
1378 v1.x -= vertices[faces[i*3]].x;
1379 v1.y -= vertices[faces[i*3]].y;
1380 v1.z -= vertices[faces[i*3]].z;
1381 v2.x -= vertices[faces[i*3]].x;
1382 v2.y -= vertices[faces[i*3]].y;
1383 v2.z -= vertices[faces[i*3]].z;
1384 normal = cross(&v1, &v2);
1385
1386 /* normalise to unit length */
1387 scale = 1.0/(sqrt(pow(normal.x, 2.0)+pow(normal.y, 2.0)+pow(normal.z, 2.0))+2.23e-9);
1388 normal.x *= scale;
1389 normal.y *= scale;
1390 normal.z *= scale;
1391 fprintf(obj_file, "vn %f %f %f\n", normal.x, normal.y, normal.z);
1392 }
1393
1394 /* export the face indices */
1395 if(keepOnlyUsedVerticesFLAG){
1396 for (i = 0; i < nFaces; i++){
1397 /* vertices are in same order as the faces, and normals are in order */
1398 fprintf(obj_file, "f %u//%u %u//%u %u//%u\n",
1399 i*3 + 1, i + 1,
1400 i*3+1 + 1, i + 1,
1401 i*3+2 + 1, i + 1);
1402 }
1403 }
1404 else {
1405 /* just normals are in order */
1406 for (i = 0; i < nFaces; i++){
1407 fprintf(obj_file, "f %u//%u %u//%u %u//%u\n",
1408 faces[i*3] + 1, i + 1,
1409 faces[i*3+1] + 1, i + 1,
1410 faces[i*3+2] + 1, i + 1);
1411 }
1412 }
1413 fclose(obj_file);
1414}
1415
1416void convhull_3d_export_m
1417(
1418 ch_vertex* const vertices,
1419 const int nVert,
1420 int* const faces,
1421 const int nFaces,
1422 char* const m_filename
1423)
1424{
1425 int i;
1426 char path[256] = { "\0" };
1427 memcpy(path, m_filename, strlen(m_filename));
1428 FILE* m_file;
1429#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1430 CV_STRCAT(path, ".m");
1431 fopen_s(&m_file, path, "wt");
1432#else
1433 m_file = fopen(strcat(path, ".m"), "wt");
1434#endif
1435
1436 /* save face indices and vertices for verification in matlab: */
1437 fprintf(m_file, "vertices = [\n");
1438 for (i = 0; i < nVert; i++)
1439 fprintf(m_file, "%f, %f, %f;\n", vertices[i].x, vertices[i].y, vertices[i].z);
1440 fprintf(m_file, "];\n\n\n");
1441 fprintf(m_file, "faces = [\n");
1442 for (i = 0; i < nFaces; i++) {
1443 fprintf(m_file, " %u, %u, %u;\n",
1444 faces[3*i+0]+1,
1445 faces[3*i+1]+1,
1446 faces[3*i+2]+1);
1447 }
1448 fprintf(m_file, "];\n\n\n");
1449 fclose(m_file);
1450}
1451
1452void extractVerticesFromObjFile(char* const obj_filename, ch_vertex** out_vertices, int* out_nVert)
1453{
1454 FILE* obj_file;
1455#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
1456 CV_STRCAT(obj_filename, ".obj");
1457 fopen_s(&obj_file, obj_filename, "r");
1458#else
1459 obj_file = fopen(strcat(obj_filename, ".obj"), "r");
1460#endif
1461
1462 /* determine number of vertices */
1463 unsigned int nVert = 0;
1464 char line[256];
1465 while (fgets(line, sizeof(line), obj_file)) {
1466 char* vexists = strstr(line, "v ");
1467 if(vexists!=NULL)
1468 nVert++;
1469 }
1470 (*out_nVert) = nVert;
1471 (*out_vertices) = (ch_vertex*)malloc(nVert*sizeof(ch_vertex));
1472
1473 /* extract the vertices */
1474 rewind(obj_file);
1475 int i=0;
1476 int vertID, prev_char_isDigit, current_char_isDigit;
1477 char vert_char[256] = { 0 };
1478 while (fgets(line, sizeof(line), obj_file)) {
1479 char* vexists = strstr(line, "v ");
1480 if(vexists!=NULL){
1481 prev_char_isDigit = 0;
1482 vertID = -1;
1483 for(int j=0; j<(int)strlen(line)-1; j++){
1484 if(isdigit(line[j])||line[j]=='.'||line[j]=='-'||line[j]=='+'||line[j]=='E'||line[j]=='e'){
1485 vert_char[strlen(vert_char)] = line[j];
1486 current_char_isDigit = 1;
1487 }
1488 else
1489 current_char_isDigit = 0;
1490 if((prev_char_isDigit && !current_char_isDigit) || j ==(int)strlen(line)-2 ){
1491 vertID++;
1492 if(vertID>4){
1493 /* not a valid file */
1494 free((*out_vertices));
1495 (*out_vertices) = NULL;
1496 (*out_nVert) = 0;
1497 return;
1498 }
1499 (*out_vertices)[i].v[vertID] = atof(vert_char);
1500 memset(vert_char, 0, 256 * sizeof(char));
1501 }
1502 prev_char_isDigit = current_char_isDigit;
1503 }
1504 i++;
1505 }
1506 }
1507}
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:1417
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:1452
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:1337
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:850
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
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