1

/* $Id$ */

     2

     3

#include "../yapf_base.hpp"

     4

     5

struct CYapfMap1

     6

{

     7

	enum {xMax = 32, yMax = 68};

     8

	static int MapZ(int x, int y)

     9

	{

    10

		static const char *z1[yMax] = {

    11

			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",

    12

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    13

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    14

			"A000000000001000000000000000000000000000000000000000000000000000000A",

    15

			"A000000000001000000000000000000000000000000000000000000000000000000A",

    16

			"A000033333333333000000000000000000000000000000000000000000000000000A",

    17

			"A000030000000000000000000000000000000000000000000000000000000000000A",

    18

			"A000030000000000000000000000000000000000000000000000000000000000000A",

    19

			"A000030000000000000000000000000000000000000000000000000000000000000A",

    20

			"A000030000000000000000000000000000000000000000000000000000000000000A",

    21

			"A000030000000000000000000000000000000000000000000000000000000000000A",

    22

			"A210030000000000000000000000000000000000000000000000000000000000000A",

    23

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    24

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    25

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    26

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    27

			"A011333323333333233333333333333333333333333333333333333333333000000A",

    28

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    29

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    30

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    31

			"A210030000000000000000000000000000000000000000000000000000003000000A",

    32

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    33

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    34

			"A000230000000000000000000000000000000000000000000000000000003000000A",

    35

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    36

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    37

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    38

			"A000000000000000000000000000003333333333333333333333333333333000000A",

    39

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    40

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    41

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    42

			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",

    43

		};

    44

    45

		static const char *z2[yMax] = {

    46

			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",

    47

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    48

			"A003333333333333333333333333333333333333333333333333300000000000000A",

    49

			"A003000000001000000000000000000000000000000000000000300000000000000A",

    50

			"A003000000001000000000000000000000000000000000000000300000000000000A",

    51

			"A003333333333333333333333333333333333333300000000000300000000000000A",

    52

			"A000030000000000000000000000000000000000300000000000300000000000000A",

    53

			"A000030000000000000000000000000000000000333333333333300000000000000A",

    54

			"A000030000000000000000000000000000000000300000000000000000000000000A",

    55

			"A000030000000000000000000000000000000000300000000000000000000000000A",

    56

			"A000030000000000000000000000000000000000300000000000000000000000000A",

    57

			"A210030000000000000000000000000000000000300000000000000000000000000A",

    58

			"A000000000000000000000000000000000000000333300000000000000000000000A",

    59

			"A000000000000000000000000000000000000000000300000000000000000000000A",

    60

			"A000000000000000000000000000000000000000000300000000000000000000000A",

    61

			"A000000000000000000000000000000000000000000300000000000000000000000A",

    62

			"A012333323333333233333333333333333333333333333333333333333333000000A",

    63

			"A000030000000000000000000000000000000000000000000000000000003000000A",

    64

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    65

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    66

			"A210030000000000000000000000000000000000000000000000000330003000000A",

    67

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    68

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    69

			"A000230000000000000000000000000000000000000000000000000300003000000A",

    70

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    71

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    72

			"A000030000000000000000000000000000000000000000000000000300003000000A",

    73

			"A000000000000000000000000000003333333333333333333333333333333000000A",

    74

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    75

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    76

			"A000000000000000000000000000000000000000000000000000000000000000000A",

    77

			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",

    78

		};

    79

    80

		static const char **z = z1;

    81

    82

		if (x >= 0 && x < xMax && y >= 0 && y < yMax) {

    83

			int ret = z[x][y];

    84

			return ret;

    85

		}

    86

		return z[0][0];

    87

	}

    88

};

    89

    90

struct CNodeKey1 {

    91

	int            m_x;

    92

	int            m_y;

    93

	Trackdir       m_td;

    94

	DiagDirection  m_exitdir;

    95

    96

	CNodeKey1() : m_x(0), m_y(0), m_td(INVALID_TRACKDIR) {}

