Advertisement
Guest User

Untitled

a guest
Jun 11th, 2017
141
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.54 KB | None | 0 0
  1. // 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
  2.  
  3. int msa2d_poly_poly1(v2 *ppoint, v2 *paxis, float *pdepth, v2 polyorg0, v2 polyorg1, int numpoly0, int numpoly1, v2 *poly0, v2 *poly1)
  4. {
  5. //
  6. float cexpandrad=0.0;
  7.  
  8. v2 rorg=polyorg1-polyorg0;
  9. v2 sdir=-rorg;
  10. //sdir.norm();
  11. sdir=sdir/v2dp(sdir,sdir);
  12.  
  13. v2 simplex[2];
  14. v2 simplexverts0[2]; //poly0
  15. v2 simplexverts1[2]; //poly1
  16.  
  17. v2 sv0;
  18. v2 sv1;
  19.  
  20. //support along relative origin ray
  21. sv0=msa2d_polysupport(numpoly0,poly0,-sdir);
  22. sv1=msa2d_polysupport(numpoly1,poly1, sdir);
  23. simplex[0]=(sv0) - (sv1);// - sdir*cexpandrad;
  24. simplexverts0[0]=sv0;
  25. simplexverts1[0]=sv1;
  26.  
  27. //separating axis?
  28. if (v2dp(simplex[0]-rorg,sdir)>0.0) return 0;
  29.  
  30. //project support onto relative origin ray, subtract support
  31. v2 projsdir=(sdir*v2dp(simplex[0],sdir))-simplex[0];
  32. //projsdir.norm();
  33.  
  34. //support along projected ray
  35. sv0=msa2d_polysupport(numpoly0,poly0, projsdir);
  36. sv1=msa2d_polysupport(numpoly1,poly1,-projsdir);
  37. simplex[1]=(sv0) - (sv1);// + projsdir*cexpandrad;
  38. simplexverts0[1]=sv0;
  39. simplexverts1[1]=sv1;
  40.  
  41. //separating axis?
  42. if (v2dp(simplex[1]-rorg,projsdir)<0.0) return 0;
  43.  
  44. v2 pn=-v2perp(simplex[0]-simplex[1]);
  45. //pn.norm();
  46.  
  47. /*
  48. //flip if necessary
  49. if (v2dp(sdir,pn)>0)
  50. {
  51. v2 tvert=simplex[0];
  52. simplex[0]=simplex[1];
  53. simplex[1]=tvert;
  54.  
  55. tvert=simplexverts0[0];
  56. simplexverts0[0]=simplexverts0[1];
  57. simplexverts0[1]=tvert;
  58.  
  59. tvert=simplexverts1[0];
  60. simplexverts1[0]=simplexverts1[1];
  61. simplexverts1[1]=tvert;
  62.  
  63. pn=-pn;
  64. }*/
  65.  
  66. /*
  67. //refinement loop
  68. int MAX_ITERATIONS=10;
  69. for (int i=0;i<MAX_ITERATIONS;i++)
  70. {
  71. //find support for current simplex axis
  72. sv0 = msa2d_polysupport(numpoly0,poly0, pn);
  73. sv1 = msa2d_polysupport(numpoly1,poly1,-pn);
  74. v2 supvert = sv0 - sv1;// + pn*cexpandrad;
  75.  
  76. //separating axis?
  77. v2 sdiff= (rorg-supvert);
  78. if (v2dp(sdiff,pn)>0.0) return 0;
  79.  
  80. v2 sdir0= (supvert-simplex[0]);
  81. v2 sdir1= (supvert-simplex[1]);
  82.  
  83. v2 projvec0=sdiff-(sdir0*v2dp(sdir0,sdiff));
  84. v2 projvec1=sdiff-(sdir1*v2dp(sdir1,sdiff));
  85.  
  86. float cpd[2];
  87. cpd[0]=v2dp(projvec0,-v2perp(sdir0));
  88. cpd[1]=v2dp(projvec1, v2perp(sdir1));
  89.  
  90. //keep larger
  91. if (cpd[0]>cpd[1])
  92. {
  93. simplex[1]=supvert;
  94. simplexverts0[1]=sv0;
  95. simplexverts1[1]=sv1;
  96. }
  97. else
  98. {
  99. simplex[0]=supvert;
  100. simplexverts0[0]=sv0;
  101. simplexverts1[0]=sv1;
  102. }
  103.  
  104. //search axis
  105. v2 supedgedir=simplex[0]-simplex[1];
  106.  
  107. //if search direction is similar, terminate
  108. if (fabsf(v2dp(pn,supedgedir))<=1e-3) break;
  109.  
  110. //if degenerate, terminate
  111. if (v2dp(supedgedir,supedgedir)<=1e-3) break;
  112.  
  113. //create axis
  114. pn=(v2perp(supedgedir));
  115. }
  116. */
  117.  
  118.  
  119. //refinement loop
  120. int MAX_ITERATIONS=10;
  121. for (int i=0;i<MAX_ITERATIONS;i++)
  122. {
  123. //find support for current simplex axis
  124. sv0 = msa2d_polysupport(numpoly0,poly0, pn);
  125. sv1 = msa2d_polysupport(numpoly1,poly1,-pn);
  126. v2 supvert = (sv0) - (sv1);// + pn*cexpandrad;
  127.  
  128. //separating axis?
  129. v2 svrodir=supvert-rorg;
  130. if (v2dp(svrodir,pn)<0.0) return 0;
  131.  
  132. //create candidate axes
  133. v2 sed[2]={supvert-simplex[0],supvert-simplex[1]};
  134. v2 cpn[2]={v2perp(sed[0]),-v2perp(sed[1])};
  135. //cpn[0].norm();
  136. //cpn[1].norm();
  137.  
  138. //relative origin's projection on candidate axes
  139. float cpd[2];
  140. cpd[0]=v2dp(svrodir,cpn[0]);
  141. cpd[1]=v2dp(svrodir,cpn[1]);
  142.  
  143. //search axis
  144. v2 supedgedir;
  145. //v2 npn;
  146. //keep larger
  147. if (cpd[0]>cpd[1])
  148. {
  149. simplex[1]=supvert;
  150. simplexverts0[1]=sv0;
  151. simplexverts1[1]=sv1;
  152. supedgedir=-sed[0];
  153. //npn=-cpn[0];
  154. }
  155. else
  156. {
  157. simplex[0]=supvert;
  158. simplexverts0[0]=sv0;
  159. simplexverts1[0]=sv1;
  160. supedgedir= sed[1];
  161. //npn=-cpn[1];
  162. }
  163.  
  164. //if search direction is similar, terminate
  165. if (fabsf(v2dp(pn,supedgedir))<=1e-3) break;
  166.  
  167. //if degenerate, terminate
  168. if (v2dp(supedgedir,supedgedir)<=1e-3) break;
  169.  
  170. //pn=npn;
  171.  
  172. //create axis
  173. pn=(v2perp(supedgedir));
  174. //pn.norm();
  175. }
  176.  
  177. //separating axis?
  178. float cpdepth=v2dp(simplex[0]-rorg,pn);
  179.  
  180. if (cpdepth<0.0)
  181. return 0;
  182. else
  183. {
  184. v2 tcpoint=(simplexverts0[0]+simplexverts0[1]+simplexverts1[0]+simplexverts1[1])*0.25+(polyorg0+polyorg1)*0.5;
  185. //v2 tcpoint=(simplexverts0[0]+simplexverts1[0]+polyorg0+polyorg1)*0.5;
  186. //*paxis=pn;
  187. //*pdepth=cpdepth;
  188. pn.norm();
  189. *paxis=pn;
  190. *ppoint=tcpoint;
  191. *pdepth=v2dp(simplex[0]-rorg,pn);
  192. //*pdepth=v2dp((simplex[0]+simplex[1])*0.5-rorg,pn);
  193. return 1;
  194. }
  195. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement