1

/* $Id$ */

     2

     3

/** @file aystar.cpp */

     4

     5

/*

     6

 * This file has the core function for AyStar

     7

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

     8

 *  AI_pathfinding and Train_pathfinding.

     9

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

    10

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

    11

 */

    12

    13

/*

    14

 * Friendly reminder:

    15

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

    16

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

    17

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

    18

 * should call clear() yourself!

    19

 */

    20

    21

#include "stdafx.h"

    22

#include "openttd.h"

    23

#include "aystar.h"

    24

#include "core/alloc_func.hpp"

    25

    26

int _aystar_stats_open_size;

    27

int _aystar_stats_closed_size;

    28

    29

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

    30

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

    31

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

    32

{

    33

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

    34

}

    35

    36

// This adds a node to the ClosedList

    37

//  It makes a copy of the data

    38

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

    39

{

    40

	// Add a node to the ClosedList

    41

	PathNode *new_node = MallocT<PathNode>(1);

    42

	*new_node = *node;

    43

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

    44

}

    45

    46

// Checks if a node is in the OpenList

    47

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

    48

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

    49

{

    50

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

    51

}

    52

    53

// Gets the best node from OpenList

    54

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

    55

// Also it deletes the node from the OpenList

    56

static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar)

    57

{

    58

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

    59

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

    60

	if (res != NULL) {

    61

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

    62

	}

    63

    64

	return res;

    65

}

    66

    67

// Adds a node to the OpenList

    68

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

    69

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

    70

{

    71

	// Add a new Node to the OpenList

    72

	OpenListNode *new_node = MallocT<OpenListNode>(1);

    73

	new_node->g = g;

    74

	new_node->path.parent = parent;

    75

	new_node->path.node = *node;

    76

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

    77

    78

	// Add it to the queue

    79

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

    80

}

    81

    82

/*

    83

 * Checks one tile and calculate his f-value

    84

 *  return values:

    85

 * AYSTAR_DONE : indicates we are done

    86

 */

    87

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

    88

{

    89

	int new_f, new_g, new_h;

    90

	PathNode *closedlist_parent;

    91

	OpenListNode *check;

    92

    93

	// Check the new node against the ClosedList

    94

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

    95

    96

	// Calculate the G-value for this node

    97

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

    98

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

    99

	if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE;

   100

   101

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

   102

	assert(new_g >= 0);

   103

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

   104

	new_g += parent->g;

   105

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

   106

   107

	// Calculate the h-value

   108

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

   109

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

   110

	assert(new_h >= 0);

   111

   112

	// The f-value if g + h

   113

	new_f = new_g + new_h;

   114

   115

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

   116

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

   117

   118

	// Check if this item is already in the OpenList

   119

	check = AyStarMain_OpenList_IsInList(aystar, current);

   120

	if (check != NULL) {

   121

		uint i;

   122

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

   123

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

   124

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

   125

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

   126

		check->g = new_g;

   127

		check->path.parent = closedlist_parent;

   128

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

   129

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

   130

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

   131

		}

   132

		// Readd him in the OpenListQueue

   133

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

   134

	} else {

   135

		// A new node, add him to the OpenList

   136

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

   137

	}

   138

   139

	return AYSTAR_DONE;

   140

}

   141

   142

/*

   143

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

   144

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

   145

 *  return values:

   146

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

   147

 *    has been found.

   148

 *   AYSTAR_LIMIT_REACHED : Indicates that the max_nodes limit has been

   149

 *    reached.

   150

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

   151

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

   152

 */

   153

int AyStarMain_Loop(AyStar *aystar)

   154

