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="http://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         }