RomkaZVO/FC1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
main
FC1/CryPhysics/qhull.cpp
romkazvo 34d6c5d489 123
2023-08-07 19:29:24 +08:00

506 lines
17 KiB
C++
Raw Permalink Blame History

//////////////////////////////////////////////////////////////////////
//
// Quick Hull
//
// File: qhull.cpp
// Description : Quick Hull algorithm implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "utils.h"
///////////////////////////// Qhull 3d ///////////////////////////////
struct ptitem {
ptitem *next,*prev;
};
struct qhtritem {
qhtritem *next,*prev;
ptitem *ptassoc;
vectorf n;
float d;
int idx[3];
qhtritem *buddy[3];
int deleted;
};
inline void relocate_ptritem(qhtritem *&ptr,intptr_t diff) {
ptr = (qhtritem*)((intptr_t)ptr+diff & ~-iszero((intptr_t)ptr)); // offset the pointer, but leave out 0s
}
inline void relocate_tritem(qhtritem *ptr,int diff) {
relocate_ptritem(ptr->next,diff); relocate_ptritem(ptr->prev,diff);
relocate_ptritem(ptr->buddy[0],diff); relocate_ptritem(ptr->buddy[1],diff); relocate_ptritem(ptr->buddy[2],diff);
}
template<class item> void delete_item_from_list(item *p) {
if (p->prev) p->prev->next = p->next;
if (p->next) p->next->prev = p->prev;
p->prev = p->next = 0;
}
template<class item> void add_item_to_list(item *&pin, item *pnew) {
if (!pin) pin=pnew->next=pnew->prev=pnew;
else {
pnew->next = pin->next; pnew->prev = pin;
pin->next->prev = pnew; pin->next = pnew;
}
}
template<class item> void merge_lists(item *&plist, item *pnew) {
if (!pnew) return;
if (!plist) plist = pnew;
else {
plist->next->prev = pnew->prev;
pnew->prev->next = plist->next;
plist->next = pnew;
pnew->prev = plist;
}
}
void associate_ptlist_with_trilist(ptitem *ptlist, qhtritem *trilist, ptitem *pt0,strided_pointer<vectorf> pvtx, float e) {
if (!ptlist) return;
ptitem *pt=ptlist,*ptnext,*ptlast=ptlist->prev;
qhtritem *tr; int i;
do {
ptnext = pt->next;
delete_item_from_list(pt);
tr=trilist; i=pt-pt0; do {
if (pvtx[i]*tr->n > tr->d+e) {
add_item_to_list(tr->ptassoc, pt);
break;
}
tr=tr->next;
} while(tr!=trilist);
if (pt==ptlast) break;
pt = ptnext;
} while(true);
}
static inline void swap(int *v,void **p, int i1,int i2)
{
int ti=v[i1]; v[i1]=v[i2]; v[i2]=ti;
void *tp=p[i1]; p[i1]=p[i2]; p[i2]=tp;
}
static void qsort(int *v,void **p, int left,int right)
{
if (left>=right) return;
int i,last;
swap(v,p, left, left+right>>1);
for(last=left,i=left+1; i<=right; i++)
if (v[i] < v[left])
swap(v,p, ++last, i);
swap(v,p, left,last);
qsort(v,p, left,last-1);
qsort(v,p, last+1,right);
}
static int bin_search(int *v,int n,int idx)
{
int left=0,right=n,m;
do {
m = left+right>>1;
if (v[m]==idx) return m;
if (v[m]<idx) left=m;
else right=m;
} while (left<right-1);
return left;
}
int __qhullcalled=0;
int qhull(strided_pointer<vectorf> pts, int npts, index_t*& pTris)
{
static ptitem ptbuf[1024];
static qhtritem trbuf[1024];
static qhtritem *tmparr_ptr_buf[512];
static int tmparr_idx_buf[512];
int iter=0,maxiter=0;
__qhullcalled++;
ptitem *pt,*ptmax,*ptdeleted,*ptlist = npts>sizeof(ptbuf)/sizeof(ptbuf[0]) ? new ptitem[npts] : ptbuf;
qhtritem *tr,*trnext,*trend,*trnew,*trdata=trbuf,*trstart=0,*trlast,*trbest;
int i,j,k,ti,trdatasz=sizeof(trbuf)/sizeof(trbuf[0]), bidx[6],n,next_iter,delbuds;
qhtritem **tmparr_ptr=tmparr_ptr_buf;
int *tmparr_idx=tmparr_idx_buf, tmparr_sz=512;
float dist,maxdist,e;
if (npts<=4) {
pTris = new index_t[6];
pTris[0]=0; pTris[1]=min(1,npts-1); pTris[2]=min(2,npts-1);
pTris[3]=0; pTris[4]=min(1,npts-1); pTris[5]=min(4,npts-1);
return 2;
}
vectorf pmin(MAX),pmax(MIN);
// select points for initial tetrahedron
// first, find 6 points corresponding to min and max coordinates
for(i=1;i<npts;i++) {
if (pts[i].x>pmax.x) { pmax.x=pts[i].x; bidx[0]=i; }
if (pts[i].x<pmin.x) { pmin.x=pts[i].x; bidx[1]=i; }
if (pts[i].y>pmax.y) { pmax.y=pts[i].y; bidx[2]=i; }
if (pts[i].y<pmin.y) { pmin.y=pts[i].y; bidx[3]=i; }
if (pts[i].z>pmax.z) { pmax.z=pts[i].z; bidx[4]=i; }
if (pts[i].z<pmin.z) { pmin.z=pts[i].z; bidx[5]=i; }
}
e = max(max(pmax.x-pmin.x,pmax.y-pmin.y),pmax.z-pmin.z)*0.01f;
// select 4 unique points from the 6 found above
for(k=0;k<2;k++) {
for(i=0;i<3;i++) for(j=i+1;j<4;j++) if (bidx[i]==bidx[j]) {
ti=bidx[4+k]; bidx[4+k]=bidx[j]; bidx[j]=ti;
goto nextk;
} break;
nextk:;
}
if (k==2) { // if 4 unique points cannot be found, select 4 arbitrary points
bidx[0]=0; bidx[1]=npts/3; bidx[2]=npts*2/3; bidx[3]=npts-1;
}
vectorf cp1 = pts[bidx[1]]-pts[bidx[0]] ^ pts[bidx[2]]-pts[bidx[0]];
if (sqr(cp1*(pts[bidx[3]]-pts[bidx[0]])) < cube(e)) { // coplanar points, find another 4th poind
for(i=0,maxdist=0; i<npts; i++) {
dist = fabs_tpl((pts[i]-pts[bidx[0]])*cp1);
if (dist > maxdist) {
maxdist=dist; bidx[3]=i;
}
}
}
// build a double linked list from all points
for(i=0;i<npts;i++) {
ptlist[i].prev = ptlist+i-1;
ptlist[i].next = ptlist+i+1;
}
ptlist[0].prev = ptlist+npts-1;
ptlist[npts-1].next = ptlist;
// remove selected points from the list
for(i=0;i<4;i++) delete_item_from_list(ptlist+bidx[i]);
// assign 3 points to each of 4 initial triangles
for(i=0;i<4;i++) {
for(j=k=0; j<4; j++)
if (j!=i) trbuf[i].idx[k++]=bidx[j]; // skip i.th point in i.th triangle
trbuf[i].n = pts[trbuf[i].idx[1]]-pts[trbuf[i].idx[0]] ^ pts[trbuf[i].idx[2]]-pts[trbuf[i].idx[0]];
trbuf[i].d = trbuf[i].n*pts[trbuf[i].idx[0]];
if (pts[bidx[i]]*trbuf[i].n > trbuf[i].d) { // flip the orientation so that ccw normal points outwards
ti=trbuf[i].idx[0]; trbuf[i].idx[0]=trbuf[i].idx[2]; trbuf[i].idx[2]=ti;
trbuf[i].n = -trbuf[i].n; trbuf[i].d = -trbuf[i].d;
}
trbuf[i].ptassoc = 0;
trbuf[i].deleted = 0;
add_item_to_list(trstart, trbuf+i);
}
// fill buddy links for each triangle
for(i=0;i<4;i++) for(j=0;j<4;j++) if (j!=i) for(k=0;k<3;k++) for(ti=0;ti<3;ti++)
if (trbuf[i].idx[k]==trbuf[j].idx[ti] && trbuf[i].idx[k==2?0:k+1]==trbuf[j].idx[ti==0?2:ti-1]) {
trbuf[i].buddy[k] = trbuf+j; break;
}
trend = trstart+4;
for(i=0;i<4;i++) if (trbuf[i].n.len2()<1E-6f) {
#ifdef _DEBUG
//OutputDebugString("WARNING: convex hull not computed because of degenerate initial triangles\n");
#endif
n=0; goto endqhull; // some degenerate case, don't party with it
}
// associate points with one of the initial triangles
for(i=0;i<npts;i++) if (ptlist[i].next) break;
associate_ptlist_with_trilist(ptlist+i, trstart, ptlist,pts, e);
#define DELETE_TRI(ptri) { \
merge_lists(ptdeleted, (ptri)->ptassoc); \
if ((ptri)==trstart) trstart=(ptri)->next; \
if ((ptri)==trnext) trnext=(ptri)->next; \
delete_item_from_list(ptri); (ptri)->deleted=1; \
}
// main loop
iter=0;maxiter=npts*npts*2;
tr=trstart; do {
trnext = tr->next;
pt=tr->ptassoc; if (pt) {
// find the fartherst of the associated with the triangle points
maxdist=-1E10; do {
if ((dist = pts[(int)(pt-ptlist)]*tr->n) > maxdist)
{ maxdist = dist; ptmax = pt; }
pt = pt->next;
} while (pt!=tr->ptassoc);
ptdeleted = 0;
if (tr->ptassoc==ptmax) tr->ptassoc=ptmax->next;
delete_item_from_list(ptmax);
if (tr->ptassoc==ptmax) tr->ptassoc=0;
// find the triangle that the point can see "most confidently"
tr=trstart; trlast=tr->prev; ti=ptmax-ptlist; maxdist=-1E10;
do {
trnext = tr->next;
if (pts[ti]*tr->n-tr->d > maxdist) {
maxdist = pts[ti]*tr->n-tr->d;
trbest = tr;
}
if (tr==trlast) break;
tr = trnext;
} while (true);
// "flood fill" triangles that the point can see around that one
DELETE_TRI(trbest)
tr = trbest->next=trbest->prev = trbest;
do {
if (!tr->buddy[0]->deleted && pts[ti]*tr->buddy[0]->n > tr->buddy[0]->d) {
DELETE_TRI(tr->buddy[0])
add_item_to_list(tr, tr->buddy[0]);
}
if (!tr->buddy[1]->deleted && pts[ti]*tr->buddy[1]->n > tr->buddy[1]->d) {
DELETE_TRI(tr->buddy[1])
add_item_to_list(tr, tr->buddy[1]);
}
if (!tr->buddy[2]->deleted && pts[ti]*tr->buddy[2]->n > tr->buddy[2]->d) {
DELETE_TRI(tr->buddy[2])
add_item_to_list(tr, tr->buddy[2]);
}
tr = tr->next;
} while(tr!=trbest);
/*// delete triangles that the point can see
tr=trstart; trlast=tr->prev; ti=ptmax-ptlist;
do {
trnext = tr->next;
if (pts[ti]*tr->n > tr->d)
DELETE_TRI(tr)
if (tr==trlast) break;
tr = trnext;
} while (true);*/
// delete near-visible triangles around deleted area edges to preserve hole convexity
// do as many iterations as needed
do {
tr=trstart; trlast=tr->prev; next_iter=0;
do {
trnext = tr->next;
if (pts[ti]*tr->n>tr->d-e*0.5f) {
delbuds = tr->buddy[0]->deleted+tr->buddy[1]->deleted+tr->buddy[2]->deleted;
if (delbuds >= 2) { // delete triangles that have 2+ buddies deleted
if (tr==trlast) trlast=tr->next;
DELETE_TRI(tr); next_iter=1;
} else if (delbuds==1) { // follow triangle fan around both shared edge ends
int bi,bi0,bi1,nfantris,fandir;
qhtritem *fantris[64],*tr1;
for(bi0=0; bi0<3 && !tr->buddy[bi0]->deleted; bi0++); // bi0 - deleted buddy index
for(fandir=-1; fandir<=1; fandir+=2) { // follow fans in 2 possible directions
tr1=tr; bi1=bi0; nfantris=0;
do {
bi=bi1+fandir; if (bi>2) bi-=3; if (bi<0) bi+=3;
for(bi1=0; bi1<3 && tr1->buddy[bi]->buddy[bi1]!=tr1; bi1++);
fantris[nfantris++] = tr1; // store this triangle in a temporary fan list
tr1=tr1->buddy[bi]; bi=bi1; // go to the next fan triangle
if (pts[ti]*tr1->n <= tr1->d-e) break; // discard this fan
if (tr1->deleted) {
if (tr1!=tr->buddy[bi0]) {
// delete fan only if it ended on _another_ deleted triangle
for(--nfantris;nfantris>=0;nfantris--) {
if (fantris[nfantris]==trlast) trlast=fantris[nfantris]->next;
DELETE_TRI(fantris[nfantris])
}
next_iter=1;
} break; // fan end
}
} while(true);
}
}
}
if (tr==trlast) break;
tr = trnext;
} while (tr);
} while (next_iter && trstart);
if (!trstart || trstart->deleted) {
#ifdef _DEBUG
//OutputDebugString("WARNING: convex hull not computed because all triangles were deleted (too thing or too small objects)\n");
#endif
n=0; goto endqhull;
}
// find triangles that shared an edge with deleted triangles
trnew=0; tr=trstart; do {
for(i=0;i<3;i++) if (tr->buddy[i]->deleted) {
// create a new triangle
if (trend>=trdata+trdatasz) {
qhtritem* trdata_new = new qhtritem[trdatasz+=256];
memcpy(trdata_new, trdata, (trend-trdata)*sizeof(qhtritem));
intptr_t diff = (intptr_t)trdata_new-(intptr_t)trdata;
for(n=0;n<trdatasz-256;n++)
relocate_tritem(trdata_new+n,diff);
relocate_ptritem(trend,diff); relocate_ptritem(trstart,diff);
relocate_ptritem(trnext,diff);relocate_ptritem(tr,diff);
relocate_ptritem(trbest,diff);relocate_ptritem(trnew,diff);
if (trdata!=trbuf) delete trdata;
trdata = trdata_new;
}
trend->idx[0] = ptmax-ptlist;
trend->idx[1] = tr->idx[i==2 ? 0:i+1];
trend->idx[2] = tr->idx[i];
trend->ptassoc = 0; trend->deleted = 0;
trend->n = pts[trend->idx[1]]-pts[trend->idx[0]] ^ pts[trend->idx[2]]-pts[trend->idx[0]];
trend->d = pts[trend->idx[0]]*trend->n;
trend->buddy[1] = tr; tr->buddy[i] = trend;
trend->buddy[0]=trend->buddy[2]=0;
add_item_to_list(trnew, trend++);
}
tr=tr->next;
} while (tr!=trstart);
// sort pointers to the new triangles by their 2nd vertex index
n=trend-trnew; if (tmparr_sz<n) {
if (tmparr_idx!=tmparr_idx_buf) delete tmparr_idx;
if (tmparr_ptr!=tmparr_ptr_buf) delete tmparr_ptr;
tmparr_idx = new int[n];
tmparr_ptr = new(qhtritem*[n]);
}
for(tr=trnew,i=0;tr<trend;tr++,i++) { tmparr_idx[i]=tr->idx[2]; tmparr_ptr[i]=tr; }
qsort(tmparr_idx,(void**)tmparr_ptr, 0,trend-trnew-1);
// find 0th buddy for each new triangle (i.e. the triangle, which has its idx[2]==tr->idx[1]
for(tr=trnew;tr<trend;tr++) {
i = bin_search(tmparr_idx,n, tr->idx[1]);
tr->buddy[0] = tmparr_ptr[i];
tmparr_ptr[i]->buddy[2] = tr;
}
for(tr=trnew;tr<trend;tr++)
if (!tr->buddy[0] || !tr->buddy[2]) {
#ifdef _DEBUG
//DEBUG_BREAK;
//OutputDebugString("WARNING: failed to compute convex hull due to geometric error\n");
#endif
goto endqh;
}
// assign all points from the deleted triangles to the new triangles
associate_ptlist_with_trilist(ptdeleted,trnew, ptlist,pts, e);
// add new triangles to the list
merge_lists(trnext, trnew);
} else if (trnext==trstart)
break; // all triangles in queue have no associated vertices
tr = trnext;
} while (++iter<maxiter);
#ifdef _DEBUG
//if (iter>=maxiter)
//OutputDebugString("WARNING: failed to compute convex hull during iterations limit\n");
#endif
endqh:
// build the final triangle list
for(tr=trstart,n=1; tr->next!=trstart; tr=tr->next,n++);
pTris = new index_t[n*3];
i=0; tr=trstart; do {
pTris[i]=tr->idx[0]; pTris[i+1]=tr->idx[1]; pTris[i+2]=tr->idx[2];
tr = tr->next; i+=3;
} while(tr!=trstart);
endqhull:
if (ptlist!=ptbuf) delete[] ptlist;
if (tmparr_idx!=tmparr_idx_buf) delete[] tmparr_idx;
if (tmparr_ptr!=tmparr_ptr_buf) delete[] tmparr_ptr;
if (trdata!=trbuf) delete[] trdata;
return n;
}
///////////////////////////// Qhull 2d ///////////////////////////////
inline void delete_item(ptitem2d *pitem, ptitem2d *&pref) {
pitem->next->prev = pitem->prev;
pitem->prev->next = pitem->next;
if (pref==pitem)
pref = pitem->next!=pitem ? pitem->next : 0;
}
inline void add_item(ptitem2d *pitem, ptitem2d *&pref) {
if (pref) {
pref->next->prev = pitem; pitem->next = pref->next;
pref->next = pitem; pitem->prev = pref;
} else {
pitem->next=pitem->prev = pitem;
pref = pitem;
}
}
int qhull2d(ptitem2d *pts,int nVtx, edgeitem *edges)
{
intptr_t imask;
int i,i0,i1,nEdges;
vector2df edge;
ptitem2d *ppt,*ppt_best,*ppt_next,*plist;
edgeitem *pedge,*pstart,*pend;
for(i=i0=0;i<nVtx;i++) { // find the 1st point - the one w/ max y
imask = -isneg(pts[i0].pt.y-pts[i].pt.y);
i0 = i&imask | i0&~imask;
}
// find the 2nd point - the one farthest from the 1st one
for(i1=0,i=1; i<nVtx; i++) {
imask = -isneg((pts[i1].pt-pts[i0].pt).len2()-(pts[i].pt-pts[i0].pt).len2());
i1 = i&imask | i1&~imask;
}
// form the 1st two edges
edges[0].prev=edges[0].next=edges+1; edges[0].pvtx=pts+i0; edges[0].plist=0;
edges[1].prev=edges[1].next=edges+0; edges[1].pvtx=pts+i1; edges[1].plist=0;
edge = pts[i1].pt-pts[i0].pt;
for(i=0; i<nVtx; i++) if ((i-i0)*(i-i1))
add_item(pts+i,edges[isneg(pts[i].pt-pts[i0].pt ^ edge)].plist);
nEdges = 2;
do {
for(i=0;i<nEdges && !edges[i].plist;i++);
if (i==nEdges)
break;
// find up the farthest point associated with the edge
edge = edges[i].next->pvtx->pt-edges[i].pvtx->pt; ppt=ppt_best = edges[i].plist;
do {
imask = -(intptr_t)isneg((ppt_best->pt-edges[i].pvtx->pt^edge) - (ppt->pt-edges[i].pvtx->pt^edge));
ppt_best = (ptitem2d*)((intptr_t)ppt&imask | (intptr_t)ppt_best&~imask);
} while((ppt=ppt->next)!=edges[i].plist);
// trace contour from i cw while edges are facing the point ppt_best (pstart - 1st edge to be deleted)
for(pstart=edges+i; pstart->prev!=edges+i &&
(ppt_best->pt-pstart->prev->pvtx->pt ^ pstart->pvtx->pt-pstart->prev->pvtx->pt)>0; pstart=pstart->prev);
// trace contour from i ccw while edges are facing the point ppt_best (pend - edge after the last edge to be deleted)
for(pend=edges[i].next; pend!=edges+i &&
(ppt_best->pt-pend->pvtx->pt ^ pend->next->pvtx->pt-pend->pvtx->pt)>0; pend=pend->next);
// delete point ppt_best from the ith edge associated list
delete_item(ppt_best,edges[i].plist);
// merge point lists for edges pstart-pend
for(pedge=pstart,plist=0; pedge!=pend && !pedge->plist; pedge=pedge->next);
if (pedge!=pend) for(plist=pedge->plist,pedge->plist=0,pedge=pedge->next; pedge!=pend; pedge=pedge->next) if (pedge->plist) {
plist->next->prev = pedge->plist->prev; pedge->plist->prev->next = plist->next;
plist->next = pedge->plist; pedge->plist->prev = plist;
pedge->plist = 0;
}
// create a new edge between pstart and pend (first one will override pstart)
pstart->next=edges+nEdges; edges[nEdges].prev=pstart;
pend->prev = edges+nEdges; edges[nEdges].next=pend;
edges[nEdges].pvtx = ppt_best;
pstart->plist = edges[nEdges].plist = 0;
// associate some points from that list with one of the 2 new edges
if (plist) {
edgeitem *pedge[2]; vector2df edge[2]; float dist[2];
ppt = plist; pedge[0] = pstart; pedge[1] = edges+nEdges;
edge[0] = pedge[0]->next->pvtx->pt-pedge[0]->pvtx->pt;
edge[1] = pedge[1]->next->pvtx->pt-pedge[1]->pvtx->pt;
do {
ppt_next = ppt->next;
dist[0] = (ppt->pt-pedge[0]->pvtx->pt ^ edge[0])*edge[1].len2();
dist[1] = (ppt->pt-pedge[1]->pvtx->pt ^ edge[1])*edge[0].len2();
i = isneg(dist[0]-dist[1]);
if (dist[i]>0)
add_item(ppt,pedge[i]->plist);
} while((ppt=ppt_next)!=plist);
}
nEdges++;
} while(true);
return nEdges;
}
Reference in New Issue View Git Blame Copy Permalink