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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225

226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276

/*
 *  AnnotatedAStar.cpp
 *  hog
 *
 *  Created by Daniel Harabor on 14/12/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "AnnotatedAStar.h"
#include "AnnotatedMapAbstraction.h"
#include "timer.h"

using namespace std;

/*
	1. get the current node on the open list
	2. check if node is the goal; goto 6 if yes.
	3. evaluate each neighbour of the newly opened node
		b. if neighbour is on the closed list, skip it
		a. if neighbour is already on the open list, update weights
		c. else check if node is reachable by the agent
			i. if reachable, push node on the open list
			ii. else, skip node
	4. push node on closed list
	5. if openlist is null return failure
	6. else, goto 1
*/
path* AnnotatedAStar::getPath(graphAbstraction *aMap, node *from, node* to, reservationProvider *rp)
{
	nodesExpanded=0;
	nodesTouched=0;
	peakmemory = 0;
	searchtime =0;

	if(aMap == NULL || !dynamic_cast<AbstractAnnotatedMapAbstraction*>(aMap))
		return NULL;
		
	setGraphAbstraction(aMap);
	int clearance = this->getClearance();
	int capability = this->getCapability();

	if(clearance <= 0)
	{	
		//if(verbose) std::cout << "AnnotatedAStar: attempted to getPath for agentsize <= 0" << std::endl;
		return NULL;
	}
	
	if(!from || !to)
		return NULL;

	if(from->getUniqueID() == to->getUniqueID())
		return NULL;
		
	if(from->getLabelL(kFirstData) == to->getLabelL(kFirstData) && from->getLabelL(kFirstData+1) == to->getLabelL(kFirstData+1))
		return NULL;

	/* both locations need to be reachable by agent */
	if(from->getClearance(capability) < clearance || to->getClearance(capability) < clearance) 
		return NULL;		
	
	//TODO: need a test to check that we've set the fCost value of the start node.
	// label start node cost 0 
	from->setLabelF(kTemporaryLabel, 1*aMap->h(from, to));
	from->markEdge(0);
	
	/* initialise the search params */
	graph *g = aMap->getAbstractGraph(from->getLabelL(kAbstractionLevel));
	heap* openList = new heap(30);
	AAStarUtil::NodeMap closedList;
	openList->add(from);
	path *p = NULL;
	
	Timer t;
	
	if(useCorridor)
		this->setCorridorClusters(from->getParentCluster(),to->getParentCluster());
		
/*	int fromx = from->getLabelL(kFirstData);
	int fromy = from->getLabelL(kFirstData+1);
	int tox = to->getLabelL(kFirstData);
	int toy = to->getLabelL(kFirstData+1);
*/
	
	t.startTimer();
	while(1) 
	{
		/* get the current node on the open list and check if it contains the goal */
		peakmemory = openList->size()>peakmemory?openList->size():peakmemory;
		node* current = ((node*)openList->remove()); 
		//int cx = current->getLabelL(kFirstData);
		//int cy = current->getLabelL(kFirstData+1);
		nodesExpanded++;
		if(current == to)
		{
			p = extractBestPath(g, current->getNum());
			break;
		}
		
		/* evaluate each neighbour of the newly opened node */
		edge_iterator ei = current->getEdgeIter();
		e = current->edgeIterNext(ei);
		while(e)
		{
			// TODO: fix HOG's graph stuff; nodes identified using position in array instead of uniqueid. graph should just store a hash_map
			int neighbourid = e->getFrom()==current->getNum()?e->getTo():e->getFrom();
			node* neighbour = g->getNode(neighbourid);
			/*int nx = neighbour->getLabelL(kFirstData);
			int ny = neighbour->getLabelL(kFirstData+1);
			double weight = e->getWeight();*/

			if(!closedList[neighbour->getUniqueID()]) // skip nodes we've already closed
			{
				// if a node on the openlist is reachable via this new edge, relax the edge (see cormen et al)
				if(openList->isIn(neighbour)) 
				{	
					if(evaluate(current, neighbour)) 
					{
						relaxEdge(openList, g, e, current->getNum(), neighbourid, to); 
						nodesTouched++;
					}
				}
				else
				{
					if(evaluate(current, neighbour)) 
					{
						neighbour->setLabelF(kTemporaryLabel, MAXINT); // initial fCost = inifinity
						neighbour->setKeyLabel(kTemporaryLabel); // an initial key value for prioritisation in the openlist
						neighbour->markEdge(0);  // reset any marked edges (we use marked edges to backtrack over optimal path when goal is found)
						openList->add(neighbour);
						relaxEdge(openList, g, e, current->getNum(), neighbourid, to); 
						nodesTouched++;
					}
				}
				
			}
			e = current->edgeIterNext(ei);
		}

		closedList[current->getUniqueID()] = true;
		
		/* check if there is anything left to search; fail if not */
		if(openList->empty())
			break;
	}
	searchtime = t.endTimer();
	delete openList; 
	closedList.clear();
	return p;	
}

