Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // MSAR - minimal C, 2d version, mind the operator overloading. Figure it out, BSD license, you can extend it to arbitrary dimensions and arbitrary convex compounds
- int msa2d_poly_poly1(v2 *ppoint, v2 *paxis, float *pdepth, v2 polyorg0, v2 polyorg1, int numpoly0, int numpoly1, v2 *poly0, v2 *poly1)
- {
- //
- float cexpandrad=0.0;
- v2 rorg=polyorg1-polyorg0;
- v2 sdir=-rorg;
- //sdir.norm();
- sdir=sdir/v2dp(sdir,sdir);
- v2 simplex[2];
- v2 simplexverts0[2]; //poly0
- v2 simplexverts1[2]; //poly1
- v2 sv0;
- v2 sv1;
- //support along relative origin ray
- sv0=msa2d_polysupport(numpoly0,poly0,-sdir);
- sv1=msa2d_polysupport(numpoly1,poly1, sdir);
- simplex[0]=(sv0) - (sv1);// - sdir*cexpandrad;
- simplexverts0[0]=sv0;
- simplexverts1[0]=sv1;
- //separating axis?
- if (v2dp(simplex[0]-rorg,sdir)>0.0) return 0;
- //project support onto relative origin ray, subtract support
- v2 projsdir=(sdir*v2dp(simplex[0],sdir))-simplex[0];
- //projsdir.norm();
- //support along projected ray
- sv0=msa2d_polysupport(numpoly0,poly0, projsdir);
- sv1=msa2d_polysupport(numpoly1,poly1,-projsdir);
- simplex[1]=(sv0) - (sv1);// + projsdir*cexpandrad;
- simplexverts0[1]=sv0;
- simplexverts1[1]=sv1;
- //separating axis?
- if (v2dp(simplex[1]-rorg,projsdir)<0.0) return 0;
- v2 pn=-v2perp(simplex[0]-simplex[1]);
- //pn.norm();
- /*
- //flip if necessary
- if (v2dp(sdir,pn)>0)
- {
- v2 tvert=simplex[0];
- simplex[0]=simplex[1];
- simplex[1]=tvert;
- tvert=simplexverts0[0];
- simplexverts0[0]=simplexverts0[1];
- simplexverts0[1]=tvert;
- tvert=simplexverts1[0];
- simplexverts1[0]=simplexverts1[1];
- simplexverts1[1]=tvert;
- pn=-pn;
- }*/
- /*
- //refinement loop
- int MAX_ITERATIONS=10;
- for (int i=0;i<MAX_ITERATIONS;i++)
- {
- //find support for current simplex axis
- sv0 = msa2d_polysupport(numpoly0,poly0, pn);
- sv1 = msa2d_polysupport(numpoly1,poly1,-pn);
- v2 supvert = sv0 - sv1;// + pn*cexpandrad;
- //separating axis?
- v2 sdiff= (rorg-supvert);
- if (v2dp(sdiff,pn)>0.0) return 0;
- v2 sdir0= (supvert-simplex[0]);
- v2 sdir1= (supvert-simplex[1]);
- v2 projvec0=sdiff-(sdir0*v2dp(sdir0,sdiff));
- v2 projvec1=sdiff-(sdir1*v2dp(sdir1,sdiff));
- float cpd[2];
- cpd[0]=v2dp(projvec0,-v2perp(sdir0));
- cpd[1]=v2dp(projvec1, v2perp(sdir1));
- //keep larger
- if (cpd[0]>cpd[1])
- {
- simplex[1]=supvert;
- simplexverts0[1]=sv0;
- simplexverts1[1]=sv1;
- }
- else
- {
- simplex[0]=supvert;
- simplexverts0[0]=sv0;
- simplexverts1[0]=sv1;
- }
- //search axis
- v2 supedgedir=simplex[0]-simplex[1];
- //if search direction is similar, terminate
- if (fabsf(v2dp(pn,supedgedir))<=1e-3) break;
- //if degenerate, terminate
- if (v2dp(supedgedir,supedgedir)<=1e-3) break;
- //create axis
- pn=(v2perp(supedgedir));
- }
- */
- //refinement loop
- int MAX_ITERATIONS=10;
- for (int i=0;i<MAX_ITERATIONS;i++)
- {
- //find support for current simplex axis
- sv0 = msa2d_polysupport(numpoly0,poly0, pn);
- sv1 = msa2d_polysupport(numpoly1,poly1,-pn);
- v2 supvert = (sv0) - (sv1);// + pn*cexpandrad;
- //separating axis?
- v2 svrodir=supvert-rorg;
- if (v2dp(svrodir,pn)<0.0) return 0;
- //create candidate axes
- v2 sed[2]={supvert-simplex[0],supvert-simplex[1]};
- v2 cpn[2]={v2perp(sed[0]),-v2perp(sed[1])};
- //cpn[0].norm();
- //cpn[1].norm();
- //relative origin's projection on candidate axes
- float cpd[2];
- cpd[0]=v2dp(svrodir,cpn[0]);
- cpd[1]=v2dp(svrodir,cpn[1]);
- //search axis
- v2 supedgedir;
- //v2 npn;
- //keep larger
- if (cpd[0]>cpd[1])
- {
- simplex[1]=supvert;
- simplexverts0[1]=sv0;
- simplexverts1[1]=sv1;
- supedgedir=-sed[0];
- //npn=-cpn[0];
- }
- else
- {
- simplex[0]=supvert;
- simplexverts0[0]=sv0;
- simplexverts1[0]=sv1;
- supedgedir= sed[1];
- //npn=-cpn[1];
- }
- //if search direction is similar, terminate
- if (fabsf(v2dp(pn,supedgedir))<=1e-3) break;
- //if degenerate, terminate
- if (v2dp(supedgedir,supedgedir)<=1e-3) break;
- //pn=npn;
- //create axis
- pn=(v2perp(supedgedir));
- //pn.norm();
- }
- //separating axis?
- float cpdepth=v2dp(simplex[0]-rorg,pn);
- if (cpdepth<0.0)
- return 0;
- else
- {
- v2 tcpoint=(simplexverts0[0]+simplexverts0[1]+simplexverts1[0]+simplexverts1[1])*0.25+(polyorg0+polyorg1)*0.5;
- //v2 tcpoint=(simplexverts0[0]+simplexverts1[0]+polyorg0+polyorg1)*0.5;
- //*paxis=pn;
- //*pdepth=cpdepth;
- pn.norm();
- *paxis=pn;
- *ppoint=tcpoint;
- *pdepth=v2dp(simplex[0]-rorg,pn);
- //*pdepth=v2dp((simplex[0]+simplex[1])*0.5-rorg,pn);
- return 1;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement