1

/*

     2

 * This file has the core function for AyStar

     3

 *  AyStar is a fast pathfinding routine and is used for things like

     4

 *  AI_pathfinding and Train_pathfinding.

     5

 *  For more information about AyStar (A* Algorithm), you can look at

     6

 *    http://en.wikipedia.org/wiki/A-star_search_algorithm

     7

 */

     8

     9

/*

    10

 * Friendly reminder:

    11

 *  Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory

    12

 *  And when not free'd, it can cause system-crashes.

    13

 * Also remember that when you stop an algorithm before it is finished, your

    14

 * should call clear() yourself!

    15

 */

    16

    17

#include "stdafx.h"

    18

#include "openttd.h"

    19

#include "aystar.h"

    20

// This looks in the Hash if a node exists in ClosedList

    21

//  If so, it returns the PathNode, else NULL

    22

static PathNode *AyStarMain_ClosedList_IsInList(AyStar *aystar, AyStarNode *node)

    23

{

    24

	return (PathNode*)Hash_Get(&aystar->ClosedListHash, node->tile, node->direction);

    25

}

    26

    27

// This adds a node to the ClosedList

    28

//  It makes a copy of the data

    29

static void AyStarMain_ClosedList_Add(AyStar *aystar, PathNode *node)

    30

{

    31

	// Add a node to the ClosedList

    32

	PathNode *new_node = malloc(sizeof(PathNode));

    33

	*new_node = *node;

    34

	Hash_Set(&aystar->ClosedListHash, node->node.tile, node->node.direction, new_node);

    35

}

    36

    37

// Checks if a node is in the OpenList

    38

//   If so, it returns the OpenListNode, else NULL

    39

static OpenListNode *AyStarMain_OpenList_IsInList(AyStar *aystar, AyStarNode *node)

    40

{

    41

	return (OpenListNode*)Hash_Get(&aystar->OpenListHash, node->tile, node->direction);

    42

}

    43

    44

// Gets the best node from OpenList

    45

//  returns the best node, or NULL of none is found

    46

// Also it deletes the node from the OpenList

    47

static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar)

    48

{

    49

	// Return the item the Queue returns.. the best next OpenList item.

    50

	OpenListNode* res = (OpenListNode*)aystar->OpenListQueue.pop(&aystar->OpenListQueue);

    51

	if (res != NULL)

    52

		Hash_Delete(&aystar->OpenListHash, res->path.node.tile, res->path.node.direction);

    53

    54

	return res;

    55

}

    56

    57

// Adds a node to the OpenList

    58

//  It makes a copy of node, and puts the pointer of parent in the struct

    59

static void AyStarMain_OpenList_Add(AyStar *aystar, PathNode *parent, AyStarNode *node, int f, int g)

    60

{

    61

	// Add a new Node to the OpenList

    62

	OpenListNode* new_node = malloc(sizeof(OpenListNode));

    63

	new_node->g = g;

    64

	new_node->path.parent = parent;

    65

	new_node->path.node = *node;

    66

	Hash_Set(&aystar->OpenListHash, node->tile, node->direction, new_node);

    67

    68

	// Add it to the queue

    69

	aystar->OpenListQueue.push(&aystar->OpenListQueue, new_node, f);

    70

}

    71

    72

/*

    73

 * Checks one tile and calculate his f-value

    74

 *  return values:

    75

 *	AYSTAR_DONE : indicates we are done

    76

 */

    77

