Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- package Algorithms;
- import java.util.ArrayList;
- import java.util.Arrays;
- import java.util.HashMap;
- import java.util.LinkedList;
- import java.util.Map.Entry;
- import java.util.Set;
- import p1.Vertex; import p1.BinaryHeap;
- public class IntegratedSearch extends SearchAlgorithm{
- private double w2;
- private char anchorHeuristic;
- private char h2 = 'e'; private char h3 = 'm';
- private char h4 = 'M'; private char h5 = 'D';
- private BinaryHeap anchorFringe;
- private BinaryHeap h1Fringe; private BinaryHeap h2Fringe;
- private BinaryHeap h3Fringe; private BinaryHeap h4Fringe;
- private HashMap<Integer,Integer> anchorFringeCoords;
- private HashMap<Integer,Integer> h1FringeCoords; private HashMap<Integer,Integer> h2FringeCoords;
- private HashMap<Integer,Integer> h3FringeCoords; private HashMap<Integer,Integer> h4FringeCoords;
- private Vertex goalA; private Vertex goal1; private Vertex goal2;
- private Vertex goal3; private Vertex goal4;
- int eCapacity = 999;
- private Vertex[] expandedV; private HashMap<Integer,Integer> expandedC;
- private int eIndex = 0; private int eGoalInd;
- public IntegratedSearch(int xStart, int yStart, int xGoal, int yGoal, char[][] mData, double w1, double w2, char anchorHeuristic)
- {
- setXStart(xStart); setXGoal(xGoal);
- setYStart(yStart); setYGoal(yGoal);
- mData[xStart][yStart] = '1'; mData[xGoal][yGoal] = '1';
- setW(w1); setW2(w2); setMData(mData);
- setAnchorHeuristic(anchorHeuristic);
- initializeSearch();
- }
- @Override
- public LinkedList<Vertex> run()
- {
- Vertex currentAnchor = null;
- // int currAX = 0; int currAY = 0;
- while(anchorFringe.getMin().getF() < Double.MAX_VALUE)
- {
- currentAnchor = anchorFringe.getMin();
- // currAX = currentAnchor.getX(); currAY = currentAnchor.getY();
- //checks if anchor is in closed set,
- //if so, loop continues to next iteration (i.e. gets next vertex from anchorFringe)
- //int cpA = cantorPairing(currAX, currAY); int aInd;
- //aInd = anchorFringeCoords.get((Integer)cpA).intValue();
- if(currentAnchor.isClosed())
- {
- anchorFringe.pop(); continue;
- }
- Vertex currentV = null;
- int currX = 0; int currY = 0;
- fLoop: for(int i=1; i<5; i++)
- {
- //System.out.println(" fringe " + i);
- BinaryHeap currFringe = null; HashMap<Integer,Integer> currFringeCoords = null;
- Vertex currGoal; int cp;
- //determines if vertex is in closed set, if so, loop continues to next iteration
- //(i.e. retrieves next vertex from hiFringe)
- if(i==1)
- {
- currFringe = h1Fringe;
- currFringeCoords = h1FringeCoords;
- //currGoal = getGoal1();
- }
- else if(i==2)
- {
- currFringe = h2Fringe;
- currFringeCoords = h2FringeCoords;
- //currGoal = getGoal2();
- }
- else if(i==3)
- {
- currFringe = h3Fringe;
- currFringeCoords = h3FringeCoords;
- //currGoal = getGoal3();
- }
- else//i==4
- {
- currFringe = h4Fringe;
- currFringeCoords = h4FringeCoords;
- //currGoal = getGoal4();
- }
- //currFringeCoords = h1FringeCoords;
- //currGoal = getGoalA();
- int ind = -1;
- while(true)
- {
- if(currFringe.isEmpty())
- continue fLoop;
- currentV = currFringe.getMin();
- currX = currentV.getX(); currY = currentV.getY();
- cp = cantorPairing(currX, currY);
- //ind = 0;
- //ind = currFringeCoords.get(new Integer(cp)).intValue();
- ind = expandedC.get(new Integer(cp)).intValue();
- // if(currFringe.getVertex2(ind).isClosed())
- // {
- // currFringe.pop(); continue;
- // }
- if(expandedV[ind].isClosed())
- {
- currFringe.pop(); continue;
- }
- else
- break;
- }
- Vertex globalGoal = expandedV[eGoalInd];
- //System.out.println("curr goal G: " + currGoal.getG());
- if(currentV.getF() <= getWeight2()*currentAnchor.getF())
- {
- if(globalGoal.getG() <= currentV.getF())
- {
- if(globalGoal.getG() < Double.MAX_VALUE)
- {
- System.out.println("goal reached");
- //this.fringeCoords = currFringeCoords;
- return getPath(globalGoal);
- }
- }
- else
- {
- currFringe.pop();
- expandState(currentV);
- //currentV.setClosed(true);
- expandedV[ind].setClosed(true);
- }
- }
- else
- {
- if(globalGoal.getG() <= currentAnchor.getF())
- {
- if(globalGoal.getG() < Double.MAX_VALUE)
- {
- System.out.println("goal reached");
- //this.fringeCoords = anchorFringeCoords;
- return getPath(globalGoal);
- }
- }
- else
- {
- anchorFringe.pop();
- expandState(currentAnchor);
- currentAnchor.setClosed(true);
- currentAnchor = anchorFringe.getMin();
- }
- }
- }
- if(anchorFringe.isEmpty())
- {
- System.out.println("anchor fringe empty"); break;
- }
- }
- return null;
- }
- private void initializeSearch()
- {
- int cpS = cantorPairing(getXStart(), getYStart()); int cpG = cantorPairing(getXGoal(),getYGoal());
- expandedV = new Vertex[1000]; expandedC = new HashMap<Integer,Integer>(100,0.75f);
- this.fringeCoords = expandedC;
- Vertex start = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- Vertex goal = new Vertex(getXGoal(), getYGoal(), mData[getYStart()][getYGoal()],null,Double.MAX_VALUE);
- expandedV[eIndex] = start; expandedC.put(cpS, eIndex); eIndex++;
- eGoalInd = eIndex;
- expandedV[eIndex] = goal; expandedC.put(cpG, eIndex); eIndex++;
- anchorFringe = new BinaryHeap(1000);
- h1Fringe = new BinaryHeap(1000); h2Fringe = new BinaryHeap(1000);
- h3Fringe = new BinaryHeap(1000); h4Fringe = new BinaryHeap(1000);
- anchorFringeCoords = new HashMap<Integer,Integer>(100,0.75f);
- h1FringeCoords = new HashMap<Integer,Integer>(100,0.75f); h2FringeCoords = new HashMap<Integer,Integer>(100,0.75f);
- h3FringeCoords = new HashMap<Integer,Integer>(100,0.75f); h4FringeCoords = new HashMap<Integer,Integer>(100,0.75f);
- Vertex startA = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- startA.setH(getW()*oneFourthEuclideanDistance(getXStart(),getYStart()));
- Vertex start1 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- start1.setH(getW()*euclideanDistance(getXStart(),getYStart()));
- Vertex start2 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- start2.setH(getW()*oneFourthManhattanDistance(getXStart(),getYStart()));
- Vertex start3 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- start3.setH(getW()*manhattanDistance(getXStart(),getYStart()));
- Vertex start4 = new Vertex(getXStart(),getYStart(),mData[getXStart()][getYStart()],null,0);
- start4.setH(getW()*diagonalDistance(getXStart(),getYStart()));
- Vertex goalA = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
- Vertex goal1 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
- Vertex goal2 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
- Vertex goal3 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
- Vertex goal4 = new Vertex(getXGoal(),getYGoal(),mData[getXGoal()][getYGoal()],null,Double.MAX_VALUE);
- anchorFringeCoords.put(cpS, new Integer(0));
- h1FringeCoords.put(cpS, new Integer(0)); h2FringeCoords.put(cpS, new Integer(0));
- h3FringeCoords.put(cpS, new Integer(0)); h4FringeCoords.put(cpS, new Integer(0));
- anchorFringe.insert(startA);
- h1Fringe.insert(start1); h2Fringe.insert(start2);
- h3Fringe.insert(start3); h4Fringe.insert(start4);
- anchorFringeCoords.put(cpG, new Integer(1));
- h1FringeCoords.put(cpG, new Integer(1)); h2FringeCoords.put(cpG, new Integer(1));
- h3FringeCoords.put(cpG, new Integer(1)); h4FringeCoords.put(cpG, new Integer(1));
- anchorFringe.insert(goalA);
- h1Fringe.insert(goal1); h2Fringe.insert(goal2);
- h3Fringe.insert(goal3); h4Fringe.insert(goal4);
- }
- private void expandState(Vertex v)
- {
- //System.out.println("eIndex: " + eIndex );
- int cpS = cantorPairing(v.getX(),v.getY());
- Vertex eV = expandedV[expandedC.get(cpS).intValue()];
- //System.out.println("expand state " + ind);
- HashMap<Integer,Integer> tempFringeCoords = null;;
- BinaryHeap tempFringe = null;
- // boolean openA = false; boolean openIA = false;
- // if(anchorFringeCoords.containsKey(cpS))
- // {
- // if(!anchorFringe.getVertex2(anchorFringeCoords.get(cpS)).isClosed())
- // openA = true;
- // }
- // if(expandedC.containsKey(cpS))
- // {
- // if(!expandedV[expandedC.get(cpS)].isClosed())
- // openA = true;
- // }
- //remove vertex from all open sets
- //int ind;
- for(int i = 0; i < 5; i++){
- if(i == 0)
- {
- tempFringe = this.anchorFringe;
- tempFringeCoords = this.anchorFringeCoords;
- }
- else if(i == 1)
- {
- tempFringe = this.h1Fringe;
- tempFringeCoords = this.h1FringeCoords;
- }
- else if(i == 2)
- {
- tempFringe = this.h2Fringe;
- tempFringeCoords = this.h2FringeCoords;
- }
- else if(i == 3)
- {
- tempFringe = this.h3Fringe;
- tempFringeCoords = this.h3FringeCoords;
- }
- else//ind == 4
- {
- tempFringe = this.h4Fringe;
- tempFringeCoords = this.h4FringeCoords;
- }
- if(tempFringeCoords.containsKey(cpS) && (tempFringe.getVertex2(tempFringeCoords.get(cpS).intValue()).getIndex() != -1))
- {
- //ind = tempFringeCoords.get(cpS).intValue();
- //if(tempFringe.getVertex2(ind).getIndex())
- //tempFringe.delete(tempFringe.getVertex2(ind).getIndex());
- }
- }
- //set global g value
- int eInd = expandedC.get(cpS).intValue();
- expandedV[eInd].setG(v.getG());
- int currX = v.getX(); int currY = v.getY();
- Vertex sP; int cp; Vertex sPA;
- for(int i=currX-1; i<currX+2; i++)
- {
- for(int j=currY-1; j<currY+2; j++)
- {
- if(!(i>=0 && i<=159 && j>=0 && j<=119) || (i==currX && j==currY))
- continue;
- if(getMData()[i][j] == '0')
- {
- eIndex++;
- if(eIndex == eCapacity)
- resizeESet();
- continue;
- }
- cp = cantorPairing(i,j);
- //successor was not generated in this search(i.e. has not been expanded)
- boolean expanded = expandedC.containsKey(cp);
- //System.out.println(i + "," + j);
- if(!expanded)
- {
- sP = new Vertex(i,j,getMData()[i][j],null,Double.MAX_VALUE);
- //will be added to expanded set
- if(getMData()[i][j] != '0')
- {
- expandedV[eIndex] = sP; expandedC.put(cp, eIndex); eIndex++;
- }
- else
- eIndex++;
- if(eIndex == eCapacity)
- resizeESet();
- //every time a vertex is expanded for the first time, it is added to the anchor open set
- if(anchorFringeCoords.containsKey(cp))
- sPA = anchorFringe.getVertex2(anchorFringeCoords.get(cp).intValue());
- else
- sPA = new Vertex(i, j, getMData()[i][j], null, Double.MAX_VALUE);
- }
- else
- {
- sP = expandedV[expandedC.get(cp).intValue()];
- //must've also been expanded in anchor at some point
- if(anchorFringeCoords.containsKey(cp))
- sPA = anchorFringe.getVertex2(anchorFringeCoords.get(cp).intValue());
- else
- sPA = new Vertex(i, j, getMData()[i][j], null, Double.MAX_VALUE);
- }
- if(sP.getG() > v.getG() + getCost(v,sP))
- {
- sP.setG(v.getG() + getCost(v,sP)); sP.setParent(eV);
- //no need for h value in expanded set
- sPA.setG(v.getG() + getCost(v,sP)); sPA.setParent(v);
- sPA.setH(getW()*oneFourthEuclideanDistance(i,j));
- if(!sPA.isClosed())
- {
- if(!anchorFringeCoords.containsKey(cp))
- {
- anchorFringeCoords.put(cp, anchorFringe.getHeap2Size());
- anchorFringe.insert(sPA);
- }
- else
- anchorFringe.updateCost(sPA.getIndex(), sPA.getG());
- BinaryHeap tempFringe2; HashMap<Integer,Integer> tempFCoords2;
- Vertex currS;
- if(!sP.isClosed())
- {
- for(int k=1; k<5; k++)
- {
- if(k==1)
- {
- tempFringe2 = h1Fringe;
- tempFCoords2 = h1FringeCoords;
- sP.setH(getW()*euclideanDistance(i,j));
- }
- else if(k==2)
- {
- tempFringe2 = h2Fringe;
- tempFCoords2 = h2FringeCoords;
- sP.setH(getW()*oneFourthManhattanDistance(i,j));
- }
- else if(k==3)
- {
- tempFringe2 = h3Fringe;
- tempFCoords2 = h3FringeCoords;
- sP.setH(getW()*oneFourthManhattanDistance(i,j));
- }
- else//k==4
- {
- tempFringe2 = h4Fringe;
- tempFCoords2 = h4FringeCoords;
- sP.setH(getW()*diagonalDistance(i,j));
- }
- if(sP.getF() <= getWeight2()*sPA.getF())
- {
- if(tempFCoords2.containsKey(cp))
- {
- currS = tempFringe2.getVertex2(tempFCoords2.get(cp).intValue());
- tempFringe2.updateCost(currS.getIndex(), sP.getG());
- }
- else
- {
- tempFCoords2.put(cp, tempFringe2.getHeap2Size());
- Vertex toAdd = new Vertex(i, j, getMData()[i][j], v, sP.getG());
- toAdd.setH(sP.getH());
- tempFringe2.insert(toAdd);
- }
- }
- }
- }
- }
- }
- }
- }
- }
- private void setAnchorHeuristic(char anchorHeuristic)
- {
- this.anchorHeuristic = anchorHeuristic;
- }
- private void setW2(double w2)
- {
- this.w2 = w2;
- }
- private double getWeight2()
- {
- return this.w2;
- }
- private void resizeESet()
- {
- eCapacity = eCapacity*2;
- expandedV = Arrays.copyOf(expandedV, eCapacity);
- }
- // private Vertex getGoalA()
- // {
- // return this.goalA;
- // }
- // private Vertex getGoal1()
- // {
- // return this.goal1;
- // }
- // private Vertex getGoal2()
- // {
- // return this.goal2;
- // }
- // private Vertex getGoal3()
- // {
- // return this.goal3;
- // }
- // private Vertex getGoal4()
- // {
- // return this.goal4;
- // }
- // private void setGoal(Vertex goal, int ind)
- // {
- // if(ind == 0)
- // this.goalA = goal;
- // else if(ind == 1)
- // this.goal1 = goal;
- // else if(ind == 2)
- // this.goal2 = goal;
- // else if(ind == 3)
- // this.goal3 = goal;
- // else//ind == 4
- // this.goal4 = goal;
- // }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement