Advertisement
Guest User

IntegratedSearch

a guest
Feb 19th, 2017
79
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 17.72 KB | None | 0 0
  1. package Algorithms;
  2.  
  3. import java.util.ArrayList;
  4. import java.util.Arrays;
  5. import java.util.HashMap;
  6. import java.util.LinkedList;
  7. import java.util.Map.Entry;
  8. import java.util.Set;
  9. import p1.Vertex; import p1.BinaryHeap;
  10.  
  11. public class IntegratedSearch extends SearchAlgorithm{
  12.  
  13. private double w2;
  14.  
  15. private char anchorHeuristic;
  16. private char h2 = 'e'; private char h3 = 'm';
  17. private char h4 = 'M'; private char h5 = 'D';
  18.  
  19. private BinaryHeap anchorFringe;
  20. private BinaryHeap h1Fringe; private BinaryHeap h2Fringe;
  21. private BinaryHeap h3Fringe; private BinaryHeap h4Fringe;
  22.  
  23. private HashMap<Integer,Integer> anchorFringeCoords;
  24. private HashMap<Integer,Integer> h1FringeCoords; private HashMap<Integer,Integer> h2FringeCoords;
  25. private HashMap<Integer,Integer> h3FringeCoords; private HashMap<Integer,Integer> h4FringeCoords;
  26.  
  27. private Vertex goalA; private Vertex goal1; private Vertex goal2;
  28. private Vertex goal3; private Vertex goal4;
  29.  
  30. int eCapacity = 999;
  31. private Vertex[] expandedV; private HashMap<Integer,Integer> expandedC;
  32. private int eIndex = 0; private int eGoalInd;
  33. public IntegratedSearch(int xStart, int yStart, int xGoal, int yGoal, char[][] mData, double w1, double w2, char anchorHeuristic)
  34. {
  35. setXStart(xStart); setXGoal(xGoal);
  36. setYStart(yStart); setYGoal(yGoal);
  37. mData[xStart][yStart] = '1'; mData[xGoal][yGoal] = '1';
  38. setW(w1); setW2(w2); setMData(mData);
  39. setAnchorHeuristic(anchorHeuristic);
  40. initializeSearch();
  41. }
  42.  
  43. @Override
  44. public LinkedList<Vertex> run()
  45. {
  46. Vertex currentAnchor = null;
  47. // int currAX = 0; int currAY = 0;
  48. while(anchorFringe.getMin().getF() < Double.MAX_VALUE)
  49. {
  50. currentAnchor = anchorFringe.getMin();
  51. // currAX = currentAnchor.getX(); currAY = currentAnchor.getY();
  52. //checks if anchor is in closed set,
  53. //if so, loop continues to next iteration (i.e. gets next vertex from anchorFringe)
  54. //int cpA = cantorPairing(currAX, currAY); int aInd;
  55. //aInd = anchorFringeCoords.get((Integer)cpA).intValue();
  56. if(currentAnchor.isClosed())
  57. {
  58. anchorFringe.pop(); continue;
  59. }
  60. Vertex currentV = null;
  61. int currX = 0; int currY = 0;
  62. fLoop: for(int i=1; i<5; i++)
  63. {
  64. //System.out.println(" fringe " + i);
  65. BinaryHeap currFringe = null; HashMap<Integer,Integer> currFringeCoords = null;
  66. Vertex currGoal; int cp;
  67. //determines if vertex is in closed set, if so, loop continues to next iteration
  68. //(i.e. retrieves next vertex from hiFringe)
  69. if(i==1)
  70. {
  71. currFringe = h1Fringe;
  72. currFringeCoords = h1FringeCoords;
  73. //currGoal = getGoal1();
  74. }
  75. else if(i==2)
  76. {
  77. currFringe = h2Fringe;
  78. currFringeCoords = h2FringeCoords;
  79. //currGoal = getGoal2();
  80. }
  81. else if(i==3)
  82. {
  83. currFringe = h3Fringe;
  84. currFringeCoords = h3FringeCoords;
  85. //currGoal = getGoal3();
  86. }
  87. else//i==4
  88. {
  89. currFringe = h4Fringe;
  90. currFringeCoords = h4FringeCoords;
  91. //currGoal = getGoal4();
  92. }
  93. //currFringeCoords = h1FringeCoords;
  94. //currGoal = getGoalA();
  95. int ind = -1;
  96. while(true)
  97. {
  98. if(currFringe.isEmpty())
  99. continue fLoop;
  100.  
  101. currentV = currFringe.getMin();
  102. currX = currentV.getX(); currY = currentV.getY();
  103. cp = cantorPairing(currX, currY);
  104. //ind = 0;
  105. //ind = currFringeCoords.get(new Integer(cp)).intValue();
  106. ind = expandedC.get(new Integer(cp)).intValue();
  107. // if(currFringe.getVertex2(ind).isClosed())
  108. // {
  109. // currFringe.pop(); continue;
  110. // }
  111. if(expandedV[ind].isClosed())
  112. {
  113. currFringe.pop(); continue;
  114. }
  115. else
  116. break;
  117. }
  118. Vertex globalGoal = expandedV[eGoalInd];
  119. //System.out.println("curr goal G: " + currGoal.getG());
  120. if(currentV.getF() <= getWeight2()*currentAnchor.getF())
  121. {
  122. if(globalGoal.getG() <= currentV.getF())
  123. {
  124. if(globalGoal.getG() < Double.MAX_VALUE)
  125. {
  126. System.out.println("goal reached");
  127. //this.fringeCoords = currFringeCoords;
  128. return getPath(globalGoal);
  129. }
  130. }
  131. else
  132. {
  133. currFringe.pop();
  134. expandState(currentV);
  135. //currentV.setClosed(true);
  136. expandedV[ind].setClosed(true);
  137. }
  138. }
  139. else
  140. {
  141. if(globalGoal.getG() <= currentAnchor.getF())
  142. {
  143. if(globalGoal.getG() < Double.MAX_VALUE)
  144. {
  145. System.out.println("goal reached");
  146. //this.fringeCoords = anchorFringeCoords;
  147. return getPath(globalGoal);
  148. }
  149. }
  150. else
  151. {
  152. anchorFringe.pop();
  153. expandState(currentAnchor);
  154. currentAnchor.setClosed(true);
  155. currentAnchor = anchorFringe.getMin();
  156. }
  157. }
  158. }
  159. if(anchorFringe.isEmpty())
  160. {
  161. System.out.println("anchor fringe empty"); break;
  162. }
  163. }
  164. return null;
  165. }
  166.  
  167. private void initializeSearch()
  168. {
  169. int cpS = cantorPairing(getXStart(), getYStart()); int cpG = cantorPairing(getXGoal(),getYGoal());
  170. expandedV = new Vertex[1000]; expandedC = new HashMap<Integer,Integer>(100,0.75f);
  171. this.fringeCoords = expandedC;
  172.  
  173. Vertex start = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  174. Vertex goal = new Vertex(getXGoal(), getYGoal(), mData[getYStart()][getYGoal()],null,Double.MAX_VALUE);
  175.  
  176. expandedV[eIndex] = start; expandedC.put(cpS, eIndex); eIndex++;
  177. eGoalInd = eIndex;
  178. expandedV[eIndex] = goal; expandedC.put(cpG, eIndex); eIndex++;
  179.  
  180. anchorFringe = new BinaryHeap(1000);
  181. h1Fringe = new BinaryHeap(1000); h2Fringe = new BinaryHeap(1000);
  182. h3Fringe = new BinaryHeap(1000); h4Fringe = new BinaryHeap(1000);
  183.  
  184. anchorFringeCoords = new HashMap<Integer,Integer>(100,0.75f);
  185. h1FringeCoords = new HashMap<Integer,Integer>(100,0.75f); h2FringeCoords = new HashMap<Integer,Integer>(100,0.75f);
  186. h3FringeCoords = new HashMap<Integer,Integer>(100,0.75f); h4FringeCoords = new HashMap<Integer,Integer>(100,0.75f);
  187.  
  188. Vertex startA = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  189. startA.setH(getW()*oneFourthEuclideanDistance(getXStart(),getYStart()));
  190. Vertex start1 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  191. start1.setH(getW()*euclideanDistance(getXStart(),getYStart()));
  192. Vertex start2 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  193. start2.setH(getW()*oneFourthManhattanDistance(getXStart(),getYStart()));
  194. Vertex start3 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  195. start3.setH(getW()*manhattanDistance(getXStart(),getYStart()));
  196. Vertex start4 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
  197. start4.setH(getW()*diagonalDistance(getXStart(),getYStart()));
  198.  
  199. Vertex goalA = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
  200. Vertex goal1 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
  201. Vertex goal2 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
  202. Vertex goal3 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
  203. Vertex goal4 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
  204.  
  205. anchorFringeCoords.put(cpS, new Integer(0));
  206. h1FringeCoords.put(cpS, new Integer(0)); h2FringeCoords.put(cpS, new Integer(0));
  207. h3FringeCoords.put(cpS, new Integer(0)); h4FringeCoords.put(cpS, new Integer(0));
  208. anchorFringe.insert(startA);
  209. h1Fringe.insert(start1); h2Fringe.insert(start2);
  210. h3Fringe.insert(start3); h4Fringe.insert(start4);
  211.  
  212. anchorFringeCoords.put(cpG, new Integer(1));
  213. h1FringeCoords.put(cpG, new Integer(1)); h2FringeCoords.put(cpG, new Integer(1));
  214. h3FringeCoords.put(cpG, new Integer(1)); h4FringeCoords.put(cpG, new Integer(1));
  215. anchorFringe.insert(goalA);
  216. h1Fringe.insert(goal1); h2Fringe.insert(goal2);
  217. h3Fringe.insert(goal3); h4Fringe.insert(goal4);
  218. }
  219.  
  220. private void expandState(Vertex v)
  221. {
  222. //System.out.println("eIndex: " + eIndex );
  223. int cpS = cantorPairing(v.getX(),v.getY());
  224. Vertex eV = expandedV[expandedC.get(cpS).intValue()];
  225. //System.out.println("expand state " + ind);
  226. HashMap<Integer,Integer> tempFringeCoords = null;;
  227. BinaryHeap tempFringe = null;
  228. // boolean openA = false; boolean openIA = false;
  229. // if(anchorFringeCoords.containsKey(cpS))
  230. // {
  231. // if(!anchorFringe.getVertex2(anchorFringeCoords.get(cpS)).isClosed())
  232. // openA = true;
  233. // }
  234. // if(expandedC.containsKey(cpS))
  235. // {
  236. // if(!expandedV[expandedC.get(cpS)].isClosed())
  237. // openA = true;
  238. // }
  239. //remove vertex from all open sets
  240. //int ind;
  241. for(int i = 0; i < 5; i++){
  242. if(i == 0)
  243. {
  244. tempFringe = this.anchorFringe;
  245. tempFringeCoords = this.anchorFringeCoords;
  246. }
  247. else if(i == 1)
  248. {
  249. tempFringe = this.h1Fringe;
  250. tempFringeCoords = this.h1FringeCoords;
  251. }
  252. else if(i == 2)
  253. {
  254. tempFringe = this.h2Fringe;
  255. tempFringeCoords = this.h2FringeCoords;
  256. }
  257. else if(i == 3)
  258. {
  259. tempFringe = this.h3Fringe;
  260. tempFringeCoords = this.h3FringeCoords;
  261. }
  262. else//ind == 4
  263. {
  264. tempFringe = this.h4Fringe;
  265. tempFringeCoords = this.h4FringeCoords;
  266. }
  267. if(tempFringeCoords.containsKey(cpS) && (tempFringe.getVertex2(tempFringeCoords.get(cpS).intValue()).getIndex() != -1))
  268. {
  269. //ind = tempFringeCoords.get(cpS).intValue();
  270. //if(tempFringe.getVertex2(ind).getIndex())
  271. //tempFringe.delete(tempFringe.getVertex2(ind).getIndex());
  272. }
  273. }
  274.  
  275. //set global g value
  276. int eInd = expandedC.get(cpS).intValue();
  277. expandedV[eInd].setG(v.getG());
  278.  
  279. int currX = v.getX(); int currY = v.getY();
  280. Vertex sP; int cp; Vertex sPA;
  281. for(int i=currX-1; i<currX+2; i++)
  282. {
  283. for(int j=currY-1; j<currY+2; j++)
  284. {
  285. if(!(i>=0 && i<=159 && j>=0 && j<=119) || (i==currX && j==currY))
  286. continue;
  287.  
  288. if(getMData()[i][j] == '0')
  289. {
  290. eIndex++;
  291. if(eIndex == eCapacity)
  292. resizeESet();
  293. continue;
  294. }
  295. cp = cantorPairing(i,j);
  296.  
  297. //successor was not generated in this search(i.e. has not been expanded)
  298. boolean expanded = expandedC.containsKey(cp);
  299. //System.out.println(i + "," + j);
  300. if(!expanded)
  301. {
  302. sP = new Vertex(i,j,getMData()[i][j],null,Double.MAX_VALUE);
  303. //will be added to expanded set
  304. if(getMData()[i][j] != '0')
  305. {
  306. expandedV[eIndex] = sP; expandedC.put(cp, eIndex); eIndex++;
  307. }
  308. else
  309. eIndex++;
  310.  
  311. if(eIndex == eCapacity)
  312. resizeESet();
  313.  
  314. //every time a vertex is expanded for the first time, it is added to the anchor open set
  315. if(anchorFringeCoords.containsKey(cp))
  316. sPA = anchorFringe.getVertex2(anchorFringeCoords.get(cp).intValue());
  317. else
  318. sPA = new Vertex(i, j, getMData()[i][j], null, Double.MAX_VALUE);
  319. }
  320. else
  321. {
  322. sP = expandedV[expandedC.get(cp).intValue()];
  323. //must've also been expanded in anchor at some point
  324. if(anchorFringeCoords.containsKey(cp))
  325. sPA = anchorFringe.getVertex2(anchorFringeCoords.get(cp).intValue());
  326. else
  327. sPA = new Vertex(i, j, getMData()[i][j], null, Double.MAX_VALUE);
  328. }
  329.  
  330. if(sP.getG() > v.getG() + getCost(v,sP))
  331. {
  332. sP.setG(v.getG() + getCost(v,sP)); sP.setParent(eV);
  333. //no need for h value in expanded set
  334.  
  335. sPA.setG(v.getG() + getCost(v,sP)); sPA.setParent(v);
  336. sPA.setH(getW()*oneFourthEuclideanDistance(i,j));
  337.  
  338. if(!sPA.isClosed())
  339. {
  340. if(!anchorFringeCoords.containsKey(cp))
  341. {
  342. anchorFringeCoords.put(cp, anchorFringe.getHeap2Size());
  343. anchorFringe.insert(sPA);
  344. }
  345. else
  346. anchorFringe.updateCost(sPA.getIndex(), sPA.getG());
  347.  
  348. BinaryHeap tempFringe2; HashMap<Integer,Integer> tempFCoords2;
  349. Vertex currS;
  350. if(!sP.isClosed())
  351. {
  352. for(int k=1; k<5; k++)
  353. {
  354. if(k==1)
  355. {
  356. tempFringe2 = h1Fringe;
  357. tempFCoords2 = h1FringeCoords;
  358. sP.setH(getW()*euclideanDistance(i,j));
  359. }
  360. else if(k==2)
  361. {
  362. tempFringe2 = h2Fringe;
  363. tempFCoords2 = h2FringeCoords;
  364. sP.setH(getW()*oneFourthManhattanDistance(i,j));
  365. }
  366. else if(k==3)
  367. {
  368. tempFringe2 = h3Fringe;
  369. tempFCoords2 = h3FringeCoords;
  370. sP.setH(getW()*oneFourthManhattanDistance(i,j));
  371. }
  372. else//k==4
  373. {
  374. tempFringe2 = h4Fringe;
  375. tempFCoords2 = h4FringeCoords;
  376. sP.setH(getW()*diagonalDistance(i,j));
  377. }
  378. if(sP.getF() <= getWeight2()*sPA.getF())
  379. {
  380. if(tempFCoords2.containsKey(cp))
  381. {
  382. currS = tempFringe2.getVertex2(tempFCoords2.get(cp).intValue());
  383. tempFringe2.updateCost(currS.getIndex(), sP.getG());
  384. }
  385. else
  386. {
  387. tempFCoords2.put(cp, tempFringe2.getHeap2Size());
  388. Vertex toAdd = new Vertex(i, j, getMData()[i][j], v, sP.getG());
  389. toAdd.setH(sP.getH());
  390. tempFringe2.insert(toAdd);
  391. }
  392. }
  393. }
  394. }
  395. }
  396. }
  397. }
  398. }
  399. }
  400.  
  401. private void setAnchorHeuristic(char anchorHeuristic)
  402. {
  403. this.anchorHeuristic = anchorHeuristic;
  404. }
  405.  
  406. private void setW2(double w2)
  407. {
  408. this.w2 = w2;
  409. }
  410.  
  411. private double getWeight2()
  412. {
  413. return this.w2;
  414. }
  415.  
  416. private void resizeESet()
  417. {
  418. eCapacity = eCapacity*2;
  419. expandedV = Arrays.copyOf(expandedV, eCapacity);
  420. }
  421.  
  422. // private Vertex getGoalA()
  423. // {
  424. // return this.goalA;
  425. // }
  426. // private Vertex getGoal1()
  427. // {
  428. // return this.goal1;
  429. // }
  430. // private Vertex getGoal2()
  431. // {
  432. // return this.goal2;
  433. // }
  434. // private Vertex getGoal3()
  435. // {
  436. // return this.goal3;
  437. // }
  438. // private Vertex getGoal4()
  439. // {
  440. // return this.goal4;
  441. // }
  442. // private void setGoal(Vertex goal, int ind)
  443. // {
  444. // if(ind == 0)
  445. // this.goalA = goal;
  446. // else if(ind == 1)
  447. // this.goal1 = goal;
  448. // else if(ind == 2)
  449. // this.goal2 = goal;
  450. // else if(ind == 3)
  451. // this.goal3 = goal;
  452. // else//ind == 4
  453. // this.goal4 = goal;
  454. // }
  455. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement