-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathBidirectional.java
137 lines (125 loc) · 5.42 KB
/
Bidirectional.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
package algorithm;
//Bidirectional Search Algorithm
//Created By Armin
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;
public class Bidirectional {
public static ArrayList<Action> search(BidirectionalProblem p){
return search(p,true);
}
public static ArrayList<Action> search(BidirectionalProblem p,boolean isGraphSearch){
Queue<Expandable> QueueA = new LinkedList<>();
Queue<Expandable> QueueB = new LinkedList<>();
ArrayList<Expandable> closedA = new ArrayList<>();
ArrayList<Expandable> closedB = new ArrayList<>();
Expandable initA = new Expandable();
initA.state = p.initialState();
initA.actionSequence = new ArrayList<>();
Expandable initB = new Expandable();
initB.state = p.goalState();
initB.actionSequence = new ArrayList<>();
QueueA.add(initA);
QueueB.add(initB);
while(!QueueA.isEmpty() && !QueueB.isEmpty()){
ArrayList<Action> intersectionResult = intersect(p,closedA,closedB);
if(intersectionResult != null){
//Goal Reached
System.out.println("[Bidirectional] Goal Reached !");
return intersectionResult;
}else{
Expandable a = QueueA.remove();
Expandable b = QueueB.remove();
//Close Current State
closedA.add(a);
closedB.add(b);
//Expand Childs of A
for(Action act : p.actions(a.state)){
for(State targetState : p.result(a.state,act)) {
boolean mustAdd = true;
if(isGraphSearch){
for (Expandable closedE : closedA) {
if (closedE.state.isEquals(targetState)) {
mustAdd = false;
break;
}
}
}
for (Expandable openState : QueueA) {
if (openState.state.isEquals(targetState)) {
mustAdd = false;
break;
}
}
if (mustAdd) {
Expandable E = new Expandable();
E.state = targetState;
//Clone Parent Action Sequence
ArrayList<Action> asClone = new ArrayList<>();
for (Action sa : a.actionSequence) {
asClone.add(sa);
}
asClone.add(act);
E.actionSequence = asClone;
QueueA.add(E);
}
}
}
//Expand Childs of B
for(Action act : p.actionsBd(b.state)){
for(State targetState : p.resultBd(b.state,act)) {
boolean mustAdd = true;
if(isGraphSearch){
for (Expandable closedE : closedB) {
if (closedE.state.isEquals(targetState)) {
mustAdd = false;
break;
}
}
}
for (Expandable openState : QueueB) {
if (openState.state.isEquals(targetState)) {
mustAdd = false;
break;
}
}
if (mustAdd) {
Expandable E = new Expandable();
E.state = targetState;
//Clone Parent Action Sequence
ArrayList<Action> asClone = new ArrayList<>();
for (Action sa : b.actionSequence) {
asClone.add(sa);
}
asClone.add(act);
E.actionSequence = asClone;
QueueB.add(E);
}
}
}
}
}
//There is no answer to algorithm.Problem
System.err.println("[Bidirectional] No Answer !");
return null;
}
private static ArrayList<Action> intersect(BidirectionalProblem p , ArrayList<Expandable> qOrigin , ArrayList<Expandable> qGoal){
for(Expandable eOrigin : qOrigin){
for(Expandable eGoal : qGoal){
if(eOrigin.state.isEquals(eGoal.state)){
ArrayList<Action> finalActions = new ArrayList<>();
//Add Actions from Origin Normally
for (int i = 0; i < eOrigin.actionSequence.size(); i++) {
finalActions.add(eOrigin.actionSequence.get(i));
}
//Add Actions from Goal Reversed
for (int i = eGoal.actionSequence.size() - 1; i >=0 ; i--) {
finalActions.add(eGoal.actionSequence.get(i));
}
return finalActions;
}
}
}
return null;
}
}