path: root/src/mtx_qhull/zhull.c

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