001 package com.croftsoft.core.ai.astar;
002
003 import java.util.*;
004
005 import com.croftsoft.core.lang.NullArgumentException;
006
007 /*********************************************************************
008 * The A* algorithm.
009 *
010 * @version
011 * 2003-05-09
012 * @since
013 * 2002-04-21
014 * @author
015 * <a href="https://www.croftsoft.com/">David Wallace Croft</a>
016 *********************************************************************/
017
018 public final class AStar
019 //////////////////////////////////////////////////////////////////////
020 //////////////////////////////////////////////////////////////////////
021 {
022
023 private final Cartographer cartographer;
024
025 private final List openNodeInfoSortedList;
026
027 private final Map nodeToNodeInfoMap;
028
029 //
030
031 private NodeInfo bestNodeInfo;
032
033 private double bestTotalCost;
034
035 private NodeInfo goalNodeInfo;
036
037 private boolean listEmpty;
038
039 //////////////////////////////////////////////////////////////////////
040 //////////////////////////////////////////////////////////////////////
041
042 public AStar ( Cartographer cartographer )
043 //////////////////////////////////////////////////////////////////////
044 {
045 NullArgumentException.check ( this.cartographer = cartographer );
046
047 nodeToNodeInfoMap = new HashMap ( );
048
049 openNodeInfoSortedList = new LinkedList ( );
050 }
051
052 //////////////////////////////////////////////////////////////////////
053 //////////////////////////////////////////////////////////////////////
054
055 public boolean isGoalFound ( ) { return goalNodeInfo != null; }
056
057 public boolean isListEmpty ( ) { return listEmpty; }
058
059 public Iterator getPath ( )
060 //////////////////////////////////////////////////////////////////////
061 {
062 List pathList = new LinkedList ( );
063
064 NodeInfo nodeInfo = goalNodeInfo;
065
066 if ( nodeInfo == null )
067 {
068 nodeInfo = bestNodeInfo;
069 }
070
071 while ( nodeInfo != null )
072 {
073 NodeInfo parentNodeInfo = nodeInfo.getParentNodeInfo ( );
074
075 if ( parentNodeInfo != null )
076 {
077 pathList.add ( 0, nodeInfo.getNode ( ) );
078 }
079
080 nodeInfo = parentNodeInfo;
081 }
082
083 return pathList.iterator ( );
084 }
085
086 public Object getFirstStep ( )
087 //////////////////////////////////////////////////////////////////////
088 {
089 NodeInfo nodeInfo = goalNodeInfo;
090
091 if ( nodeInfo == null )
092 {
093 nodeInfo = bestNodeInfo;
094 }
095
096 Object node = null;
097
098 while ( nodeInfo != null )
099 {
100 NodeInfo parentNodeInfo = nodeInfo.getParentNodeInfo ( );
101
102 if ( parentNodeInfo != null )
103 {
104 node = nodeInfo.getNode ( );
105 }
106
107 nodeInfo = parentNodeInfo;
108 }
109
110 return node;
111 }
112
113 //////////////////////////////////////////////////////////////////////
114 //////////////////////////////////////////////////////////////////////
115
116 public void reset ( Object startNode )
117 //////////////////////////////////////////////////////////////////////
118 {
119 goalNodeInfo = null;
120
121 listEmpty = false;
122
123 openNodeInfoSortedList.clear ( );
124
125 nodeToNodeInfoMap.clear ( );
126
127 NodeInfo nodeInfo = new NodeInfo ( startNode );
128
129 nodeToNodeInfoMap.put ( startNode, nodeInfo );
130
131 openNodeInfoSortedList.add ( nodeInfo );
132
133 bestTotalCost = Double.POSITIVE_INFINITY;
134 }
135
136 //////////////////////////////////////////////////////////////////////
137 //////////////////////////////////////////////////////////////////////
138
139 public boolean loop ( )
140 //////////////////////////////////////////////////////////////////////
141 {
142 if ( openNodeInfoSortedList.isEmpty ( ) )
143 {
144 listEmpty = true;
145
146 return false;
147 }
148
149 NodeInfo nodeInfo
150 = ( NodeInfo ) openNodeInfoSortedList.remove ( 0 );
151
152 Object node = nodeInfo.getNode ( );
153
154 if ( cartographer.isGoalNode ( node ) )
155 {
156 if ( ( goalNodeInfo == null )
157 || ( nodeInfo.getCostFromStart ( )
158 < goalNodeInfo.getCostFromStart ( ) ) )
159 {
160 goalNodeInfo = nodeInfo;
161 }
162
163 return false;
164 }
165
166 Iterator iterator = cartographer.getAdjacentNodes ( node );
167
168 while ( iterator.hasNext ( ) )
169 {
170 Object adjacentNode = iterator.next ( );
171
172 double newCostFromStart
173 = nodeInfo.getCostFromStart ( )
174 + cartographer.getCostToAdjacentNode ( node, adjacentNode );
175
176 NodeInfo adjacentNodeInfo
177 = ( NodeInfo ) nodeToNodeInfoMap.get ( adjacentNode );
178
179 if ( adjacentNodeInfo == null )
180 {
181 adjacentNodeInfo = new NodeInfo ( adjacentNode );
182
183 nodeToNodeInfoMap.put ( adjacentNode, adjacentNodeInfo );
184
185 openNodeInfoSortedList.add ( adjacentNodeInfo );
186 }
187 else if (
188 adjacentNodeInfo.getCostFromStart ( ) <= newCostFromStart )
189 {
190 continue;
191 }
192
193 adjacentNodeInfo.setParentNodeInfo ( nodeInfo );
194
195 adjacentNodeInfo.setCostFromStart ( newCostFromStart );
196
197 double totalCost = newCostFromStart
198 + cartographer.estimateCostToGoal ( adjacentNode );
199
200 adjacentNodeInfo.setTotalCost ( totalCost );
201
202 if ( totalCost < bestTotalCost )
203 {
204 bestNodeInfo = adjacentNodeInfo;
205
206 bestTotalCost = totalCost;
207 }
208
209 Collections.sort ( openNodeInfoSortedList );
210 }
211
212 return true;
213 }
214
215 //////////////////////////////////////////////////////////////////////
216 //////////////////////////////////////////////////////////////////////
217 }