    97

    98

	int CalcHash() const {return m_x | (m_y << 5) | (m_td << 10);}

    99

	bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_td == other.m_td);}

   100

};

   101

   102

struct CNodeKey2 : public CNodeKey1

   103

{

   104

	int CalcHash() const {return m_x | (m_y << 5) | (m_exitdir << 10);}

   105

	bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_exitdir == other.m_exitdir);}

   106

};

   107

   108

template <class Tkey_>

   109

struct CTestYapfNodeT {

   110

	typedef Tkey_ Key;

   111

	typedef CTestYapfNodeT<Tkey_> Node;

   112

   113

	Tkey_           m_key;

   114

	CTestYapfNodeT *m_parent;

   115

	int             m_cost;

   116

	int             m_estimate;

   117

	CTestYapfNodeT *m_next;

   118

   119

	CTestYapfNodeT(CTestYapfNodeT* parent = NULL) : m_parent(parent), m_cost(0), m_estimate(0), m_next(NULL) {}

   120

   121

	const Tkey_& GetKey() const {return m_key;}

   122

	int GetCost() {return m_cost;}

   123

	int GetCostEstimate() {return m_estimate;}

   124

	bool operator < (const CTestYapfNodeT& other) const {return m_estimate < other.m_estimate;}

   125

	CTestYapfNodeT* GetHashNext() {return m_next;}

   126

	void SetHashNext(CTestYapfNodeT* next) {m_next = next;}

   127

};

   128

   129

typedef CTestYapfNodeT<CNodeKey1> CYapfNode1;

   130

typedef CTestYapfNodeT<CNodeKey2> CYapfNode2;

   131

   132

template <class Types>

   133

struct CYapfTestBaseT

   134

