#include "stdafx.h" #include "utils.h" #include "primitives.h" #include "intersectionchecks.h" CIntersectionChecker g_Intersector; CIntersectionChecker::CIntersectionChecker() { for(int i=0;in^ptri2->n; axes[0] = ptri2->n^axes[2]; axes[1] = ptri1->n^axes[2]; matrix3x3RM &mtx((matrix3x3RM&)axes[0]); real nlen2 = axes[2].len2(), ptw; vectorr ptloc, pt0=vectorr(ptri1->pt[0]*ptri1->n,-ptri2->pt[0]*ptri2->n,ptri1->pt[0]*axes[2])*mtx; vector2d pt2d[2][3]; int i, sgnx[2][3]; for(i=0;i<3;i++) { ptloc=ptri1->pt[i]*nlen2-pt0; pt2d[0][i].set(axes[1]*ptloc,axes[2]*ptloc); ptloc=ptri2->pt[i]*nlen2-pt0; pt2d[1][i].set(axes[0]*ptloc,axes[2]*ptloc); sgnx[0][i] = sgnnz(pt2d[0][i].x); sgnx[1][i] = sgnnz(pt2d[1][i].x); } quotient t[2],tc[2],tr[2],tmin[2],tmax[2]; int imin,imax,iEdge[2][2], nCross,iNotCross,bCross; for(i=0;i<2;i++) { bCross = sgnx[i][0]*sgnx[i][1]-1>>1; nCross = -bCross; iNotCross = 0; bCross = sgnx[i][1]*sgnx[i][2]-1>>1; nCross -= bCross; iNotCross |= 1&~bCross; bCross = sgnx[i][2]*sgnx[i][0]-1>>1; nCross -= bCross; (iNotCross&=bCross) |= 2&~bCross; if (nCross!=2) return 0; t[0].x = pt2d[i][dec_mod3[iNotCross]] ^ pt2d[i][inc_mod3[iNotCross]]; t[0].y = pt2d[i][dec_mod3[iNotCross]].x - pt2d[i][inc_mod3[iNotCross]].x; t[1].x = pt2d[i][iNotCross] ^ pt2d[i][dec_mod3[iNotCross]]; t[1].y = pt2d[i][iNotCross].x - pt2d[i][dec_mod3[iNotCross]].x; t[0].fixsign(); t[1].fixsign(); imin = isneg(t[1].x*t[0].y-t[0].x*t[1].y); iEdge[i][imin] = inc_mod3[iNotCross]; iEdge[i][imin^1] = dec_mod3[iNotCross]; tmin[i] = t[imin]; tmax[i] = t[imin^1]; tc[i] = (t[0]+t[1])*(real)0.5; tr[i] = fabs_tpl(t[0]-t[1])*(real)0.5; } if (fabs_tpl((tc[0]-tc[1]).x) > (tr[0]+tr[1]).x) return 0; imax = (tmin[0]-tmin[1]).isneg(); // start_t = max(min1,min2); imin = (tmax[1]-tmax[0]).isneg(); // end_t = min(max1,max2) // 1 division scheme - less accurate ptw = nlen2*nlen2*tmin[imax].y*tmax[imin].y; if (ptw==0) return 0; ptw = (real)1.0/ptw; pt0 *= nlen2*tmin[imax].y*tmax[imin].y; pinters->pt[0] = (pt0 + axes[2]*(tmin[imax].x*tmax[imin].y))*ptw; pinters->pt[1] = (pt0 + axes[2]*(tmax[imin].x*tmin[imax].y))*ptw; //nlen2 = (real)1.0/nlen2; pt0 *= nlen2; //pinters->pt[0] = pt0 + axes[2]*(nlen2*nlen2*tmin[imax].val()); //pinters->pt[1] = pt0 + axes[2]*(nlen2*nlen2*tmax[imin].val()); pinters->iFeature[0][imax] = iEdge[imax][0] | 0xA0; pinters->iFeature[0][imax^1] = 0x40; pinters->iFeature[1][imin] = iEdge[imin][1] | 0xA0; pinters->iFeature[1][imin^1] = 0x40; return 1; } int tri_box_intersection(const triangle *ptri, const box *pbox, prim_inters *pinters) { vectorr dp,dp1,pt[3],n,ptbox; quotient t[3][2],t1[2],dist,mindist; int i,j,idir,iStart,iStartBest,iEndBest,bOutside[3],bBest,bEnd,bSwitch,iEndCur,bState; real sg; vectori axsg; pt[0] = pbox->Basis*(ptri->pt[0]-pbox->center); pt[1] = pbox->Basis*(ptri->pt[1]-pbox->center); pt[2] = pbox->Basis*(ptri->pt[2]-pbox->center); n = pbox->Basis*ptri->n; for(i=0,iStart=-1; i<3; i++) { bOutside[i] = isneg(pbox->size.x-fabs_tpl(pt[i].x)) | isneg(pbox->size.y-fabs_tpl(pt[i].y)) | isneg(pbox->size.z-fabs_tpl(pt[i].z)); (iStart&=~-bOutside[i]) |= i&-bOutside[i]; } if (iStart<0) // all triangle points are inside the box return 0; if (bOutside[0]+bOutside[1]+bOutside[2]<3) { // if at least one triangle point is inside the box, use the closest box face normal as intersection normal i = bOutside[0]+(bOutside[0]&bOutside[1]); // i - index of the 1st inside vertex pinters->ptbest = ptri->pt[i]; dp(fabs_tpl(fabs_tpl(pt[i].x)-pbox->size.x), fabs_tpl(fabs_tpl(pt[i].y)-pbox->size.y), fabs_tpl(fabs_tpl(pt[i].z)-pbox->size.z)); j = idxmin3((real*)&dp); // j - the closest box face pinters->n = pbox->Basis.GetRow(j)*-sgnnz(pt[i][j]); pinters->nBestPtVal = 6; } i=iStart; iEndCur=iStartBest=iEndBest=-1; bState=0; mindist.set(1,0); do { dp = pt[dec_mod3[i]]-pt[i]; t[i][0].set(0,1); t[i][1].set(1,1); for(j=0;j<3;j++) { // crop edge with box sides (plane sandwiches) idir = isneg(dp[j]); sg = 1-(idir<<1); t1[idir].set((-pbox->size[j]-pt[i][j])*sg, dp[j]*sg); t1[idir^1].set((pbox->size[j]-pt[i][j])*sg,dp[j]*sg); t[i][0] = max(t[i][0],t1[0]); t[i][1] = min(t[i][1],t1[1]); } if (t[i][1]>t[i][0]) // edge intersects the box for(j=0;j<2;j++) { //bVertexContact = isneg(sqr(fabs_tpl(t[i][j].x*2-t[i][j].y)-t[i][j].y)*dplen2 - pinters->minPtDist2*sqr(t[i][j].y)*4); bSwitch = t[i][j].isin01(); bEnd = bSwitch & bState; // remember starting (ccw) point of every stripe (it'll serve as end point in result) (iEndCur&=~-bEnd) |= (i<<1|j)&-bEnd; // if a point is a stripe start, check if it is the current closest to the suggested stripe start dp1 = (ptri->pt[i]-pinters->pt[1]); dist.set((dp1*t[i][j].y+(ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][j].x).len2(), sqr(t[i][j].y)); bBest = isneg(dist-mindist) & bSwitch & (bState^1); (mindist.x*=1-bBest) += dist.x*bBest; (mindist.y*=1-bBest) += dist.y*bBest; (iStartBest&=~-bBest) |= (i<<1|j)&-bBest; (iEndBest&=~-bBest) |= iEndCur&-bBest; bState ^= bSwitch; } } while((i=dec_mod3[i])!=iStart); if (iStartBest<0) { // triangle edges do not intersect box // find the box vertices that have the min. and max. (signed) distance to place // check if the line between them intersects the triangle vectorr ptmin(pbox->size.x*-sgnnz(n.x),pbox->size.y*-sgnnz(n.y),pbox->size.z*-sgnnz(n.z)); dp = ptmin*-2; t1[0].set((pt[0]-ptmin)*n,dp*n); bBest = t1[0].isin01(); ptmin = ptmin*t1[0].y + dp*t1[0].x; for(i=0; i<3; i++) // check if this point lies in triangle's voronoi region bBest &= isneg((ptmin-pt[i]*t1[0].y ^ pt[inc_mod3[i]]-pt[i])*n); if (!bBest) return 0; pinters->pt[0] = pinters->pt[1] = (ptmin/t1[0].y)*pbox->Basis+pbox->center; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; pinters->ptbest = pinters->pt[0]; pinters->n = ptri->n; pinters->nBestPtVal = 1000; goto haveinters; } iEndBest = iEndBest-(iEndBest>>31) | iEndCur&iEndBest>>31; if (iEndBest<0) return 0; i = iStartBest>>1; pinters->pt[0] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iStartBest&1].val(); pinters->iFeature[0][0] = 0xA0 | dec_mod3[i]; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; i = iEndBest>>1; pinters->pt[1] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iEndBest&1].val(); pinters->iFeature[1][0] = 0xA0 | dec_mod3[i]; haveinters: iStart = idxmax3((const real*)&n.abs()); // if triangle's area is comparable with the most parallel box face, check box edges vs triangle also if ((pt[1]-pt[0]^pt[2]-pt[0]).len2() > sqr(pbox->size[inc_mod3[iStart]]*pbox->size[dec_mod3[iStart]])) { int nborderpt0 = pinters->nborderpt; for(i=0;i<12;i++) { idir = i>>2; axsg[idir]=-1; axsg[inc_mod3[idir]]=((i&1)<<1)-1; axsg[dec_mod3[idir]]=(i&2)-1; ptbox(pbox->size.x*axsg.x, pbox->size.y*axsg.y, pbox->size.z*axsg.z); t1[0].set((pt[0]-ptbox)*n,n[idir]); if (inrange(t1[0].x,(real)0,pbox->size[idir]*t1[0].y*2)) { t1[0].fixsign(); (ptbox*=t1[0].y)[idir] += t1[0].x; for(j=0,bSwitch=1;j<3;j++) bSwitch &= isneg((ptbox-pt[j]*t1[0].y ^ pt[inc_mod3[j]]-pt[j])*n); if (bSwitch & isneg(pinters->nborderpt-pinters->nbordersz)) pinters->ptborder[pinters->nborderpt++] = (ptbox/t1[0].y)*pbox->Basis+pbox->center; } } // only mark this triangle's normal as intersection normal if the box has at least 2 edge intersections // with it, and the box center is in triangle's Voronoi region if (fabs_tpl(n[iStart])>0.98f && pinters->nborderpt>nborderpt0+1 && pinters->nBestPtVal<6) { ptbox.zero()[iStart] = pbox->size[iStart]*sgnnz(pt[0][iStart]); if (isneg((ptbox-pt[0]^pt[1]-pt[0])*n) & isneg((ptbox-pt[1]^pt[2]-pt[1])*n) & isneg((ptbox-pt[2]^pt[0]-pt[2])*n)) { pinters->nBestPtVal = 6; pinters->n = ptri->n; pinters->ptbest = pinters->ptborder[nborderpt0]; } } } return 1; } int box_tri_intersection(const box *pbox, const triangle *ptri, prim_inters *pinters) { int res = tri_box_intersection(ptri,pbox,pinters); vectorf pt; int iFeature; pt=pinters->pt[0]; pinters->pt[0]=pinters->pt[1]; pinters->pt[1]=pt; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; pinters->n.Flip(); return res; } int tri_cylinder_intersection(const triangle *ptri, const cylinder *pcyl, prim_inters *pinters) { vectorr pc,dp,dp1,vec1,vec0; vectorf pt,axisx,axisy; vector2df pt2d; real r2,a,b,c,d; quotient t[3][2],dist,mindist; int i,j,iStart,iStartBest,iEndBest,bOutside,bEnd,iEndCur,bSwitch,bState,bBest,sg,bInters;//,bVertexContact,bPrevVertexContact; r2 = sqr(pcyl->r); for(i=0,iStart=-1; i<3; i++) { pc = ptri->pt[i]-pcyl->center; bOutside = isneg(r2 - (pc^pcyl->axis).len2()) | isneg(pcyl->hh - fabs_tpl(pc*pcyl->axis)); (iStart&=~-bOutside) |= i&-bOutside; } if (iStart<0) // all triangle points are inside the cylinder return 0; i=iStart; iEndCur=iStartBest=iEndBest=-1; bState=0; mindist.set(1,0); do { // find edge-cylinder intersections dp = ptri->pt[dec_mod3[i]]-ptri->pt[i]; pc = ptri->pt[i]-pcyl->center; t[i][0].x = t[i][1].x = -pc*pcyl->axis; t[i][0].y = t[i][1].y = dp*pcyl->axis; sg = sgnnz(t[i][0].y); t[i][sg+1>>1].x += pcyl->hh; t[i][sg+1>>1^1].x -= pcyl->hh; t[i][0].x*=sg; t[i][0].y*=sg; t[i][1].x*=sg; t[i][1].y*=sg; vec1 = dp^pcyl->axis; vec0 = pc^pcyl->axis; a = vec1*vec1; b = vec0*vec1; c = vec0*vec0-r2; d = b*b-a*c; if (d>=0 && vec1.len2()>1E-10) { d = sqrt_tpl(d); t[i][0] = max(t[i][0],quotient(-b-d,a)); t[i][1] = min(t[i][1],quotient(-b+d,a)); bInters = isneg(t[i][0]-t[i][1]); } else bInters = isneg(vec0.len2()-r2); if (bInters) for(j=0;j<2;j++) { //bVertexContact = isneg(sqr(t[i][j].x-t[i][j].y)*dplen2-pinters->minPtDist2*sqr(t[i][j].y)); // t is close to 1 //bPrevVertexContact = isneg(sqr(t[i][j].x)*dplen2-pinters->minPtDist2*sqr(t[i][j].y)); // t is close to 0 bSwitch = isneg(fabs_tpl(t[i][j].x*2-t[i][j].y)-t[i][j].y); // t is in (0..1) bEnd = bSwitch & bState; // remember starting (ccw) point of every stripe (it'll serve as end point in result) (iEndCur&=~-bEnd) |= (i<<1|j)&-bEnd; // if a point is a stripe start, check if it is the current closest to the suggested stripe start dp1 = (ptri->pt[i]-pinters->pt[1]); dist.set((dp1*t[i][j].y+(ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][j].x).len2(), sqr(t[i][j].y)); bBest = isneg(dist-mindist) & bSwitch & (bState^1); (mindist.x*=1-bBest) += dist.x*bBest; (mindist.y*=1-bBest) += dist.y*bBest; (iStartBest&=~-bBest) |= (i<<1|j)&-bBest; (iEndBest&=~-bBest) |= iEndCur&-bBest; bState ^= bSwitch; } } while((i=dec_mod3[i])!=iStart); if (iStartBest<0) { // triangle edges do not intersect cylinder vectorr ptmin,ptmax; ptmax = ptri->n-pcyl->axis*(ptri->n*pcyl->axis); if (ptmax.len2()>1E-4f) ptmax.normalize(); else ptmax.zero(); // choose the points on cylinder caps that have the maximum and the minimum projections on triangle normal ptmax = ptmax*pcyl->r + pcyl->axis*pcyl->hh*(j=sgnnz(pcyl->axis*ptri->n)); ptmin = pcyl->center - ptmax; ptmax += pcyl->center; pt = ptmin; // check if a line between them intersects triangle t[0][0].set((ptri->pt[0]-ptmin)*ptri->n, (ptmax-ptmin)*ptri->n); bInters = isneg(fabs_tpl(t[0][0].x*2-t[0][0].y)-t[0][0].y); // t is in (0..1) ptmin = ptmin*t[0][0].y + (ptmax-ptmin)*t[0][0].x; for(i=0; i<3; i++) // check if this point lies in triangle's voronoi region bInters &= isneg((ptmin-ptri->pt[i]*t[0][0].y ^ ptri->pt[inc_mod3[i]]-ptri->pt[i])*ptri->n); if (!bInters) return 0; pinters->pt[0] = pinters->pt[1] = ptmin/t[0][0].y; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; pinters->ptbest = pinters->pt[0]; pinters->n = ptri->n; // force triangle normal as global intersection normal in this case pinters->nBestPtVal = 1000; // check the bottommost cylinder edge (wrt triangle normal) if (inrange((pt-ptri->pt[0])*ptri->n, (pcyl->hh+pcyl->r)*-0.2f,0.0f) && (pt-ptri->pt[0]^ptri->pt[1]-ptri->pt[0])*ptri->n<0 && (pt-ptri->pt[1]^ptri->pt[2]-ptri->pt[1])*ptri->n<0 && (pt-ptri->pt[2]^ptri->pt[0]-ptri->pt[2])*ptri->n<0 && pinters->nborderptnbordersz) pinters->ptborder[pinters->nborderpt++] = pt; dist.set((ptri->pt[0]-pt)*ptri->n, fabs_tpl(pcyl->axis*ptri->n)); if (inrange(dist.x, (real)0,pcyl->hh*2*dist.y)) pt += pcyl->axis*(j*dist.val()); else pt += pcyl->axis*(pcyl->hh*2*j); if (inrange((pt-ptri->pt[0])*ptri->n, (pcyl->hh+pcyl->r)*-0.2f,0.0f) && (pt-ptri->pt[0]^ptri->pt[1]-ptri->pt[0])*ptri->n<0 && (pt-ptri->pt[1]^ptri->pt[2]-ptri->pt[1])*ptri->n<0 && (pt-ptri->pt[2]^ptri->pt[0]-ptri->pt[2])*ptri->n<0 && pinters->nborderptnbordersz) pinters->ptborder[pinters->nborderpt++] = pt; // add circle points that are below the triangle as ptborder pt = pcyl->center - pcyl->axis*(pcyl->hh*j); axisx = pcyl->axis.orthogonal().normalized()*pcyl->r; axisy = axisx^pcyl->axis; for(i=0,pt2d.set(1,0); i<8; i++) { vectorf pt1 = pinters->ptborder[pinters->nborderpt] = pt+axisx*pt2d.x+axisy*pt2d.y; pinters->nborderpt += isneg((pt1-ptri->pt[0])*ptri->n) & isneg(pinters->nborderpt-pinters->nbordersz); float t=pt2d.x; pt2d.x=(pt2d.x-pt2d.y)*(sqrt2/2); pt2d.y=(t+pt2d.y)*(sqrt2/2); // rotate by 45 degrees } return 1; } iEndBest = iEndBest-(iEndBest>>31) | iEndCur&iEndBest>>31; if (iEndBest<0) return 0; i = iStartBest>>1; pinters->pt[0] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iStartBest&1].val(); pinters->iFeature[0][0] = 0xA0 | dec_mod3[i]; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; i = iEndBest>>1; pinters->pt[1] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iEndBest&1].val(); pinters->iFeature[1][0] = 0xA0 | dec_mod3[i]; // if cylinder cap's lowest (wrt trinagle normal) point is inside tringle's voronoi region, force triangle normal as global intersection normal // make this check only if the cylinder is not too below the triangle surface pt = pcyl->center - pcyl->axis*(pcyl->hh*(j=sgnnz(pcyl->axis*ptri->n))); if ((pcyl->axis^ptri->n).len2()>0.001) { pt += ((ptri->n^pcyl->axis)^pcyl->axis).normalized()*pcyl->r; if ((pt-ptri->pt[0])*ptri->n > (pcyl->hh+pcyl->r)*-0.2f && (pt-ptri->pt[0]^ptri->pt[1]-ptri->pt[0])*ptri->n<0 && (pt-ptri->pt[1]^ptri->pt[2]-ptri->pt[1])*ptri->n<0 && (pt-ptri->pt[2]^ptri->pt[0]-ptri->pt[2])*ptri->n<0 && pinters->nborderptnbordersz) { pinters->ptbest = pt; pinters->n = ptri->n; pinters->nBestPtVal = 1000; pinters->ptborder[pinters->nborderpt++] = pt; } dist.set((ptri->pt[0]-pt)*ptri->n, fabs_tpl(pcyl->axis*ptri->n)); if (inrange(dist.x, (real)0,pcyl->hh*2*dist.y)) pt += pcyl->axis*(j*dist.val()); else pt += pcyl->axis*(pcyl->hh*2*j); if (inrange((pt-ptri->pt[0])*ptri->n, (pcyl->hh+pcyl->r)*-0.2f,0.0f) && (pt-ptri->pt[0]^ptri->pt[1]-ptri->pt[0])*ptri->n<0 && (pt-ptri->pt[1]^ptri->pt[2]-ptri->pt[1])*ptri->n<0 && (pt-ptri->pt[2]^ptri->pt[0]-ptri->pt[2])*ptri->n<0 && pinters->nborderptnbordersz) pinters->ptborder[pinters->nborderpt++] = pt; } else if ((pt-ptri->pt[0])*ptri->n > (pcyl->hh+pcyl->r)*-0.2f) { pinters->ptbest = pt; pinters->n = ptri->n; pinters->nBestPtVal = 1000; // add circle points to ptborder if they are inside the triangle and below it axisx = pcyl->axis.orthogonal().normalized()*pcyl->r; axisy = axisx^pcyl->axis; for(i=0,pt2d.set(1,0); i<8; i++) { vectorf pt1 = pinters->ptborder[pinters->nborderpt] = pt+axisx*pt2d.x+axisy*pt2d.y; pinters->nborderpt += isneg((pt1-ptri->pt[0])*ptri->n) & isneg((pt1-ptri->pt[0]^ptri->pt[1]-ptri->pt[0])*ptri->n) & isneg((pt1-ptri->pt[1]^ptri->pt[2]-ptri->pt[1])*ptri->n) & isneg((pt1-ptri->pt[2]^ptri->pt[0]-ptri->pt[2])*ptri->n) & isneg(pinters->nborderpt-pinters->nbordersz); float t=pt2d.x; pt2d.x=(pt2d.x-pt2d.y)*(sqrt2/2); pt2d.y=(t+pt2d.y)*(sqrt2/2); // rotate by 45 degrees } } return 1; } int cylinder_tri_intersection(const cylinder *pcyl, const triangle *ptri, prim_inters *pinters) { int res = tri_cylinder_intersection(ptri,pcyl,pinters); vectorf pt; int iFeature; pt=pinters->pt[0]; pinters->pt[0]=pinters->pt[1]; pinters->pt[1]=pt; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int tri_sphere_intersection(const triangle *ptri, const sphere *psphere, prim_inters *pinters) { vectorr pc,dp,dp1,vec1,vec0; real r2,a,b,c,d; quotient t[3][2],dist,mindist; int i,j,iStart,iStartBest,iEndBest,bOutside,bEnd,iEndCur,bState,bBest,bSwitch,bFaceContact=0; r2 = sqr(psphere->r); for(i=0,iStart=-1; i<3; i++) { bOutside = isneg(r2 - (ptri->pt[i]-psphere->center).len2()); (iStart&=~-bOutside) |= i&-bOutside; } if (iStart<0) // all triangle points are inside the sphere return 0; bOutside = isneg(psphere->r-fabs_tpl((psphere->center-ptri->pt[0])*ptri->n)); for(i=0; i<3; i++) // check if this point lies in triangle's voronoi region bOutside |= isneg((ptri->pt[inc_mod3[i]]-ptri->pt[i] ^ psphere->center-ptri->pt[i])*ptri->n); if (!bOutside) { // choose the point on the sphere that has minimum projection on triangle normal pinters->nBestPtVal = 1000; pinters->ptbest = psphere->center - ptri->n*psphere->r; pinters->n = ptri->n; bFaceContact = 1; } i=iStart; iEndCur=iStartBest=iEndBest=-1; bState=0; mindist.set(1,0); do { dp = ptri->pt[dec_mod3[i]]-ptri->pt[i]; pc = ptri->pt[i]-psphere->center; a = dp.len2(); b = dp*pc; c = pc.len2()-r2; d = b*b-a*c; if (d>=0) { d = sqrt_tpl(d); t[i][0].set(-b-d,a); t[i][1].set(-b+d,a); if (t[i][0]pt[i]-pinters->pt[1]); dist.set((dp1*t[i][j].y+(ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][j].x).len2(), sqr(t[i][j].y)); bBest = isneg(dist-mindist) & bSwitch & (bState^1); (mindist.x*=1-bBest) += dist.x*bBest; (mindist.y*=1-bBest) += dist.y*bBest; (iStartBest&=~-bBest) |= (i<<1|j)&-bBest; (iEndBest&=~-bBest) |= iEndCur&-bBest; bState ^= bSwitch; } } } while((i=dec_mod3[i])!=iStart); if (iStartBest<0) { // triangle edges do not intersect sphere if (!bFaceContact) return 0; pinters->pt[0] = pinters->pt[1] = pinters->ptbest; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; return 1; } iEndBest = iEndBest-(iEndBest>>31) | iEndCur&iEndBest>>31; if (iEndBest<0) return 0; i = iStartBest>>1; pinters->pt[0] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iStartBest&1].val(); pinters->iFeature[0][0] = 0xA0 | dec_mod3[i]; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; i = iEndBest>>1; pinters->pt[1] = ptri->pt[i] + (ptri->pt[dec_mod3[i]]-ptri->pt[i])*t[i][iEndBest&1].val(); pinters->iFeature[1][0] = 0xA0 | dec_mod3[i]; return 1; } int sphere_tri_intersection(const sphere *psphere, const triangle *ptri, prim_inters *pinters) { int res = tri_sphere_intersection(ptri,psphere,pinters); vectorf pt; int iFeature; pt=pinters->pt[0]; pinters->pt[0]=pinters->pt[1]; pinters->pt[1]=pt; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int ray_tri_intersection(const ray *pray, const triangle *ptri, prim_inters *pinters) { quotient t((ptri->pt[0]-pray->origin)*ptri->n, pray->dir*ptri->n); if (sqr(t.y) > pinters->minPtDist2*sqr(1E-4f)) { t.fixsign(); vectorr pt = pray->origin*t.y + pray->dir*t.x, edge; real nlen2 = ptri->n.len2()*t.y; int bOutside = isneg(t.y-fabsf(t.x*2-t.y)); edge = ptri->pt[1]-ptri->pt[0]; bOutside |= isneg(sqr_signed(ptri->n*(edge ^ pt-ptri->pt[0]*t.y))+pinters->minPtDist2*edge.len2()*nlen2); edge = ptri->pt[2]-ptri->pt[1]; bOutside |= isneg(sqr_signed(ptri->n*(edge ^ pt-ptri->pt[1]*t.y))+pinters->minPtDist2*edge.len2()*nlen2); edge = ptri->pt[0]-ptri->pt[2]; bOutside |= isneg(sqr_signed(ptri->n*(edge ^ pt-ptri->pt[2]*t.y))+pinters->minPtDist2*edge.len2()*nlen2); if (!bOutside) { pinters->pt[0] = pinters->pt[1] = pray->origin + pray->dir*t.val(); pinters->n = ptri->n; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x20; return 1; } } return 0; } int tri_ray_intersection(const triangle *ptri, const ray *pray, prim_inters *pinters) { int res = ray_tri_intersection(pray,ptri,pinters); int iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; pinters->n.Flip(); return res; } int tri_plane_intersection(const triangle *ptri, const plane *pplane, prim_inters *pinters) { // axes: n2 x n n1 x n n = n1 x n2 vectorr axes[3]; axes[2] = ptri->n^pplane->n; axes[0] = pplane->n^axes[2]; axes[1] = ptri->n^axes[2]; matrix3x3RM &mtx((matrix3x3RM&)axes[0]); real nlen2 = axes[2].len2(), ptw; vectorr ptloc, pt0=vectorr(ptri->pt[0]*ptri->n, -pplane->origin*pplane->n, ptri->pt[0]*axes[2])*mtx; vector2d pt2d[3]; int sgnx[3],nCross,iNotCross,bCross; quotient tmin,tmax; ptloc=ptri->pt[0]*nlen2-pt0; pt2d[0].set(axes[1]*ptloc,axes[2]*ptloc); sgnx[0]=sgnnz(pt2d[0].x); ptloc=ptri->pt[1]*nlen2-pt0; pt2d[1].set(axes[1]*ptloc,axes[2]*ptloc); sgnx[1]=sgnnz(pt2d[1].x); ptloc=ptri->pt[2]*nlen2-pt0; pt2d[2].set(axes[1]*ptloc,axes[2]*ptloc); sgnx[2]=sgnnz(pt2d[2].x); bCross = sgnx[0]*sgnx[1]-1>>1; nCross = -bCross; iNotCross = 0; bCross = sgnx[1]*sgnx[2]-1>>1; nCross -= bCross; iNotCross |= 1&~bCross; bCross = sgnx[2]*sgnx[0]-1>>1; nCross -= bCross; (iNotCross&=bCross) |= 2&~bCross; if (nCross!=2) return 0; tmax.x = pt2d[dec_mod3[iNotCross]] ^ pt2d[inc_mod3[iNotCross]]; tmax.y = pt2d[dec_mod3[iNotCross]].x - pt2d[inc_mod3[iNotCross]].x; tmin.x = pt2d[iNotCross] ^ pt2d[dec_mod3[iNotCross]]; tmin.y = pt2d[iNotCross].x - pt2d[dec_mod3[iNotCross]].x; ptw = (real)1.0/(nlen2*tmin.y*tmax.y); pinters->pt[0] = (pt0*tmin.y*tmax.y + axes[2]*tmin.x*tmax.y)*ptw; pinters->iFeature[0][0] = 0x20 | dec_mod3[iNotCross]; pinters->pt[1] = (pt0*tmin.y*tmax.y + axes[2]*tmax.x*tmin.y)*ptw; pinters->iFeature[1][0] = 0x20 | inc_mod3[iNotCross]; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; return 1; } int plane_tri_intersection(const plane *pplane, const triangle *ptri, prim_inters *pinters) { int res = tri_plane_intersection(ptri,pplane,pinters); vectorf pt; int iFeature; pt=pinters->pt[0]; pinters->pt[0]=pinters->pt[1]; pinters->pt[1]=pt; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int box_box_intersection(const box *pbox1, const box *pbox2, prim_inters *pinters) { vectorr center[2],axes[3],dp,pt0; vectori axsg; matrix3x3RM &Basis21((matrix3x3RM&)axes[0]); int ibox,i,idir,iEdge,bBest,sg,bLocInters[2]; quotient t[2],t1[2]; const box *pbox[2] = { pbox1,pbox2 }; center[0] = pbox1->Basis*(pbox2->center-pbox1->center); center[1] = pbox2->Basis*(pbox1->center-pbox2->center); Basis21 = pbox2->Basis*pbox1->Basis.T(); pinters->nborderpt = 0; for(ibox=0; ibox<2; ibox++) { for(iEdge=0;iEdge<12;iEdge++) { idir = iEdge>>2; axsg[idir]=-1; axsg[inc_mod3[idir]]=((iEdge&1)<<1)-1; axsg[dec_mod3[idir]]=(iEdge&2)-1; pt0 = center[ibox] + axes[0]*(pbox[ibox^1]->size.x*axsg.x) + axes[1]*(pbox[ibox^1]->size.y*axsg.y) + axes[2]*(pbox[ibox^1]->size.z*axsg.z); dp = axes[idir]; t[0].set(0,1); t[1].set(pbox[ibox^1]->size[idir]*2,1); for(i=bLocInters[0]=bLocInters[1]=0; i<3; i++) { idir = isneg(dp[i]); sg = 1-(idir<<1); t1[idir]. set((-pbox[ibox]->size[i]-pt0[i])*sg, fabs_tpl(dp[i])); t1[idir^1].set((pbox[ibox]->size[i]-pt0[i])*sg, fabs_tpl(dp[i])); bLocInters[0] |= (bBest = isneg(t[0]-t1[0])); // t[0] = max(t[0],t1[0]); t[0].x = t[0].x*(bBest^1) + t1[0].x*bBest; t[0].y = t[0].y*(bBest^1) + t1[0].y*bBest; bLocInters[1] |= (bBest = isneg(t1[1]-t[1])); // t[1] = min(t[1],t1[1]); t[1].x = t[1].x*(bBest^1) + t1[1].x*bBest; t[1].y = t[1].y*(bBest^1) + t1[1].y*bBest; } if (t[0]ptborder[pinters->nborderpt++] = (pt0+dp*t[0].val())*pbox[ibox]->Basis + pbox[ibox]->center; if (bLocInters[1]) pinters->ptborder[pinters->nborderpt++] = (pt0+dp*t[1].val())*pbox[ibox]->Basis + pbox[ibox]->center; } } Basis21.Transpose(); } if (!pinters->nborderpt) return 0; pinters->pt[0] = pinters->pt[1] = pinters->ptborder[0]; pinters->n.zero(); // normal and features not filled pinters->iFeature[0][0]=pinters->iFeature[0][1]=pinters->iFeature[1][0]=pinters->iFeature[1][1] = 0x40; return 1; } int box_cylinder_intersection(const box *pbox, const cylinder *pcyl, prim_inters *pinters) { vectorr axis,center,n,pt,vec0,vec1,size,c,nx; int i,ix,iy,iz,idir,icap,isgz,imask,bNega,bNegad,bInRange,bLocInters[2],sg,bBest; quotient t[2],t1[2]; real ka,kb,kc,kd,rnz,rx2,rx,ry,r=pcyl->r,hh=pcyl->hh; axis = pbox->Basis*pcyl->axis; center = pbox->Basis*(pcyl->center-pbox->center); size = pbox->size; pinters->nborderpt = 0; // check box edges - cylinder intersections for(iz=0;iz<3;iz++) { ix = inc_mod3[iz]; iy = dec_mod3[iz]; for(i=0;i<4;i++) { // edge-cap intersections pt[ix]=size[ix]*((i<<1&2)-1); pt[iy]=size[iy]*((i&2)-1); pt[iz]=0; t[0].set((center-pt)*axis-hh, axis[iz]); (pt*=t[0].y)[iz]=t[0].x; if (isneg(fabs_tpl(t[0].x)-size[iz]*fabs_tpl(t[0].y)) & isneg((pt-center*t[0].y^axis).len2()-sqr(r*t[0].y))) pinters->ptborder[pinters->nborderpt++] = (pt/t[0].y)*pbox->Basis+pbox->center; t[0].x+=hh*2; pt[iz]=t[0].x; if (isneg(fabs_tpl(t[0].x)-size[iz]*fabs_tpl(t[0].y)) & isneg((pt-center*t[0].y^axis).len2()-sqr(r*t[0].y))) pinters->ptborder[pinters->nborderpt++] = (pt/t[0].y)*pbox->Basis+pbox->center; // edge-side intersections pt[ix]=size[ix]*((i<<1&2)-1); pt[iy]=size[iy]*((i&2)-1); pt[iz]=0; vec0=pt-center^axis; vec1=-cross_with_ort(axis,iz); ka=vec1*vec1; kb=vec0*vec1; kc=vec0*vec0-r*r; kd=kb*kb-ka*kc; if (kd>0) { kd=sqrt_tpl(kd); t[0].set(-kb-kd,ka); (pt*=t[0].y)[iz]=t[0].x; if (isneg(fabs_tpl(t[0].x)-size[iz]*t[0].y) & isneg(fabs_tpl((pt-center)*axis)-hh)) pinters->ptborder[pinters->nborderpt++] = (pt/t[0].y)*pbox->Basis+pbox->center; t[0].x+=kd*2; pt[iz]=t[0].x; if (isneg(fabs_tpl(t[0].x)-size[iz]*t[0].y) & isneg(fabs_tpl((pt-center)*axis)-hh)) pinters->ptborder[pinters->nborderpt++] = (pt/t[0].y)*pbox->Basis+pbox->center; } } } // check cylinder cap edges - box intersections for(iz=0;iz<3;iz++) if (fabs_tpl(axis[iz])<(real)0.999) { ix = inc_mod3[iz]; iy = dec_mod3[iz]; (n = axis*axis[iz])[iz]-=1; for(i=0;i<4;i++) { icap = (i<<1&2)-1; isgz = (i&2)-1; if (sqr(center[iz]+axis[iz]*(hh*icap)-size[iz]*isgz)*n.len2() < sqr(n[iz]*r)) { n.normalize(); // inters_point(sg) = ( center.z + n*(size.z*isgz-center.z) + sg*nx*(r^2*nz^2-(size.z*isgz-center.z)^2*n^2)^1/2 )/n.z c = center+axis*(icap*hh); ry = size[iz]*isgz-c[iz]; rx2 = sqr(r*n[iz])-ry*ry; nx = n^axis; // for x,y coordinates check |inters_point(sg).x| < size.x, group into | a+sg*b^1/2 | < c ka = c[ix]*n[iz]+n[ix]*ry; kb = nx[ix]; kc = size[ix]*n[iz]; kd = kc*kc-ka*ka-kb*kb*rx2; bInRange = isneg(sqr(ka*kb*2)*rx2-kd*kd); bNegad = isneg(ka*kb*kd); bNega = isneg(ka*kb); imask = (bNegad&bNega | (bNegad^1)&(bInRange^bNega))<<1 | ((bNegad^1)&(bNega^1) | bNegad&(bInRange^bNega^1)); ka = c[iy]*n[iz]+n[iy]*ry; kb = nx[iy]; kc = size[iy]*n[iz]; kd = kc*kc-ka*ka-kb*kb*rx2; bInRange = isneg(sqr(ka*kb*2)*rx2-kd*kd); bNegad = isneg(ka*kb*kd); bNega = isneg(ka*kb); imask &= (bNegad&bNega | (bNegad^1)&(bInRange^bNega))<<1 | ((bNegad^1)&(bNega^1) | bNegad&(bInRange^bNega^1)); if (imask) { rx = sqrt_tpl(rx2); rnz = (real)1.0/n[iz]; if (imask&1) pinters->ptborder[pinters->nborderpt++] = (c+(n*ry-nx*rx)*rnz)*pbox->Basis+pbox->center; if (imask&2) pinters->ptborder[pinters->nborderpt++] = (c+(n*ry+nx*rx)*rnz)*pbox->Basis+pbox->center; } } } } // check cylinder axis-box intersections t[0].set(-hh,1); t[1].set(hh,1); for(iz=bLocInters[0]=bLocInters[1]=0; iz<3; iz++) { idir = isneg(axis[iz]); sg = 1-(idir<<1); t1[idir]. set((-size[iz]-center[iz])*sg, fabs_tpl(axis[iz])); t1[idir^1].set((size[iz]-center[iz])*sg, fabs_tpl(axis[iz])); bLocInters[0] |= (bBest = isneg(t[0]-t1[0])); // t[0] = max(t[0],t1[0]); t[0].x = t[0].x*(bBest^1) + t1[0].x*bBest; t[0].y = t[0].y*(bBest^1) + t1[0].y*bBest; bLocInters[1] |= (bBest = isneg(t1[1]-t[1])); // t[1] = min(t[1],t1[1]); t[1].x = t[1].x*(bBest^1) + t1[1].x*bBest; t[1].y = t[1].y*(bBest^1) + t1[1].y*bBest; } if (t[0]ptborder[pinters->nborderpt++] = (center+axis*t[0].val())*pbox->Basis + pbox->center; if (bLocInters[1]) pinters->ptborder[pinters->nborderpt++] = (center+axis*t[1].val())*pbox->Basis + pbox->center; } if (!pinters->nborderpt) return 0; pinters->pt[0] = pinters->pt[1] = pinters->ptborder[0]; pinters->n.zero(); // normal and features not filled pinters->iFeature[0][0]=pinters->iFeature[0][1]=pinters->iFeature[1][0]=pinters->iFeature[1][1] = 0x40; return 1; } int cylinder_box_intersection(const cylinder *pcyl, const box *pbox, prim_inters *pinters) { int res = box_cylinder_intersection(pbox,pcyl,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; pinters->n.Flip(); return res; } int box_sphere_intersection(const box *pbox, const sphere *psphere, prim_inters *pinters) { int idir; vectorf center,dir; center = pbox->Basis*(psphere->center-pbox->center); for(idir=0;idir<3;idir++) dir[idir] = min(0.0f,pbox->size[idir]-fabs_tpl(center[idir]))*sgnnz(center[idir]); pinters->ptborder[0]=pinters->pt[0]=pinters->pt[1] = (center+dir)*pbox->Basis+pbox->center; pinters->nborderpt = 1; pinters->n = psphere->center - pinters->pt[0]; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; return isneg(dir.len2()-sqr(psphere->r)); } int sphere_box_intersection(const sphere *psphere, const box *pbox, prim_inters *pinters) { int res = box_sphere_intersection(pbox,psphere,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; pinters->n.Flip(); return res; } int box_ray_intersection(const box *pbox, const ray *pray, prim_inters *pinters) { quotient t[2],t1[2]; int i,idir,iBest[2]={-1,-1},sg; vectorr origin=pbox->Basis*(pray->origin-pbox->center), dir=pbox->Basis*pray->dir; t[0].set(0,1); t[1].set(1,1); for(i=0;i<3;i++) { idir = isneg(dir[i]); sg = 1-(idir<<1); t1[idir].set((-pbox->size[i]-origin[i])*sg, dir[i]*sg); t1[idir^1].set((pbox->size[i]-origin[i])*sg,dir[i]*sg); if (t1[0]>t[0]) // t[0] = max(t[0],t1[0]); { t[0]=t1[0]; iBest[0]=i; } if (t1[1]t[1] || (iBest[0]&iBest[1])==-1) return 0; i = -(iBest[0]>>31); pinters->pt[0]=pinters->pt[1] = pray->origin + pray->dir*t[i].val(); pinters->n = pbox->Basis.GetRow(iBest[i]); pinters->n *= sgnnz(pinters->n*(pinters->pt[0]-pbox->center)); pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x20; return 1; } int ray_box_intersection(const ray *pray, const box *pbox, prim_inters *pinters) { int res = box_ray_intersection(pbox,pray,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int box_plane_intersection(const box *pbox, const plane *pplane, prim_inters *pinters) { vectorf dir = pbox->Basis.GetRow(0)*pbox->size.x*sgnnz(pbox->Basis.GetRow(0)*pplane->n) + pbox->Basis.GetRow(1)*pbox->size.y*sgnnz(pbox->Basis.GetRow(1)*pplane->n) + pbox->Basis.GetRow(2)*pbox->size.z*sgnnz(pbox->Basis.GetRow(2)*pplane->n); pinters->pt[0]=pinters->pt[1] = pbox->center-dir; pinters->n = -pplane->n; pinters->iFeature[0][0] = pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1] = pinters->iFeature[1][1] = 0x40; return isneg((pinters->pt[0]-pplane->origin)*pplane->n); } int plane_box_intersection(const plane *pplane, const box *pbox, prim_inters *pinters) { int res = box_plane_intersection(pbox,pplane,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } inline int root_inrange(float a,float ad,float b,float hca,float d,float h) { int sg = sgnnz((-b+d)*ad+hca); return isneg(d*sqr_signed(ad)*sg - sqr_signed(h*a+(b*ad-hca)*sg)); } int cylinder_cylinder_intersection(const cylinder *pcyl1, const cylinder *pcyl2, prim_inters *pinters) { /*box abox; abox.center = pcyl2->center; abox.size(pcyl2->r,pcyl2->r,pcyl2->hh); abox.bOriented = 1; abox.Basis.SetRow(2,pcyl2->axis); vectorf axisx(0,0,1); axisx -= pcyl2->axis*(pcyl2->axis*axisx); if (axisx.len2()axis); abox.Basis.SetRow(0,axisx.normalize()); abox.Basis.SetRow(1,pcyl2->axis^axisx); return cylinder_box_intersection(pcyl1,&abox,pinters);*/ const cylinder *pcyl[2] = { pcyl1,pcyl2 }; vectorf center[2],dc,pt,axisx,axisy,u,l,a1; vector2df c2d; float a,b,c,d,k,r0,r1,h,span2,p,q,cosa,sina,rsina; quotient t; int icyl,i,j,sg,bHasInters=0; pinters->nborderpt = 0; cosa = pcyl[0]->axis*pcyl[1]->axis; sina = (axisx = pcyl[0]->axis^pcyl[1]->axis).len(); if (sina>0.003f) { axisx *= (rsina=1/sina); // check axis-axis closest points to lie within cylinder bounds dc = pcyl[1]->center-pcyl[0]->center; if (isneg(fabs_tpl(dc*axisx)-pcyl[0]->r-pcyl[1]->r) & isneg(fabs_tpl((dc^pcyl[1]->axis)*axisx)-pcyl[0]->hh*sina) & isneg(fabs_tpl((dc^pcyl[0]->axis)*axisx)-pcyl[1]->hh*sina)) bHasInters = 1; } else axisx = pcyl[0]->axis.orthogonal().normalized(); for(icyl=0; icyl<2; icyl++) { axisy = pcyl[icyl^1]->axis^axisx; r0 = pcyl[icyl]->r; r1 = pcyl[icyl^1]->r; // check for icyl axis - icyl^1 cap intersections t.set((pcyl[icyl^1]->center-pcyl[icyl]->center)*pcyl[icyl^1]->axis-pcyl[icyl^1]->hh, cosa); pt = (pcyl[icyl]->center-pcyl[icyl^1]->center)*t.y + pcyl[icyl]->axis*t.x; pt -= pcyl[icyl^1]->axis*(pt*pcyl[icyl^1]->axis); for(i=0; i<2; i++,t.x+=pcyl[icyl^1]->hh*2,pt+=pcyl[icyl]->axis*(pcyl[icyl^1]->hh*2)) if (isneg(fabs_tpl(t.x)-fabs_tpl(t.y)*pcyl[icyl]->hh) & isneg(pt.len2()center-pcyl[icyl^1]->center; a = 1-sqr(cosa); if (a>0.0001f) { b = pcyl[icyl]->axis*center[0]-(center[0]*pcyl[icyl^1]->axis)*cosa; c = center[0].len2()-sqr(pcyl[icyl^1]->axis*center[0])-sqr(r1); d = b*b-a*c; if (d>0 && (root_inrange(a,1,b,0,d,pcyl[icyl]->hh) && root_inrange(a,cosa,b,-(center[0]*pcyl[icyl^1]->axis)*a,d,pcyl[icyl^1]->hh) || root_inrange(a,1,b,0,-d,pcyl[icyl]->hh) && root_inrange(a,cosa,b,-(center[0]*pcyl[icyl^1]->axis)*a,-d,pcyl[icyl^1]->hh))) bHasInters = 1; } // check for icyl cap - icyl^1 side intersections for(i=-1; i<=1; i+=2) { dc = center[0]+pcyl[icyl]->axis*(pcyl[icyl]->hh*i); c2d.set(-(dc*axisx), -(dc*axisy)); if (fabs_tpl(cosa)<0.003f) { if (fabs_tpl(c2d.y) < r1 && inrange(sqr(r1)-sqr(c2d.y), sqr_signed(fabs_tpl(c2d.x)-r0),sqr_signed(fabs_tpl(c2d.x)+r0)) && fabs_tpl((dc+axisx*(r0*sgnnz(c2d.x)))*pcyl[icyl^1]->axis) < pcyl[icyl^1]->hh) bHasInters = 1; } else { a = r0*r0*(1+sqr(cosa)) + r1*r1 - sqr(c2d.x) - sqr(c2d.y); b = sqr(r0*r0*cosa) + sqr(r1*r0)*(1+sqr(cosa)) - sqr(r0)*(sqr(c2d.x)*sqr(cosa)+sqr(c2d.y)); c = sqr(r1*r0*r0*cosa); p = b-a*a*(1.0f/3); q = a*(b*(1.0f/6)-a*a*(1.0f/27))-c*0.5f; if (cube(p)*(1.0f/27)+sqr(q) > 0) { // now check if the closest point on the edge to the icyl^1 axis is inside h range vector2di sg2d(sgnnz(c2d.x),sgnnz(c2d.y)); vector2df pt2d,kpt2d,n2d,kn2d; kpt2d.set(r0*sg2d.x, r0*sg2d.y*fabs_tpl(cosa)); kn2d.set(sg2d.x*fabs_tpl(cosa), sg2d.y); j = 1-sg2d.x*sg2d.y>>1; int iangle,idx[2] = {0,SINCOSTABSZ}; do { iangle = idx[0]+idx[1]>>1; pt2d.set(g_costab[iangle]*kpt2d.x, g_sintab[iangle]*kpt2d.y); n2d.set(g_costab[iangle]*kn2d.x, g_sintab[iangle]*kn2d.y); idx[isneg(n2d^c2d-pt2d)^j] = iangle; } while(idx[1]-idx[0] > 1); if (fabs_tpl(dc*pcyl[icyl^1]->axis+sina*sgnnz(cosa)*g_sintab[iangle]*sg2d.y*r0*(icyl*2-1)) < pcyl[icyl^1]->hh) { bHasInters = 1; int icenter,idx1,imiddle,iquad,ipass; icenter = idx[0]*sg2d.x*sg2d.y + SINCOSTABSZ*(1-sg2d.x) & SINCOSTABSZ*4-1; kpt2d.set(r0, r0*fabs_tpl(cosa)); for(ipass=-1; ipass<=1; ipass+=2) { idx[0] = icenter; idx1=idx[1] = idx[0]+ipass*SINCOSTABSZ; do { imiddle = idx[0]+idx[1]>>1; iquad = imiddle>>SINCOSTABSZ_LOG2 & 3; iangle = (SINCOSTABSZ-1 & -(iquad&1)) + (imiddle & SINCOSTABSZ-1)*(1-(iquad&1)*2); pt2d.set(g_costab[iangle]*(1-((iquad^iquad<<1)&2))*kpt2d.x, g_sintab[iangle]*(1-(iquad&2))*kpt2d.y); idx[isneg(sqr(r1)-(pt2d-c2d).len2())] = imiddle; } while(fabs_tpl(idx[0]-idx[1])>2); pt2d.y = sgnnz(cosa)*g_sintab[iangle]*(1-(iquad&2))*r0; if (idx[1]!=idx1 && fabs_tpl(dc*pcyl[icyl^1]->axis+sina*pt2d.y*(icyl*2-1)) < pcyl[icyl^1]->hh) pinters->ptborder[pinters->nborderpt++] = dc+axisx*pt2d.x+(pcyl[icyl]->axis^axisx)*pt2d.y+pcyl[icyl^1]->center; } } } } } } // check for icyl cap - icyl^1 cap intersections if (sina>0.003f) { u = (pcyl[0]->axis*cosa-pcyl[1]->axis)*rsina; l = pcyl[0]->axis^u; a1 = pcyl[1]->axis*rsina; for(i=-1;i<=1;i+=2) for(j=-1;j<=1;j+=2) { center[0] = pcyl[0]->center+pcyl[0]->axis*(pcyl[0]->hh*i)-pcyl[1]->center; center[1] = pcyl[1]->axis*(pcyl[1]->hh*j); h = a1*(center[0]-=center[1]); center[0] += u*h; b = l*center[0]; c = center[0].len2()-sqr(pcyl[1]->r); d = b*b-c; span2 = sqr(pcyl[0]->r)-sqr(h); sg = sgnnz(d-b*b); k = d-span2-b*b; if (min(span2,d) > 0) { if (inrange(b*b, d,span2) | isneg(sqr_signed(k)*sg-span2*sqr(b)*4)) { d = sqrt_tpl(d); for(sg=-1; sg<=1; sg+=2) if (sqr(-b+d*sg)ptborder[pinters->nborderpt++] = center[0]+l*(-b+d*sg)+center[1]+pcyl[1]->center; } if (sqr(2*center[0]*l)*span2 > max(0.0f,center[0].len2()+span2-sqr(pcyl[1]->r))) { span2 = sqrt_tpl(span2); for(sg=-1; sg<=1; sg+=2) if ((center[0]+l*(span2*sg)).len2()r)) pinters->ptborder[pinters->nborderpt++] = center[0]+l*(span2*sg)+center[1]+pcyl[1]->center; } } } } return bHasInters | isneg(-pinters->nborderpt); } int cylinder_sphere_intersection(const cylinder *pcyl, const sphere *psphere, prim_inters *pinters) { vectorf dir,dc; float dh,h; int bOutside; bOutside = isneg(dh = pcyl->hh - fabs_tpl(h = (psphere->center-pcyl->center)*pcyl->axis)); dir = pcyl->axis*dh*bOutside*sgnnz(h); bOutside = isneg(sqr(pcyl->r) - ((dc = psphere->center-pcyl->center) ^ pcyl->axis).len2()); if (bOutside) { dc -= pcyl->axis*(dc*pcyl->axis); dc -= dc.normalized()*pcyl->r; dir += dc; } pinters->pt[0]=pinters->pt[1] = psphere->center+dir; pinters->n = psphere->center - pinters->pt[0]; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x40; return isneg(dir.len2() - sqr(psphere->r)); } int sphere_cylinder_intersection(const sphere *psphere, const cylinder *pcyl, prim_inters *pinters) { int res = cylinder_sphere_intersection(pcyl,psphere,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int cylinder_ray_intersection(const cylinder *pcyl, const ray *pray, prim_inters *pinters) { int sg,bHit,bHitSide=0; quotient t; vectorr vec0,vec1; real a,b,c,d; sg = sgnnz(pcyl->axis*pray->dir); t.set(pcyl->axis*(pcyl->center-pcyl->axis*(pcyl->hh*sg)-pray->origin), pray->dir*pcyl->axis); bHit = isneg(((pray->origin-pcyl->center)*t.y + pray->dir*t.x ^ pcyl->axis).len2() - sqr(pcyl->r*t.y)); t.set(pcyl->axis*(pcyl->center-pcyl->axis*(pcyl->hh*sg*(bHit*2-1))-pray->origin), pray->dir*pcyl->axis); bHit = isneg(((pray->origin-pcyl->center)*t.y + pray->dir*t.x ^ pcyl->axis).len2() - sqr(pcyl->r*t.y)); if (!bHit) { vec0 = pray->origin-pcyl->center ^ pcyl->axis; vec1 = pray->dir ^ pcyl->axis; a = vec1*vec1; b = vec0*vec1; c = vec0*vec0-sqr(pcyl->r); d = b*b-a*c; if (d>=0) { d = sqrt_tpl(d); t.set(-b-d, a); bHitSide = bHit = isneg(fabs_tpl(t.x*2-t.y)-t.y) & isneg(fabs_tpl(((pray->origin-pcyl->center)*t.y+pray->dir*t.x)*pcyl->axis)-pcyl->hh*t.y); t.x += d*(bHit^1)*2; bHitSide = bHit = isneg(fabs_tpl(t.x*2-t.y)-t.y) & isneg(fabs_tpl(((pray->origin-pcyl->center)*t.y+pray->dir*t.x)*pcyl->axis)-pcyl->hh*t.y); } } if (!bHit || t.x*t.y<0) return 0; pinters->pt[0]=pinters->pt[1] = pray->origin + pray->dir*t.val(); if (bHitSide) { pinters->n = pinters->pt[0]-pcyl->center; (pinters->n -= pcyl->axis*(pcyl->axis*pinters->n)).normalize(); pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; } else { sg = sgnnz((pinters->pt[0]-pcyl->center)*pcyl->axis); pinters->n = pcyl->axis*sg; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x41+(sg+1>>1); } pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x20; return 1; } int ray_cylinder_intersection(const ray *pray, const cylinder *pcyl, prim_inters *pinters) { int res = cylinder_ray_intersection(pcyl,pray,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int cylinder_plane_intersection(const cylinder *pcyl, const plane *pplane, prim_inters *pinters) { vectorf dirdown = pcyl->axis*(pcyl->axis*pplane->n); if (dirdown.len2() > sqr(pcyl->r*1E-3f)) dirdown.normalize() *= pcyl->r; pinters->pt[0]=pinters->pt[1] = pcyl->center - pcyl->axis*pcyl->hh*sgnnz(pcyl->axis*pplane->n) + dirdown; pinters->n = -pplane->n; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x40; return isneg((pinters->pt[0]-pplane->origin)*pplane->n); } int plane_cylinder_intersection(const plane *pplane, const cylinder *pcyl, prim_inters *pinters) { int res = cylinder_plane_intersection(pcyl,pplane,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int sphere_sphere_intersection(const sphere *psphere1, const sphere *psphere2, prim_inters *pinters) { vectorr dc = psphere1->center-psphere2->center; if (dc.len2() > sqr(psphere1->r+psphere2->r)) return 0; dc.normalize(); pinters->pt[0]=pinters->pt[1] = (dc*(psphere1->r-psphere2->r) + psphere1->center+psphere2->center)*(real)0.5; pinters->n = psphere2->center - psphere1->center; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x40; return 1; } int sphere_ray_intersection(const sphere *psphere, const ray *pray, prim_inters *pinters) { int bHit; real a,b,c,d; quotient t; a=pray->dir.len2(); b=pray->dir*(pray->origin-psphere->center); c=(pray->origin-psphere->center).len2()-sqr(psphere->r); if ((d=b*b-a*c)>=0) { d=sqrt_tpl(d); t.set(-b-d,a); bHit = isneg(fabs_tpl(t.x*2-t.y)-t.y); t.x += d*(bHit^1)*2; bHit = isneg(fabs_tpl(t.x*2-t.y)-t.y); if (!bHit) return 0; pinters->pt[0]=pinters->pt[1] = pray->origin + pray->dir*t.val(); pinters->n = (pinters->pt[0]-psphere->center).normalized(); pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x20; return 1; } return 0; } int ray_sphere_intersection(const ray *pray, const sphere *psphere, prim_inters *pinters) { int res = sphere_ray_intersection(psphere,pray,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int sphere_plane_intersection(const sphere *psphere, const plane *pplane, prim_inters *pinters) { pinters->pt[0]=pinters->pt[1] = psphere->center-pplane->n*psphere->r; pinters->n = -pplane->n; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x40; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x40; return isneg((pinters->pt[0]-pplane->origin)*pplane->n); } int plane_sphere_intersection(const plane *pplane, const sphere *psphere, prim_inters *pinters) { int res = sphere_plane_intersection(psphere,pplane,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; } int ray_plane_intersection(const ray *pray, const plane *pplane, prim_inters *pinters) { quotient t(pplane->n*(pplane->origin-pray->origin), pray->dir*pplane->n); if (t.y-isneg(t.x*2-t.y)) return 0; pinters->pt[0]=pinters->pt[1] = pray->origin + pray->dir*t.val(); pinters->n = -pplane->n; pinters->iFeature[0][0]=pinters->iFeature[1][0] = 0x20; pinters->iFeature[0][1]=pinters->iFeature[1][1] = 0x40; return 1; } int plane_ray_intersection(const plane *pplane, const ray *pray, prim_inters *pinters) { int res = ray_plane_intersection(pray,pplane,pinters), iFeature; iFeature=pinters->iFeature[0][0]; pinters->iFeature[0][0]=pinters->iFeature[1][1]; pinters->iFeature[1][1]=iFeature; iFeature=pinters->iFeature[0][1]; pinters->iFeature[0][1]=pinters->iFeature[1][0]; pinters->iFeature[1][0]=iFeature; return res; }