{

   155

	int i, r;

   156

   157

	// Get the best node from OpenList

   158

	OpenListNode *current = AyStarMain_OpenList_Pop(aystar);

   159

	// If empty, drop an error

   160

	if (current == NULL) return AYSTAR_EMPTY_OPENLIST;

   161

   162

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

   163

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

   164

		if (aystar->FoundEndNode != NULL)

   165

			aystar->FoundEndNode(aystar, current);

   166

		free(current);

   167

		return AYSTAR_FOUND_END_NODE;

   168

	}

   169

   170

	// Add the node to the ClosedList

   171

	AyStarMain_ClosedList_Add(aystar, &current->path);

   172

   173

	// Load the neighbours

   174

	aystar->GetNeighbours(aystar, current);

   175

   176

	// Go through all neighbours

   177

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

   178

		// Check and add them to the OpenList if needed

   179

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

   180

	}

   181

   182

	// Free the node

   183

	free(current);

   184

   185

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

   186

		/* We've expanded enough nodes */

   187

		return AYSTAR_LIMIT_REACHED;

   188

	} else {

   189

		// Return that we are still busy

   190

		return AYSTAR_STILL_BUSY;

   191

	}

   192

}

   193

   194

/*

   195

 * This function frees the memory it allocated

   196

 */

   197

void AyStarMain_Free(AyStar *aystar)

   198

{

   199

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

   200

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

   201

	 * once */

   202

	delete_Hash(&aystar->OpenListHash, true);

   203

	delete_Hash(&aystar->ClosedListHash, true);

   204

#ifdef AYSTAR_DEBUG

   205

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

   206

#endif

   207

}

   208

   209

/*

   210

 * This function make the memory go back to zero

   211

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

   212

 */

   213

void AyStarMain_Clear(AyStar *aystar)

   214

{

   215

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

   216

	// the hash.

   217

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

   218

	// Clean the hashes

   219

	clear_Hash(&aystar->OpenListHash, true);

   220

	clear_Hash(&aystar->ClosedListHash, true);

   221

   222

#ifdef AYSTAR_DEBUG

   223

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

   224

#endif

   225

}

   226

   227

/*

   228

 * This is the function you call to run AyStar.

   229

 *  return values:

   230

 *   AYSTAR_FOUND_END_NODE : indicates we found an end node.

   231

 *   AYSTAR_NO_PATH : indicates that there was no path found.

   232

 *   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.

   233

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

   234

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

   235

 * you should still call clear() yourself!

   236

 */

   237

int AyStarMain_Main(AyStar *aystar) {

   238

	int r, i = 0;

   239

	// Loop through the OpenList

   240

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

   241

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

   242

#ifdef AYSTAR_DEBUG

   243

	switch (r) {

   244

		case AYSTAR_FOUND_END_NODE: printf("[AyStar] Found path!\n"); break;

   245

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

   246

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

   247

		default: break;

   248

	}

   249

#endif

   250

	if (r != AYSTAR_STILL_BUSY) {

   251

		/* We're done, clean up */

   252

		_aystar_stats_open_size = aystar->OpenListHash.size;

   253

		_aystar_stats_closed_size = aystar->ClosedListHash.size;

   254

		aystar->clear(aystar);

   255

	}

   256

   257

	switch (r) {

   258

		case AYSTAR_FOUND_END_NODE: return AYSTAR_FOUND_END_NODE;

   259

		case AYSTAR_EMPTY_OPENLIST:

   260

		case AYSTAR_LIMIT_REACHED:  return AYSTAR_NO_PATH;

   261

		default:                    return AYSTAR_STILL_BUSY;

   262

	}

   263

}

   264

   265

/*

   266

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

   267

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

   268

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

   269

 * clear() automatically when the algorithm finishes

   270

 * g is the cost for starting with this node.

   271

 */

   272

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

   273

{

   274

#ifdef AYSTAR_DEBUG

   275

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

   276

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

   277

#endif

   278

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

   279

}

   280

   281

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

   282

{

   283

	// Allocated the Hash for the OpenList and ClosedList

   284

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

   285

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

   286

   287

	// Set up our sorting queue

   288

	//  BinaryHeap allocates a block of 1024 nodes

   289

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

   290

	//  That is why it can stay this high

   291

	init_BinaryHeap(&aystar->OpenListQueue, 102400);

   292

   293

	aystar->addstart  = AyStarMain_AddStartNode;

   294

	aystar->main      = AyStarMain_Main;

   295

	aystar->loop      = AyStarMain_Loop;

   296

	aystar->free      = AyStarMain_Free;

   297

	aystar->clear     = AyStarMain_Clear;

   298

	aystar->checktile = AyStarMain_CheckTile;

   299

}