269 lines
11 KiB
C++
269 lines
11 KiB
C++
#include "stdafx.h"
|
|
|
|
#include "utils.h"
|
|
|
|
vector2df g_BoolPtBuf[4096];
|
|
int g_BoolIdBuf[4096];
|
|
int g_BoolGrid[4096];
|
|
unsigned int g_BoolHash[8192];
|
|
struct inters2d {
|
|
vector2df pt;
|
|
int iedge[2];
|
|
};
|
|
inters2d g_BoolInters[256];
|
|
|
|
inline int line_seg_inters(const vector2df *seg0, const vector2df *seg1, vector2df *ptres)
|
|
{
|
|
float denom,t0,t1,sg;
|
|
vector2df dp0,dp1,ds;
|
|
dp0 = seg0[1]-seg0[0]; dp1 = seg1[1]-seg1[0]; ds = seg1[0]-seg0[0];
|
|
denom = dp0^dp1;
|
|
|
|
if (sqr(denom) > 1E-6f*dp0.len2()*dp1.len2()) { // stable intersection
|
|
t0 = ds^dp1; t1 = ds^dp0;
|
|
sg = sgnnz(denom); denom*=sg; t0*=sg; t1*=sg;
|
|
if (isneg(fabs_tpl(t0*2-denom)-denom) & isneg(fabs_tpl(t1*2-denom)-denom)) {
|
|
ptres[0] = seg0[0] + dp0*(t0/denom); return 1;
|
|
} else return 0;
|
|
}
|
|
|
|
if (sqr(ds^dp0) < 1E-6f*ds.len2()*dp0.len2()) { // segments are [almost] parallel and touching
|
|
float t[2][2]; const vector2df *ppt[2][2]; int idir;
|
|
t[0][0]=0; t[0][1]=dp0.len2(); ppt[0][0]=seg0; ppt[0][1]=seg0+1;
|
|
t0=(seg1[0]-seg0[0])*dp0; t1=(seg1[1]-seg0[0])*dp0;
|
|
idir = isneg(t1-t0); t[1][idir]=t0; t[1][idir^1]=t0; ppt[1][idir]=seg1; ppt[1][idir^1]=seg1+1;
|
|
if (max(t[0][0],t[1][0])>min(t[0][1],t[1][1]))
|
|
return 0;
|
|
ptres[0] = *ppt[0][isneg(t[0][0]-t[1][0])];
|
|
ptres[1] = *ppt[1][isneg(t[1][1]-t[0][1])];
|
|
return 2;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
inline vector2di& get_cell(const vector2df &pt, const vector2df &rstep, vector2di &ipt)
|
|
{
|
|
ipt.set(float2int(pt.x*rstep.x-0.5f),float2int(pt.y*rstep.y-0.5f));
|
|
return ipt;
|
|
}
|
|
|
|
inline void get_rect(const vector2di &ipt0,const vector2di &ipt1,vector2di *irect,const vector2di &isz)
|
|
{
|
|
irect[0].x = max(0,min(ipt0.x,ipt1.x)); irect[0].y = min(isz.y,max(0,min(ipt0.y,ipt1.y)));
|
|
irect[1].x = min(isz.x-1,max(ipt0.x,ipt1.x)); irect[1].y = min(isz.y,max(ipt0.y,ipt1.y));
|
|
}
|
|
|
|
inline int check_if_inside(int iobj, vector2di &ipt,const vector2di &isz,const vector2df *ptsrc, const vector2df &pt)
|
|
{
|
|
int bInside,i,bStop=0;
|
|
vector2df pt0,pt1,dp;
|
|
quotientf ycur;
|
|
if ((unsigned int)ipt.x>=(unsigned int)isz.x || ipt.y<0)
|
|
return 0;
|
|
|
|
for(bInside=0; ipt.y<=isz.y && !bStop; ipt.y++)
|
|
for(i=g_BoolGrid[ipt.y*isz.x+ipt.x]; i<g_BoolGrid[ipt.y*isz.x+ipt.x+1]; i++) if (g_BoolHash[i]>>31^iobj) {
|
|
pt0 = ptsrc[g_BoolHash[i]&0x7FFFFFFF]; pt1 = ptsrc[(g_BoolHash[i]&0x7FFFFFFF)+1]; dp = pt1-pt0;
|
|
ycur.set((dp^pt0)+pt.x*dp.y, dp.x).fixsign();
|
|
if (isneg(fabs_tpl((pt0.x+pt1.x)-pt.x*2)-fabs_tpl(pt0.x-pt1.x)) & isneg(pt.y-ycur))
|
|
{ bInside -= sgn(dp.x); bStop=1; }
|
|
}
|
|
return isneg(-bInside);
|
|
}
|
|
|
|
int boolean2d(booltype type, vector2df *ptbuf1,int npt1, vector2df *ptbuf2,int npt2,int bClosed, vector2df *&ptres,int *&pidres)
|
|
{
|
|
vector2df *ptsrc[2] = { ptbuf1,ptbuf2 };
|
|
int npt[2] = { npt1,npt2 };
|
|
vector2df dp,dp1,ptmin[2],ptmax[2],sz,ptbox[2],ptint[2],pttest,rstep;
|
|
int iobj,i,j,idx,nsz,n,npttmp,nptres,ninters=0,istart,ix,iy,bInside,bPrevInside,idx_next,idx_prev,inext,inext1,iobj1;
|
|
vector2di ipt0,ipt1,irect[2],isz;
|
|
float t,ratioxy,ratioyx;
|
|
|
|
static int __bforce1=0,__bforce2=0;
|
|
//for(i=0;i<max(npt1,npt2);i++) g_BoolIdBuf[i]=0;
|
|
|
|
pidres = g_BoolIdBuf;
|
|
if (npt1<3 || __bforce2)
|
|
{ ptres = ptbuf2; return npt2&-(bClosed|__bforce2); }
|
|
if (npt2<2+bClosed || __bforce1)
|
|
{ ptres = ptbuf1; return npt1&-(bClosed|__bforce1); }
|
|
|
|
for(iobj=0;iobj<2;iobj++) {
|
|
ptmin[iobj] = ptmax[iobj] = ptsrc[iobj][0];
|
|
for(i=1;i<npt[iobj];i++) {
|
|
ptmin[iobj].x = min(ptmin[iobj].x,ptsrc[iobj][i].x); ptmin[iobj].y = min(ptmin[iobj].y,ptsrc[iobj][i].y);
|
|
ptmax[iobj].x = max(ptmax[iobj].x,ptsrc[iobj][i].x); ptmax[iobj].y = max(ptmax[iobj].y,ptsrc[iobj][i].y);
|
|
}
|
|
}
|
|
ptbox[0].x = max(ptmin[0].x,ptmin[1].x); ptbox[0].y = max(ptmin[0].y,ptmin[1].y);
|
|
ptbox[1].x = min(ptmax[0].x,ptmax[1].x); ptbox[1].y = min(ptmax[0].y,ptmax[1].y);
|
|
sz = ptbox[1]-ptbox[0];
|
|
sz.x += fabs_tpl(sz.y)*1E-5f; sz.y += fabs_tpl(sz.x)*1E-5f;
|
|
if (sz.x<fabs_tpl(sz.y)*5E-6f || sz.y<fabs_tpl(sz.x)*5E-6f)
|
|
return 0;
|
|
ptbox[0] -= sz*0.01f; ptbox[1] += sz*0.01f; sz = ptbox[1]-ptbox[0];
|
|
sz.x += fabs_tpl(sz.y)*1E-5f; sz.y += fabs_tpl(sz.x)*1E-5f;
|
|
|
|
i = (int)(sizeof(g_BoolGrid)/sizeof(g_BoolGrid[0])-1);
|
|
npttmp = min(npt[0]+npt[1]<<1, i);
|
|
ratioyx = max(min(sz.y/sz.x,(float)npttmp),1.0f);
|
|
ratioxy = max(min(sz.x/sz.y,(float)npttmp),1.0f);
|
|
isz.y = max(1,float2int(cry_sqrtf(npttmp*4*ratioyx+1)*0.5f-1.0f));
|
|
isz.x = max(1,float2int(isz.y*ratioxy-0.5f));
|
|
if (isz.x*(isz.y+1)>i) {
|
|
isz.y = min(isz.y,i); isz.x = i/(isz.y+1);
|
|
}
|
|
rstep.set(isz.x/sz.x, isz.y/sz.y);
|
|
nsz = isz.x*(isz.y+1);
|
|
if (nsz>=sizeof(g_BoolGrid)/sizeof(g_BoolGrid[0])-1)
|
|
return 0;
|
|
npt[1] -= bClosed^1;
|
|
|
|
for(i=0;i<=nsz;i++) g_BoolGrid[i] = 0;
|
|
|
|
for(iobj=0;iobj<2;iobj++) { // calculate number of elements in each cell
|
|
for(get_cell(ptsrc[iobj][i=0]-ptbox[0],rstep,ipt0); i<npt[iobj]; i++) {
|
|
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
|
|
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
|
|
g_BoolGrid[iy*isz.x+ix]++;
|
|
ipt0 = ipt1;
|
|
}
|
|
}
|
|
|
|
for(i=1;i<=nsz;i++) g_BoolGrid[i]+=g_BoolGrid[i-1];
|
|
if (g_BoolGrid[nsz-1]>(int)(sizeof(g_BoolHash)/sizeof(g_BoolHash[0])))
|
|
return 0;
|
|
|
|
for(iobj=0;iobj<2;iobj++) { // put each line segment into the corresponding hash cell(s)
|
|
for(get_cell(ptsrc[iobj][i=0]-ptbox[0],rstep,ipt0); i<npt[iobj]; i++) {
|
|
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
|
|
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
|
|
g_BoolHash[--g_BoolGrid[iy*isz.x+ix]] = iobj<<31 | i;
|
|
ipt0 = ipt1;
|
|
}
|
|
}
|
|
|
|
// select iobj which is likely to have shorter edges
|
|
if (bClosed)
|
|
iobj = isneg((ptmax[1].x-ptmin[1].x+ptmax[1].y-ptmin[1].y)*npt[0] -
|
|
(ptmax[0].x-ptmin[0].x+ptmax[0].y-ptmin[0].y)*npt[1]);
|
|
else iobj = 1;
|
|
|
|
// build list of intersections by traversing iobj
|
|
if (!bClosed) {
|
|
g_BoolInters[0].pt = ptsrc[1][0];
|
|
g_BoolInters[0].iedge[0] = -1; g_BoolInters[0].iedge[1] = 0;
|
|
ninters = 1;
|
|
}
|
|
for(get_cell(ptsrc[iobj][0]-ptbox[0],rstep,ipt0),i=0; i<npt[iobj]; i++) {
|
|
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
|
|
istart = ninters;
|
|
|
|
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
|
|
for(j=g_BoolGrid[iy*isz.x+ix]; j<g_BoolGrid[iy*isz.x+ix+1]; j++) if (g_BoolHash[j]>>31^iobj) {
|
|
for(n=line_seg_inters(ptsrc[iobj]+i, ptsrc[iobj^1]+(g_BoolHash[j]&0x7FFFFFFF), ptint)-1; n>=0; n--) {
|
|
dp = ptsrc[iobj][i+1]-ptsrc[iobj][i]; t = (ptint[n]-ptsrc[iobj][i])*dp;
|
|
for(idx=istart; idx<ninters && fabs_tpl((g_BoolInters[idx].pt-ptsrc[iobj][i])*dp-t)>t*1E-7f; idx++);
|
|
if (idx<ninters)
|
|
continue; // ignore possible intersections with the same line found in different cell
|
|
if (ninters==sizeof(g_BoolInters)/sizeof(g_BoolInters[0])-1)
|
|
return 0;
|
|
for(idx=ninters-1; idx>=istart && (g_BoolInters[idx].pt-ptsrc[iobj][i])*dp>t; idx--)
|
|
g_BoolInters[idx+1] = g_BoolInters[idx];
|
|
g_BoolInters[idx+1].pt = ptint[n];
|
|
g_BoolInters[idx+1].iedge[iobj] = i;
|
|
g_BoolInters[idx+1].iedge[iobj^1] = g_BoolHash[j]&0x7FFFFFFF;
|
|
ninters++;
|
|
}
|
|
}
|
|
ipt0 = ipt1;
|
|
}
|
|
if (!bClosed) {
|
|
if (ninters==sizeof(g_BoolInters)/sizeof(g_BoolInters[0])-1)
|
|
return 0;
|
|
g_BoolInters[ninters].pt = ptsrc[1][npt[1]];
|
|
g_BoolInters[ninters].iedge[0] = -1; g_BoolInters[ninters].iedge[1] = npt[1];
|
|
g_BoolInters[ninters+1] = g_BoolInters[ninters]; ninters++;
|
|
} else
|
|
g_BoolInters[ninters] = g_BoolInters[0];
|
|
|
|
nptres = 0; ptres = g_BoolPtBuf;
|
|
// if there were no intersections, return the object that is inside the other
|
|
if (ninters-(bClosed^1)*2==0) {
|
|
get_cell(ptsrc[iobj][0]-ptbox[0],rstep,ipt0);
|
|
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],ptsrc[iobj][0]);
|
|
npt[1] += bClosed^1;
|
|
if (bClosed) // assume that objects cannot have empty intersection area, thus if iobj is not inside iobj^1, ibj1^1 should be inside iobj
|
|
iobj ^= bInside^1;
|
|
ptres = ptsrc[iobj];
|
|
for(nptres=0;nptres<npt[iobj];nptres++)
|
|
g_BoolIdBuf[nptres] = nptres+1<<iobj*16;
|
|
return nptres & -(bClosed|bInside);
|
|
}
|
|
npt[1] += bClosed^1; // compensate for 1 subtracted earlier
|
|
|
|
// build boolean intersection by at each intersection point selecting the stripe that is more 'inward' than the other one
|
|
for(idx=bPrevInside=0; idx<ninters; idx++) {
|
|
idx_next = idx+1; //& ~(ninters-2-idx>>31);
|
|
idx_prev = idx-1; idx_prev = idx_prev&~(idx_prev>>31) | ninters-1&idx_prev>>31;
|
|
i = g_BoolInters[idx].iedge[iobj]; inext = i+1 & i+1-npt[iobj]>>31;
|
|
dp = ptsrc[iobj][inext]-ptsrc[iobj][i];
|
|
inext1 = min(inext+1,npt[iobj]-1) & (i+1-npt[iobj]>>31 | ~-bClosed);
|
|
|
|
j = g_BoolInters[idx].iedge[iobj^1];
|
|
if (j>=0) {
|
|
dp1 = ptsrc[iobj^1][j+1 & j+1-npt[iobj^1]>>31]-ptsrc[iobj^1][j];
|
|
bInside = isneg(dp^dp1);
|
|
} else {
|
|
pttest = g_BoolInters[idx].pt;
|
|
get_cell(pttest-ptbox[0],rstep,ipt0);
|
|
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],pttest);
|
|
}
|
|
/*
|
|
// build boolean intersection by selecting fragments (stripes between 2 intersection points) of the object that is inside another object
|
|
// cannot just switch "inside" state at each intersection, since it's less stable in case of degenerate contacts
|
|
// find some representative point for this fragment
|
|
if (g_BoolInters[idx_next].iedge[iobj]==i && (g_BoolInters[idx_next].pt-ptsrc[iobj][i])*dp>(g_BoolInters[idx].pt-ptsrc[iobj][i])*dp)
|
|
pttest = (g_BoolInters[idx].pt+g_BoolInters[idx_next].pt)*0.5f; // next intersection lies on the same edge
|
|
else if ((g_BoolInters[idx].pt-ptsrc[iobj][i])*dp < dp.len2()*0.95f*0.95f)
|
|
pttest = (g_BoolInters[idx].pt+ptsrc[iobj][inext1])*0.5f; // this intersection is not too close to the edge end
|
|
else if (g_BoolInters[idx_next].iedge[iobj]==inext)
|
|
pttest = (ptsrc[iobj][inext]+g_BoolInters[idx_next].pt)*0.5f; // next intersection lies on the next edge
|
|
else
|
|
pttest = (ptsrc[iobj][inext]+ptsrc[iobj][inext1])*0.5f;
|
|
|
|
// check if a ray upwards from pttest first hits right-to-left edge of iobj^1, in this case we are inside
|
|
get_cell(pttest-ptbox[0],rstep,ipt0);
|
|
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],pttest);*/
|
|
|
|
iobj1 = iobj^bInside^1;
|
|
if (bInside | bPrevInside | bClosed) {
|
|
g_BoolPtBuf[nptres] = g_BoolInters[idx].pt;
|
|
g_BoolIdBuf[nptres++] = g_BoolInters[idx].iedge[1]+1<<16 | g_BoolInters[idx].iedge[0]+1;
|
|
if (nptres>=(int)(sizeof(g_BoolPtBuf)/sizeof(g_BoolPtBuf[0])))
|
|
return nptres;
|
|
}
|
|
if (bInside | bClosed) {
|
|
i = g_BoolInters[idx].iedge[iobj1];
|
|
inext = i+1 & ~(npt[iobj1]-2-i>>31);
|
|
dp = ptsrc[iobj1][inext]-ptsrc[iobj1][i];
|
|
bool bForceFirstStep = i==g_BoolInters[idx_next].iedge[iobj1] &&
|
|
(g_BoolInters[idx].pt-ptsrc[iobj1][i])*dp > (g_BoolInters[idx_next].pt-ptsrc[iobj1][i])*dp;
|
|
for(; bForceFirstStep ||
|
|
i!=g_BoolInters[idx_next].iedge[iobj1] && (iobj1==iobj || i!=g_BoolInters[idx_prev].iedge[iobj1]);
|
|
i=i+1&~(npt[iobj1]-2-i>>31))
|
|
{
|
|
g_BoolPtBuf[nptres] = ptsrc[iobj1][i+1];
|
|
g_BoolIdBuf[nptres++] = i+2<<iobj1*16;
|
|
if (nptres>=(int)(sizeof(g_BoolPtBuf)/sizeof(g_BoolPtBuf[0])))
|
|
return nptres;
|
|
bForceFirstStep = false;
|
|
}
|
|
}
|
|
bPrevInside = bInside;
|
|
}
|
|
|
|
return nptres;
|
|
}
|