convex hull algorithm added

svn path=/trunk/externals/iem/iemmatrix/; revision=16165

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);
+}
+