{

   135

	typedef typename Types::Tpf Tpf;              ///< the pathfinder class (derived from THIS class)

   136

	typedef typename Types::NodeList::Titem Node; ///< this will be our node type

   137

	typedef typename Node::Key Key;               ///< key to hash tables

   138

	typedef typename Types::Map Map;

   139

   140

	int m_x1, m_y1;

   141

	int m_x2, m_y2;

   142

	Trackdir m_td1;

   143

   144

	CYapfTestBaseT()

   145

		: m_x1(0), m_y1(0), m_x2(0), m_y2(0), m_td1(INVALID_TRACKDIR)

   146

	{

   147

	}

   148

   149

	void Set(int x1, int y1, int x2, int y2, Trackdir td1)

   150

	{

   151

		m_x1 = x1;

   152

		m_y1 = y1;

   153

		m_x2 = x2;

   154

		m_y2 = y2;

   155

		m_td1 = td1;

   156

	}

   157

   158

	/// to access inherited path finder

   159

	Tpf& Yapf() {return *static_cast<Tpf*>(this);}

   160

	FORCEINLINE char TransportTypeChar() const {return 'T';}

   161

   162

	/** Called by YAPF to move from the given node to the next tile. For each

   163

	 *  reachable trackdir on the new tile creates new node, initializes it

   164

	 *  and adds it to the open list by calling Yapf().AddNewNode(n) */

   165

	FORCEINLINE void PfFollowNode(Node& org)

   166

	{

   167

		int x_org = org.m_key.m_x;

   168

		int y_org = org.m_key.m_y;

   169

		int z_org = Map::MapZ(x_org, y_org);

   170

		DiagDirection exitdir = TrackdirToExitdir(org.m_key.m_td);

   171

   173

		int x_new = x_org + diff.x;

   174

		int y_new = y_org + diff.y;

   175

		int z_new = Map::MapZ(x_new, y_new);

   176

   177

		int z_diff = z_new - z_org;

   178

		if (abs(z_diff) > 1) return;

   179

   180

		TrackdirBits trackdirs = DiagdirReachesTrackdirs(exitdir);

   181

		TrackdirBits trackdirs90 = TrackdirCrossesTrackdirs(org.m_key.m_td);

   182

		trackdirs &= (TrackdirBits)~(int)trackdirs90;

   183

   184

		while (trackdirs != TRACKDIR_BIT_NONE) {

   185

			Trackdir td_new = (Trackdir)FindFirstBit2x64(trackdirs);

   186

			trackdirs = (TrackdirBits)KillFirstBit2x64(trackdirs);

   187

   188

			Node& n = Yapf().CreateNewNode();

   189

			n.m_key.m_x  =  x_new;

   190

			n.m_key.m_y  =  y_new;

   191

			n.m_key.m_td = td_new;

   192

			n.m_key.m_exitdir = TrackdirToExitdir(n.m_key.m_td);

   193

			n.m_parent   = &org;

   194

			Yapf().AddNewNode(n);

   195

		}

   196

	}

   197

   198

	/// Called when YAPF needs to place origin nodes into open list

   199

	FORCEINLINE void PfSetStartupNodes()

   200

	{

   201

		Node& n1 = Yapf().CreateNewNode();

   202

		n1.m_key.m_x = m_x1;

   203

		n1.m_key.m_y = m_y1;

   204

		n1.m_key.m_td = m_td1;

   205

		n1.m_key.m_exitdir = TrackdirToExitdir(n1.m_key.m_td);

   206

		Yapf().AddStartupNode(n1);

   207

	}

   208

   209

	/** Called by YAPF to calculate the cost from the origin to the given node.

   210

	 *  Calculates only the cost of given node, adds it to the parent node cost

   211

	 *  and stores the result into Node::m_cost member */

   212

	FORCEINLINE bool PfCalcCost(Node& n)

   213

	{

   214

		// base tile cost depending on distance

   215

		int c = IsDiagonalTrackdir(n.m_key.m_td) ? 10 : 7;

   216

		// additional penalty for curve

   217

		if (n.m_parent != NULL && n.m_key.m_td != n.m_parent->m_key.m_td) c += 3;

   218

		// z-difference cost

   219

		int z_new = Map::MapZ(n.m_key.m_x, n.m_key.m_y);

   220

		int z_old = Map::MapZ(n.m_parent->m_key.m_x, n.m_parent->m_key.m_y);

   221

		if (z_new > z_old) n.m_cost += (z_new - z_old) * 10;

   222

		// apply it

   223

		n.m_cost = n.m_parent->m_cost + c;

   224

		return true;

   225

	}

   226

   227

	/** Called by YAPF to calculate cost estimate. Calculates distance to the destination

   228

	 *  adds it to the actual cost from origin and stores the sum to the Node::m_estimate */

   229

	FORCEINLINE bool PfCalcEstimate(Node& n)

   230

	{

   231

		int dx = abs(n.m_key.m_x - m_x2);

   232

		int dy = abs(n.m_key.m_y - m_y2);

   233

		int dd = min(dx, dy);

   234

		int dxy = abs(dx - dy);

   235

		int d = 14 * dd + 10 * dxy;

   236

		n.m_estimate = n.m_cost + d /*+ d / 4*/;

   237

		return true;

   238

	}

   239

   240

	/// Called by YAPF to detect if node ends in the desired destination

   241

	FORCEINLINE bool PfDetectDestination(Node& n)

   242

	{

   243

		bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2);

   244

		return bDest;

   245

	}

   246

   247

	static int stTestAstar(bool silent)

   248

	{

   249

		Tpf pf;

   250

		pf.Set(3, 3, 20, 56, TRACKDIR_X_NE);

   251

		int ret = pf.TestAstar(silent);

   252

		return ret;

   253

	}

   254

   255

	int TestAstar(bool silent)

   256

	{

   257

		CPerformanceTimer pc;

   258

   259

		pc.Start();

   260

		bool bRet = Yapf().FindPath(NULL);

   261

		pc.Stop();

   262

   263

		if (!bRet) return 1;

   264

   265

		typedef CFixedSizeArrayT<int, 1024> Row;

   266

		typedef CFixedSizeArrayT<Row, 1024> Box;

   267

   268

		Box box;

   269

		{

   270

			for (int x = 0; x < Map::xMax; x++) {

   271

				Row& row = box.Add();

   272

				for (int y = 0; y < Map::yMax; y++) {

   273

					row.Add() = Map::MapZ(x, y);

   274

				}

   275

			}

   276

		}

   277

		int nPathTiles = 0;

   278

		{

   279

			for (Node* pNode = &Yapf().GetBestNode(); pNode != NULL; pNode = pNode->m_parent) {

   280

				box[pNode->m_key.m_x][pNode->m_key.m_y] = '.';

   281

				nPathTiles++;

   282

			}

   283

		}

   284

		{

   285

			printf("\n\n");

   286

			for (int x = 0; x < Map::xMax; x++) {

   287

				for (int y = 0; y < Map::yMax; y++) {

   288

					printf("%c", box[x][y]);

   289

				}

   290

				printf("\n");

   291

			}

   292

		}

   293

   294

		{

   295

			printf("\n");

   296

			printf("Path Tiles:    %6d\n", nPathTiles);

   297

//			printf("Closed nodes:  %6d\n", pf.m_nodes.ClosedCount());

   298

//			printf("Open nodes:    %6d\n", pf.m_nodes.OpenCount());

   299

//			printf("A-star rounds: %6d\n", pf.m_num_steps);

   300

		}

   301

		int total_time = pc.Get(1000000);

   302

		if (total_time != 0)

   303

			printf("Total time:    %6d us\n", pc.Get(1000000));

   304

   305

		printf("\n");

   306

   307

		{

   308

			int nCnt = Yapf().m_nodes.TotalCount();

   309

			for (int i = 0; i < nCnt; i++) {

   310

				Node& n = Yapf().m_nodes.ItemAt(i);

   311

				int& z = box[n.m_key.m_x][n.m_key.m_y];

   312

				z = (z < 'a') ? 'a' : (z + 1);

   313

			}

   314

		}

   315

		{

   316

			for (int x = 0; x < Map::xMax; x++) {

   317

				for (int y = 0; y < Map::yMax; y++) {

   318

					printf("%c", box[x][y]);

   319

				}

   320

				printf("\n");

   321

			}

   322

		}

   323

   324

		return 0;

   325

	}

   326

};

   327

   328