int AyStarMain_CheckTile(AyStar *aystar, AyStarNode *current, OpenListNode *parent) {

    78

	int new_f, new_g, new_h;

    79

	PathNode *closedlist_parent;

    80

	OpenListNode *check;

    81

    82

	// Check the new node against the ClosedList

    83

	if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE;

    84

    85

	// Calculate the G-value for this node

    86

	new_g = aystar->CalculateG(aystar, current, parent);

    87

	// If the value was INVALID_NODE, we don't do anything with this node

    88

	if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE;

    89

    90

	// There should not be given any other error-code..

    91

	assert(new_g >= 0);

    92

	// Add the parent g-value to the new g-value

    93

	new_g += parent->g;

    94

	if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE;

    95

    96

	// Calculate the h-value

    97

	new_h = aystar->CalculateH(aystar, current, parent);

    98

	// There should not be given any error-code..

    99

	assert(new_h >= 0);

   100

   101

	// The f-value if g + h

   102

	new_f = new_g + new_h;

   103

   104

	// Get the pointer to the parent in the ClosedList (the currentone is to a copy of the one in the OpenList)

   105

	closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node);

   106

   107

	// Check if this item is already in the OpenList

   108

	if ((check = AyStarMain_OpenList_IsInList(aystar, current)) != NULL) {

   109

		uint i;

   110

		// Yes, check if this g value is lower..

   111

		if (new_g > check->g) return AYSTAR_DONE;

   112

		aystar->OpenListQueue.del(&aystar->OpenListQueue, check, 0);

   113

		// It is lower, so change it to this item

   114

		check->g = new_g;

   115

		check->path.parent = closedlist_parent;

   116

		/* Copy user data, will probably have changed */

   117

		for (i=0;i<lengthof(current->user_data);i++)

   118

			check->path.node.user_data[i] = current->user_data[i];

   119

		// Readd him in the OpenListQueue

   120

		aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f);

   121

	} else {

   122

		// A new node, add him to the OpenList

   123

		AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g);

   124

	}

   125

   126

	return AYSTAR_DONE;

   127

}

   128

   129

/*

   130

 * This function is the core of AyStar. It handles one item and checks

   131

 *  his neighbour items. If they are valid, they are added to be checked too.

   132

 *  return values:

   133

 *	AYSTAR_EMPTY_OPENLIST : indicates all items are tested, and no path

   134

 *	has been found.

   135

 *	AYSTAR_LIMIT_REACHED : Indicates that the max_nodes limit has been

   136

 *	reached.

   137

 *	AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found.

   138

 *	AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try.

   139

 */

   140

int AyStarMain_Loop(AyStar *aystar) {

   141

	int i, r;

   142

   143

	// Get the best node from OpenList

   144

	OpenListNode *current = AyStarMain_OpenList_Pop(aystar);

   145

	// If empty, drop an error

   146

	if (current == NULL) return AYSTAR_EMPTY_OPENLIST;

   147

   148

	// Check for end node and if found, return that code

   149

	if (aystar->EndNodeCheck(aystar, current) == AYSTAR_FOUND_END_NODE) {

   150

		if (aystar->FoundEndNode != NULL)

   151

			aystar->FoundEndNode(aystar, current);

   152

		free(current);

   153

		return AYSTAR_FOUND_END_NODE;

   154

	}

   155

   156

	// Add the node to the ClosedList

   157

	AyStarMain_ClosedList_Add(aystar, &current->path);

   158

   159

	// Load the neighbours

   160

	aystar->GetNeighbours(aystar, current);

   161

   162

	// Go through all neighbours

   163

	for (i=0;i<aystar->num_neighbours;i++) {

   164

		// Check and add them to the OpenList if needed

   165

		r = aystar->checktile(aystar, &aystar->neighbours[i], current);

   166

	}

   167

   168

	// Free the node

   169

	free(current);

   170

   171

	if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes)

   172

		/* We've expanded enough nodes */

   173

		return AYSTAR_LIMIT_REACHED;

   174

	else

   175

		// Return that we are still busy

   176

		return AYSTAR_STILL_BUSY;

   177

}

   178

   179

/*

   180

 * This function frees the memory it allocated

   181

 */

   182

void AyStarMain_Free(AyStar *aystar) {

   183

	aystar->OpenListQueue.free(&aystar->OpenListQueue, false);

   184

	/* 2nd argument above is false, below is true, to free the values only

   185

	 * once */

   186

	delete_Hash(&aystar->OpenListHash, true);

   187

	delete_Hash(&aystar->ClosedListHash, true);

   188

#ifdef AYSTAR_DEBUG

   189

	printf("[AyStar] Memory free'd\n");

   190

#endif

   191

}

   192

   193

/*

   194

 * This function make the memory go back to zero

   195

 *  This function should be called when you are using the same instance again.

   196

 */

   197

