diff options
Diffstat (limited to 'src/mtx_qhull/zhull.c')
| -rw-r--r-- | src/mtx_qhull/zhull.c | 482 |
1 files changed, 482 insertions, 0 deletions
diff --git a/src/mtx_qhull/zhull.c b/src/mtx_qhull/zhull.c new file mode 100644 index 0000000..d13260e --- /dev/null +++ b/src/mtx_qhull/zhull.c @@ -0,0 +1,482 @@ +#define TOL 1e-7 +#define MAXIT 1000000 + + +vector_t averageListedPoints(const points_t points, const list_t list) { + index_t i; + vector_t m = initVector(0.0f, 0.0f, 0.0f); + for (i=0; i<getLength(list); i++) { + m=addVectors(points.v[getEntry(list,i)],m); + } + m=scaleVector(m,1.0f/((float)getLength(list))); + return m; +} + +vector_t normalOfListedPoints(const points_t points, const list_t list) { + vector_t m = averageListedPoints(points,list); + vector_t n = initVector(0.0f, 0.0f, 0.0f); + vector_t d1,d2,c; + index_t i; + for (i=1; i<getLength(list); i++) { + d1=subtractVectors(points.v[getEntry(list,i-1)],m); + d2=subtractVectors(points.v[getEntry(list,i)],m); + c=crossProduct(d1,d2); + n=addVectors(c,n); + } + return n; +} + +plane_t planeFromListedPoints(const points_t points, const list_t list) { + vector_t p=averageListedPoints(points,list); + vector_t n=normalOfListedPoints(points,list); + plane_t pl=initPlane(n,p); + return pl; +} + +/* facets, memory things */ + +void freeFacet(facet_t *facet) { +/* printf("deleting facet %li\n",facet); + printList(facet->corners); + printList(facet->outsideset); + printList(facet->insideset); + printList(facet->neighbors);*/ + freeList(&(facet->corners)); + freeList(&(facet->outsideset)); + freeList(&(facet->insideset)); + freeList(&(facet->neighbors)); +} + +list_t appendNewFacets(zhull_t * const zh, const size_t num_facets) { + facet_t *new_facet; + list_t new_facets=initConstantList(0,num_facets); + index_t i; + for (i=0; i<getLength(new_facets); i++) { + new_facet = (facet_t*) malloc(sizeof(facet_t)); + if (new_facet==0) + break; + new_facet->neighbors=emptyList(); + new_facet->corners=emptyList(); + new_facet->outsideset=emptyList(); + new_facet->insideset=emptyList(); + new_facet->maxdistance=0; + new_facet->farthest_outside_point=0; +// printf("%li, %li\n", new_facet, (entry_t)new_facet); + setEntry(new_facets,i,(entry_t)new_facet); +// printf("created facet %li\n",new_facet); + } + appendListToList(&(zh->facets),new_facets); + return new_facets; +} + +/* facets, interface */ +/*entry_t getFacetCornerByIndex(const facet_t *facet, + index_t index_corner) { + if (facet!=0) + return getEntry(facet->corners, + index_corner%getLength(facet->corners)); + else + return 0; + } + */ + +facet_t *getFacetByIndex(const list_t facets, + const index_t index) { + return (facet_t*) getEntry(facets,index); +} + +facet_t *getFacetByPointer(const entry_t ptr) { + return (facet_t*) ptr; +} + +void getHorizonEdgeByIndex(index_t *corners, + const list_t horizon_fcts, const list_t horizon_fcts_edges, + const index_t index) { + index_t i=(index+getLength(horizon_fcts_edges)) + %getLength(horizon_fcts_edges); + facet_t *f = getFacetByIndex(horizon_fcts,i); + index_t j=getEntry(horizon_fcts_edges,i); + + corners[0]=getEntry(f->corners, j); + if (getLength(f->corners)>0) + corners[1]=getEntry(f->corners, (j+1) % getLength(f->corners)); + else + corners[1]=corners[0]; +} + + +void removeFacetByPointer(zhull_t * const zh, + facet_t * const pointer) { + removeValueFromList(&(zh->facets), (entry_t)pointer); + removeValueFromList(&(zh->facets_with_outsidepoints), + (entry_t)pointer); + removeValueFromList(&(zh->facets_with_insidepoints), + (entry_t)pointer); + freeFacet((facet_t*)pointer); +} + +void removeFacetByPointerList(zhull_t * const zh, + const list_t pointers) { + index_t i; + for (i=0; i<getLength(pointers); i++) { + removeFacetByPointer(zh, + getFacetByIndex(pointers,i)); + } +} + +void removeFacetByIndex(zhull_t * const zh, + const index_t index) { + removeFacetByPointer(zh, + getFacetByIndex(zh->facets, index)); +} + +void removeFacetByIndexList(zhull_t * const zh, + const list_t indices) { + facet_t *f; + index_t i; + for (i=0; i<getLength(indices); i++) { + f = getFacetByIndex(zh->facets, getEntry(indices,i)); + removeFacetByPointer(zh,f); + } +} + + +void freeFacets(zhull_t * const zh) { + int i; + facet_t *f; + if (getLength(zh->facets)>0) { + for (i=0; i<getLength(zh->facets); i++) { + f=getFacetByIndex(zh->facets,i); +// printf("deleting facet %li\n",i); + freeFacet(f); + } + freeList(&(zh->facets)); + } +} + +void freeZhull(zhull_t *zh) { + if (zh!=0) { +// printf("free zhull\n"); + freeFacets(zh); + freeList(&(zh->facets_with_insidepoints)); + freeList(&(zh->facets_with_outsidepoints)); + freePoints(&(zh->pts)); + } +} + + +// *********************************** +// +// interface + + +vector_t getPoint(const zhull_t * const zh, + const index_t index) { + return (vector_t) zh->pts.v[index]; +} + +zhull_t zhullInitPoints(const float *x, const float *y, + const float *z, const size_t num_points) { + zhull_t zh; + zh.pts=initPoints(x,y,z,num_points); + zh.facets=emptyList(); + zh.facets_with_outsidepoints=emptyList(); + zh.facets_with_insidepoints=emptyList(); + return zh; +} + + + +void dividePointsBetweenNewFacets ( + zhull_t * const zh, const list_t assoc, + const list_t new_facets, const list_t horizon_fcts) { + index_t i,j; + list_t new_and_horizon_fcts=mergeLists(new_facets,horizon_fcts); + printList(new_and_horizon_fcts); + uniquefyListEntries(&new_and_horizon_fcts); + printList(new_and_horizon_fcts); + float d; + for (i=0; i<getLength(assoc); i++) { + printf("p %d\n", getEntry(assoc,i)); + for (j=0; j<getLength(new_and_horizon_fcts); j++) { + d=distancePointPlane(getPoint(zh,getEntry(assoc,i)), + getFacetByIndex(new_and_horizon_fcts,j)->plane); + // printf("distance %5.2f to %d\n",d,findValueInList((entry_t)getFacetByIndex(new_and_horizon_fcts,j),zh->facets)); + /* + if ((d>=-TOL) && (d<=TOL)) { + appendToList(&(getFacetByIndex(new_facets,j)->insideset), + getEntry(assoc,i)); + if (notInList(getEntry(new_facets,j),zh->facets_with_insidepoints)) + appendToList(&(zh->facets_with_insidepoints), + getEntry(new_facets,j)); + break; + } + else */ + if (d>=-TOL) { + appendToList(&(getFacetByIndex(new_and_horizon_fcts,j)->outsideset), + getEntry(assoc,i)); + // printf("appended "); + if (notInList(getEntry(new_and_horizon_fcts,j),zh->facets_with_outsidepoints)) + appendToList(&(zh->facets_with_outsidepoints),getEntry(new_and_horizon_fcts,j)); + if (getFacetByIndex(new_and_horizon_fcts,j)->maxdistance<d) { + getFacetByIndex(new_and_horizon_fcts,j)->maxdistance=d; + getFacetByIndex(new_and_horizon_fcts,j)->farthest_outside_point=getEntry(assoc,i); + } + break; + } + } + } +} + +void zhullInitialFacets(zhull_t *zh) { + list_t assoc = emptyList(); + list_t new_facets = emptyList(); + index_t i; + if (zh->pts.num_points >= 3) { + assoc = initListFromTo(0,zh->pts.num_points-1); + new_facets = appendNewFacets(zh,2); + if (getLength(new_facets)==2) { + getFacetByIndex(new_facets,0)->corners = initListFromTo(0,2); + getFacetByIndex(new_facets,1)->corners = initListFromTo(2,0); + getFacetByIndex(new_facets,0)->neighbors = + initConstantList((entry_t)getFacetByIndex(new_facets,1),3); + getFacetByIndex(new_facets,1)->neighbors = + initConstantList((entry_t)getFacetByIndex(new_facets,0),3); + for (i=0; i<2; i++) { + getFacetByIndex(new_facets,i)->plane = + planeFromListedPoints(zh->pts, getFacetByIndex(new_facets,i)->corners); + getFacetByIndex(new_facets,i)->outsideset = emptyList(); + getFacetByIndex(new_facets,i)->insideset = emptyList(); + getFacetByIndex(new_facets,i)->maxdistance = 0.0f; + removeValueListFromList(&assoc,getFacetByIndex(new_facets,i)->corners); + } + dividePointsBetweenNewFacets(zh, assoc, new_facets, emptyList()); + } + freeList(&new_facets); + freeList(&assoc); + } +} + +void printHorizonEdges(list_t *horizon_fcts, + list_t *horizon_fcts_edges) { + index_t i; + index_t c1[2]; + + printf("horizon edges: "); + for (i=0; i<getLength(*horizon_fcts_edges); i++) { + getHorizonEdgeByIndex(c1, *horizon_fcts, *horizon_fcts_edges, i); + printf(", %d->%d",c1[0],c1[1]); + } + printf("\n"); +} + + +void sortHorizonEdges(list_t *horizon_fcts, + list_t *horizon_fcts_edges) { + index_t i,j; + facet_t *fi; + index_t ei; + index_t c1[2]; + index_t c2[2]; + + if (getLength(*horizon_fcts_edges)==0) + return; + for (i=0; i<getLength(*horizon_fcts_edges)-1; i++) { + getHorizonEdgeByIndex(c1, *horizon_fcts, *horizon_fcts_edges, i); + for (j=i+1; j<getLength(*horizon_fcts_edges); j++) { + getHorizonEdgeByIndex(c2, *horizon_fcts, *horizon_fcts_edges, j); + if (c1[1]==c2[0]) { //found edge continuation: swap positions + ei=getEntry(*horizon_fcts,j); + setEntry(*horizon_fcts,j,getEntry(*horizon_fcts,i+1)); + setEntry(*horizon_fcts,i+1,ei); + ei=getEntry(*horizon_fcts_edges,j); + setEntry(*horizon_fcts_edges,j,getEntry(*horizon_fcts_edges,i+1)); + setEntry(*horizon_fcts_edges,i+1,ei); + break; + } + // HIER MUSS MEHR LOGIK HINEIN: HORIZONT KANN + // FEHLER BEINHALTEN + } + } +} + +void removeVisibleFacetsGetHorizonAndAvailablePoints( + zhull_t * const zh, index_t point_index, + facet_t *facet, + list_t *horizon_fcts, + list_t *horizon_fcts_edges, + list_t *avail_points) { + + index_t i,j,k; + facet_t *f, *n; + float d; + list_t visible_fcts = emptyList(); + list_t indices_for_printing = emptyList(); + + *avail_points = emptyList(); + *horizon_fcts = emptyList(); + *horizon_fcts_edges = emptyList(); + + appendToList(&visible_fcts, (entry_t)facet); + for(i=0;i<getLength(visible_fcts);i++) { + f=getFacetByIndex(visible_fcts,i); + appendListToList(avail_points, f->outsideset); + appendListToList(avail_points, f->insideset); + for (j=0; j<getLength(f->neighbors); j++) { + n=getFacetByIndex(f->neighbors,j); + if (notInList((entry_t) n, visible_fcts)) { + d=distancePointPlane(getPoint(zh,point_index), + n->plane); + if (d>-TOL) { + if (notInList((entry_t)n,visible_fcts)) + appendToList(&visible_fcts,(entry_t)n); + } + else { + appendToList(horizon_fcts,(entry_t)n); + k=getEntry(f->corners,(j+1)%getLength(f->corners)); + // printf("searching for corner with %d in\n",k); + // printList(n->corners); + k=findValueInList(k,n->corners); + // printf("found %d\n",k); + appendToList(horizon_fcts_edges,k); + } + } + } + } + printf("removing facets"); + indices_for_printing=findValueListInList(visible_fcts, zh->facets); + printList(indices_for_printing); + removeFacetByPointerList(zh,visible_fcts); + freeList(&visible_fcts); + // printHorizonEdges(horizon_fcts, horizon_fcts_edges); + sortHorizonEdges(horizon_fcts, horizon_fcts_edges); + printHorizonEdges(horizon_fcts, horizon_fcts_edges); +} + + +void initNewFacets(zhull_t *zh, int point_index, + list_t new_facets, list_t horizon_fcts, + list_t horizon_fcts_edges) { + index_t i,j; + entry_t array[3]; + + array[0]=point_index; + for (i=0; i<getLength(new_facets); i++) { + // printf("adding facet %d\n",getEntry(new_facets,i)); + + // corners + getHorizonEdgeByIndex(array,horizon_fcts, horizon_fcts_edges,i); + array[2]=array[1]; + array[1]=array[0]; + array[0]=array[2]; + array[2]=point_index; + getFacetByIndex(new_facets,i)->corners=initList(array,3); + // printList(getFacetByIndex(new_facets,i)->corners); + + // neighbors + // previous new neighbor + j=(getLength(horizon_fcts)+i-1) % getLength(horizon_fcts); + array[1]=getEntry(new_facets,j); + // next new neighbor + j=(i+1) % getLength(horizon_fcts); + array[2]=getEntry(new_facets,j); + // old neighbor + array[0]=getEntry(horizon_fcts,i); + getFacetByIndex(new_facets,i)->neighbors= + initList(array,3); + + // registering at old neighbor: + setEntry(getFacetByIndex(horizon_fcts,i)->neighbors, + getEntry(horizon_fcts_edges,i), getEntry(new_facets,i)); + + // initializing normal vectors and lists + getFacetByIndex(new_facets,i)->plane = + planeFromListedPoints(zh->pts, + getFacetByIndex(new_facets,i)->corners); + } +} + +void makePyramidFacetsToHorizon(zhull_t *zh, index_t point_index, + list_t horizon_fcts, list_t horizon_fcts_edges, + list_t avail_points) { + list_t new_facets = appendNewFacets(zh, getLength(horizon_fcts_edges)); + initNewFacets(zh,point_index,new_facets,horizon_fcts,horizon_fcts_edges); + // printf("available points: "); + // printList(avail_points); + // printf("new facets : "); + // printList(new_facets); + dividePointsBetweenNewFacets(zh, avail_points, new_facets, horizon_fcts); + freeList(&new_facets); +} + +void calculateZHull(zhull_t *zh,int maxit) { + index_t fli=0; + index_t pi; + facet_t *f; + list_t outsideset; + int cnt=0; + list_t horizon_fcts=emptyList(); + list_t horizon_fcts_edges=emptyList(); + list_t available_points=emptyList(); + if (maxit>MAXIT) + maxit=MAXIT; + if (zh->pts.num_points!=0){ + zhullInitialFacets(zh); + printZhull(zh); + while((getLength(zh->facets_with_outsidepoints)>0)&&(cnt++<maxit)) { + printf("//////////////// ITERATION %d ///////\n",cnt); + fli%=getLength(zh->facets_with_outsidepoints); + f=getFacetByIndex(zh->facets_with_outsidepoints,fli); + pi=f->farthest_outside_point; + removeVisibleFacetsGetHorizonAndAvailablePoints(zh,pi,f, + &horizon_fcts, &horizon_fcts_edges, + &available_points); + removeValueFromList(&available_points, pi); + makePyramidFacetsToHorizon(zh,pi,horizon_fcts,horizon_fcts_edges, + available_points); + printZhull(zh); + + freeList(&horizon_fcts); + freeList(&horizon_fcts_edges); + freeList(&available_points); + fli++; + } + } +} + +void printZhull(const zhull_t * const zh) { + index_t fi; + list_t indices = emptyList(); + printf("zhull from %d points\n", zh->pts.num_points); + printf("facets with outsidepoints: "); + indices=findValueListInList(zh->facets_with_outsidepoints,zh->facets); + printList(indices); + freeList(&indices); +// printf("facets with insidepoints: "); +// printList(zh->facets_with_insidepoints); + printf("zhull has %d facets\n", getLength(zh->facets)); + for (fi=0; fi<getLength(zh->facets); fi++) { + printf("facet %d<%d>: ",fi,getFacetByIndex(zh->facets,fi)); + printFacet(zh,getFacetByIndex(zh->facets,fi)); + } +} + +void printFacet(const zhull_t * const zh, + const facet_t * const f) { + list_t indices=emptyList(); + indices=findValueListInList(f->neighbors,zh->facets); + printf("plane: "); + printPlane(f->plane); + printf("\n"); + printf("corners: "); + printList(f->corners); + printf("outsideset: "); + printList(f->outsideset); + printf("insideset: "); + printList(f->insideset); + printf("neighbors: "); + printList(indices); + freeList(&indices); + printf("pt %d with maxdist %5.2f\n",f->farthest_outside_point, f->maxdistance); +} + |