struct CDummy1 {};

   329

struct CDummy2 {};

   330

struct CDummy3 {};

   331

   332

template <class Tpf_, class Tnode_list, class Tmap>

   333

struct CYapf_TypesT

   334

{

   335

	typedef CYapf_TypesT<Tpf_, Tnode_list, Tmap> Types;

   336

   337

	typedef Tpf_                              Tpf;

   338

	typedef Tnode_list                        NodeList;

   339

	typedef Tmap                              Map;

   340

	typedef CYapfBaseT<Types>                 PfBase;

   341

	typedef CYapfTestBaseT<Types>             PfFollow;

   342

	typedef CDummy1                           PfOrigin;

   343

	typedef CDummy2                           PfDestination;

   344

	typedef CYapfSegmentCostCacheNoneT<Types> PfCache;

   345

	typedef CDummy3                           PfCost;

   346

};

   347

   348

typedef CNodeList_HashTableT<CYapfNode1, 12, 16> CNodeList1;

   349

typedef CNodeList_HashTableT<CYapfNode2, 12, 16> CNodeList2;

   350

   351

struct CTestYapf1

   352

	: public CYapfT<CYapf_TypesT<CTestYapf1, CNodeList1, CYapfMap1> >

   353

{

   354

};

   355

   356

struct CTestYapf2

   357

	: public CYapfT<CYapf_TypesT<CTestYapf2, CNodeList2, CYapfMap1> >

   358

{

   359

};