/* evaluate()
	check if it is possible to move from the current location to an adjacent target location.
	things we look for:
		- clearance value of the target >= minclearance
		- if the traversal involves a diagonal move, is there an equivalent 2-step move using the cardinal directions?
		
		NB: we assume that an edge exists between the current and target node parameters. Could check this explicitly but HOG's 
		implementation for this stuff is expensive (iteratres over all neighbours). We also only call this from getPath which ensures that
		we only evaluate pairs of connected nodes. 
		
		Probably need to make this function protected rather than public.
								
		Other stuff: 
			* need to move this into abstract implementation; if edge weight > 1.0 (ie. we're looking at an edge part of an abstract graph) then
			* we need to check the width of the corridor to determine if the edge is traversable.
		
			So, does that mean I don't need a separate AHA*?! The actual A* search should be identical to AA* except for this bit!
*/
bool AnnotatedAStar::evaluate(node* current, node* target)
{
	if(!current || !target)
		return false;
				
	AbstractAnnotatedMapAbstraction* ama = (AbstractAnnotatedMapAbstraction*)getGraphAbstraction();
	//graph *g = ama->getAbstractGraph(0);

	int tx, ty, tcl, tterr;
	tterr = target->getTerrainType();
	tcl = target->getClearance(this->getCapability());
	tx = target->getLabelL(kFirstData);
	ty = target->getLabelL(kFirstData+1);
	if(target->getClearance(this->getCapability()) < this->getClearance())
		return false;

	if(useCorridor && !isInCorridor(target))
		return false;

	/* check if we're moving in a cardinal direction */
	tDirection dir = getDirection(current, target);
	if(dir == kStay)
		return false;
		
	if(dir == kN || dir == kS || dir == kE || dir == kW)
		return true;
		
	/* check diagonal move is equivalent to 2-step cardinal move */
	int curx = current->getLabelL(kFirstData);
	int cury = current->getLabelL(kFirstData+1);

	switch(dir) // nb: use of abstractannotatedmapabstraction implies edge exists between each pair of nodes
	{
		case kNW:
			if(evaluate(current, ama->getNodeFromMap(curx-1,cury)) && evaluate(current, ama->getNodeFromMap(curx, cury-1)))
				return true;
			break;
		case kNE: 
			if(evaluate(current, ama->getNodeFromMap(curx+1,cury)) && evaluate(current, ama->getNodeFromMap(curx, cury-1)))
				return true;
			break;
		case kSE:
			if(evaluate(current, ama->getNodeFromMap(curx+1,cury)) && evaluate(current, ama->getNodeFromMap(curx, cury+1)))
				return true;
			break;
		case kSW:
			if(evaluate(current, ama->getNodeFromMap(curx-1,cury)) && evaluate(current, ama->getNodeFromMap(curx, cury+1)))
				return true;
			break;
		default:
			cerr << "\nfatal: edge weight > 1 but move direction is not diagonal!\n ";
			break;
	}
	
	return false;
}

/* given two adjacent locations, the current position and a target position, figure out which of the eight compass directions the move
is equivalent to (n,ne,e,se,s,sw,w,nw) 
TODO: there's alot of duplication; this method has an equivalent addPathToCache in almost every Unit class. Should merge into a static.
*/
tDirection AnnotatedAStar::getDirection(node* current, node* target)
{
		int deltax = current->getLabelL(kFirstData) - target->getLabelL(kFirstData);
		int deltay = current->getLabelL(kFirstData+1) - target->getLabelL(kFirstData+1);
		
		int dir = kStay;
		switch(deltax)
		{
			case 1: // add westerly component
				dir = kW;
				break;
			case -1: // add easterly component
				dir = kE;
				break;
			case 0:
				break;
			default: // not moving along x-axis
				return kStay;
		};
		
		switch(deltay)
		{
			case 1: // add northerly component
				dir = dir|kN;
				break;
			case -1: // add southerly component
				dir = dir|kS;
				break;
			case 0:
				break;
			default: // not moving along y-axis
				return kStay;
		}
		
		return (tDirection)dir;
}

void AnnotatedAStar::logFinalStats(statCollection *stats)
{
	stats->addStat("nodesExpanded",getName(),getNodesExpanded());
	stats->addStat("nodesTouched",getName(),getNodesTouched());
	stats->addStat("peakMemory",getName(),getPeakMemory());
	stats->addStat("searchTime",getName(),getSearchTime());
	stats->addStat("agentSize", getName(), (long)getClearance());
	stats->addStat("agentCapability", getName(), (long)getCapability());
}