void AyStarMain_Clear(AyStar *aystar) {

   198

	// Clean the Queue, but not the elements within. That will be done by

   199

	// the hash.

   200

	aystar->OpenListQueue.clear(&aystar->OpenListQueue, false);

   201

	// Clean the hashes

   202

	clear_Hash(&aystar->OpenListHash, true);

   203

	clear_Hash(&aystar->ClosedListHash, true);

   204

   205

#ifdef AYSTAR_DEBUG

   206

	printf("[AyStar] Cleared AyStar\n");

   207

#endif

   208

}

   209

   210

/*

   211

 * This is the function you call to run AyStar.

   212

 *  return values:

   213

 *	AYSTAR_FOUND_END_NODE : indicates we found an end node.

   214

 *	AYSTAR_NO_PATH : indicates that there was no path found.

   215

 *	AYSTAR_STILL_BUSY : indicates we have done some checked, that we did not found the path yet, and that we still have items left to try.

   216

 * When the algorithm is done (when the return value is not AYSTAR_STILL_BUSY)

   217

 * aystar->clear() is called. Note that when you stop the algorithm halfway,

   218

 * you should still call clear() yourself!

   219

 */

   220

int AyStarMain_Main(AyStar *aystar) {

   221

	int r, i = 0;

   222

	// Loop through the OpenList

   223

	//  Quit if result is no AYSTAR_STILL_BUSY or is more than loops_per_tick

   224

	while ((r = aystar->loop(aystar)) == AYSTAR_STILL_BUSY && (aystar->loops_per_tick == 0 || ++i < aystar->loops_per_tick)) { }

   225

#ifdef AYSTAR_DEBUG

   226

	if (r == AYSTAR_FOUND_END_NODE)

   227

		printf("[AyStar] Found path!\n");

   228

	else if (r == AYSTAR_EMPTY_OPENLIST)

   229

		printf("[AyStar] OpenList run dry, no path found\n");

   230

	else if (r == AYSTAR_LIMIT_REACHED)

   231

		printf("[AyStar] Exceeded search_nodes, no path found\n");

   232

#endif

   237

	if (r != AYSTAR_STILL_BUSY)

   238

		/* We're done, clean up */

   239

		aystar->clear(aystar);

   240

   241

	// Check result-value

   242

	if (r == AYSTAR_FOUND_END_NODE) return AYSTAR_FOUND_END_NODE;

   243

	// Check if we have some left in the OpenList

   244

	if (r == AYSTAR_EMPTY_OPENLIST || r == AYSTAR_LIMIT_REACHED) return AYSTAR_NO_PATH;

   245

   246

	// Return we are still busy

   247

	return AYSTAR_STILL_BUSY;

   248

}

   249

   250

/*

   251

 * Adds a node from where to start an algorithm. Multiple nodes can be added

   252

 * if wanted. You should make sure that clear() is called before adding nodes

   253

 * if the AyStar has been used before (though the normal main loop calls

   254

 * clear() automatically when the algorithm finishes

   255

 * g is the cost for starting with this node.

   256

 */

   257

void AyStarMain_AddStartNode(AyStar *aystar, AyStarNode *start_node, uint g) {

   258

#ifdef AYSTAR_DEBUG

   259

	printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n",

   260

		TileX(start_node->tile), TileY(start_node->tile), start_node->direction);

   261

#endif

   262

	AyStarMain_OpenList_Add(aystar, NULL, start_node, 0, g);

   263

}

   264

   265

void init_AyStar(AyStar* aystar, Hash_HashProc hash, uint num_buckets) {

   266

	// Allocated the Hash for the OpenList and ClosedList

   267

	init_Hash(&aystar->OpenListHash, hash, num_buckets);

   268

	init_Hash(&aystar->ClosedListHash, hash, num_buckets);

   269

   270

	// Set up our sorting queue

   271

	//  BinaryHeap allocates a block of 1024 nodes

   272

	//  When thatone gets full it reserves an otherone, till this number

   273

	//  That is why it can stay this high

   274

	init_BinaryHeap(&aystar->OpenListQueue, 102400);

   275

   276

	aystar->addstart	= AyStarMain_AddStartNode;

   277

	aystar->main		= AyStarMain_Main;

   278

	aystar->loop		= AyStarMain_Loop;

   279

	aystar->free		= AyStarMain_Free;

   280

	aystar->clear		= AyStarMain_Clear;

   281

	aystar->checktile	= AyStarMain_CheckTile;

   282

}