KUDr@3900: /* $Id$ */
KUDr@3900: 
KUDr@3900: #include "../yapf_base.hpp"
KUDr@3900: 
KUDr@3900: struct CYapfMap1
KUDr@3900: {
KUDr@3900: 	enum {xMax = 32, yMax = 68};
KUDr@3900: 	static int MapZ(int x, int y)
KUDr@3900: 	{
KUDr@3900: 		static const char *z1[yMax] = {
KUDr@3900: 			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000001000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000001000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000033333333333000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A210030000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A011333323333333233333333333333333333333333333333333333333333000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A210030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000230000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000000000000000000000000000003333333333333333333333333333333000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
KUDr@3900: 		};
KUDr@3900: 
KUDr@3900: 		static const char *z2[yMax] = {
KUDr@3900: 			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A003333333333333333333333333333333333333333333333333300000000000000A",
KUDr@3900: 			"A003000000001000000000000000000000000000000000000000300000000000000A",
KUDr@3900: 			"A003000000001000000000000000000000000000000000000000300000000000000A",
KUDr@3900: 			"A003333333333333333333333333333333333333300000000000300000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000300000000000300000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000333333333333300000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000300000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000300000000000000000000000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000300000000000000000000000000A",
KUDr@3900: 			"A210030000000000000000000000000000000000300000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000333300000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000300000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000300000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000300000000000000000000000A",
KUDr@3900: 			"A012333323333333233333333333333333333333333333333333333333333000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000000003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A210030000000000000000000000000000000000000000000000000330003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000230000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000030000000000000000000000000000000000000000000000000300003000000A",
KUDr@3900: 			"A000000000000000000000000000003333333333333333333333333333333000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"A000000000000000000000000000000000000000000000000000000000000000000A",
KUDr@3900: 			"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
KUDr@3900: 		};
KUDr@3900: 
KUDr@3900: 		static const char **z = z1;
KUDr@3900: 
KUDr@3900: 		if (x >= 0 && x < xMax && y >= 0 && y < yMax) {
KUDr@3900: 			int ret = z[x][y];
KUDr@3900: 			return ret;
KUDr@3900: 		}
KUDr@3900: 		return z[0][0];
KUDr@3900: 	}
KUDr@3900: };
KUDr@3900: 
KUDr@3900: struct CNodeKey1 {
KUDr@3900: 	int            m_x;
KUDr@3900: 	int            m_y;
KUDr@3900: 	Trackdir       m_td;
KUDr@3900: 	DiagDirection  m_exitdir;
KUDr@3900: 
KUDr@3900: 	CNodeKey1() : m_x(0), m_y(0), m_td(INVALID_TRACKDIR) {}
KUDr@3900: 
KUDr@3900: 	int CalcHash() const {return m_x | (m_y << 5) | (m_td << 10);}
KUDr@3900: 	bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_td == other.m_td);}
KUDr@3900: };
KUDr@3900: 
KUDr@3900: struct CNodeKey2 : public CNodeKey1
KUDr@3900: {
KUDr@3900: 	int CalcHash() const {return m_x | (m_y << 5) | (m_exitdir << 10);}
KUDr@3900: 	bool operator == (const CNodeKey1& other) const {return (m_x == other.m_x) && (m_y == other.m_y) && (m_exitdir == other.m_exitdir);}
KUDr@3900: };
KUDr@3900: 
KUDr@3900: template <class Tkey_>
KUDr@3900: struct CTestYapfNodeT {
KUDr@3900: 	typedef Tkey_ Key;
KUDr@3900: 	typedef CTestYapfNodeT<Tkey_> Node;
KUDr@3900: 
KUDr@3900: 	Tkey_           m_key;
KUDr@3900: 	CTestYapfNodeT *m_parent;
KUDr@3900: 	int             m_cost;
KUDr@3900: 	int             m_estimate;
KUDr@3900: 	CTestYapfNodeT *m_next;
KUDr@3900: 
KUDr@3900: 	CTestYapfNodeT(CTestYapfNodeT* parent = NULL) : m_parent(parent), m_cost(0), m_estimate(0), m_next(NULL) {}
KUDr@3900: 
KUDr@3900: 	const Tkey_& GetKey() const {return m_key;}
KUDr@3900: 	int GetCost() {return m_cost;}
KUDr@3900: 	int GetCostEstimate() {return m_estimate;}
KUDr@3900: 	bool operator < (const CTestYapfNodeT& other) const {return m_estimate < other.m_estimate;}
KUDr@3900: 	CTestYapfNodeT* GetHashNext() {return m_next;}
KUDr@3900: 	void SetHashNext(CTestYapfNodeT* next) {m_next = next;}
KUDr@3900: };
KUDr@3900: 
KUDr@3900: typedef CTestYapfNodeT<CNodeKey1> CYapfNode1;
KUDr@3900: typedef CTestYapfNodeT<CNodeKey2> CYapfNode2;
KUDr@3900: 
KUDr@3900: template <class Types>
KUDr@3900: struct CYapfTestBaseT
KUDr@3900: {
KUDr@3900: 	typedef typename Types::Tpf Tpf;
KUDr@3900: 	typedef typename Types::NodeList::Titem Node; ///< this will be our node type
KUDr@3900: 	typedef typename Node::Key Key;    ///< key to hash tables
KUDr@3900: 	typedef typename Types::Map Map;
KUDr@3900: 
KUDr@3900: 	int m_x1, m_y1;
KUDr@3900: 	int m_x2, m_y2;
KUDr@3900: 	Trackdir m_td1;
KUDr@3900: 
KUDr@3900: 	CYapfTestBaseT()
KUDr@3900: 		: m_x1(0), m_y1(0), m_x2(0), m_y2(0), m_td1(INVALID_TRACKDIR)
KUDr@3900: 	{
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	void Set(int x1, int y1, int x2, int y2, Trackdir td1)
KUDr@3900: 	{
KUDr@3900: 		m_x1 = x1;
KUDr@3900: 		m_y1 = y1;
KUDr@3900: 		m_x2 = x2;
KUDr@3900: 		m_y2 = y2;
KUDr@3900: 		m_td1 = td1;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	Tpf& Yapf() {return *static_cast<Tpf*>(this);}
KUDr@3900: 	FORCEINLINE char TransportTypeChar() const {return 'T';}
KUDr@3900: 
KUDr@3900: 	FORCEINLINE void PfFollowNode(Node& org)
KUDr@3900: 	{
KUDr@3900: 		int x_org = org.m_key.m_x;
KUDr@3900: 		int y_org = org.m_key.m_y;
KUDr@3900: 		int z_org = Map::MapZ(x_org, y_org);
KUDr@3900: 		DiagDirection exitdir = TrackdirToExitdir(org.m_key.m_td);
KUDr@3900: 
KUDr@3900: 		TileIndexDiffC diff = TileIndexDiffCByDir(exitdir);
KUDr@3900: 		int x_new = x_org + diff.x;
KUDr@3900: 		int y_new = y_org + diff.y;
KUDr@3900: 		int z_new = Map::MapZ(x_new, y_new);
KUDr@3900: 
KUDr@3900: 		int z_diff = z_new - z_org;
KUDr@3900: 		if (abs(z_diff) > 1) return;
KUDr@3900: 
KUDr@3900: 		TrackdirBits trackdirs = DiagdirReachesTrackdirs(exitdir);
KUDr@3900: 		TrackdirBits trackdirs90 = TrackdirCrossesTrackdirs(org.m_key.m_td);
KUDr@3900: 		trackdirs &= (TrackdirBits)~(int)trackdirs90;
KUDr@3900: 
KUDr@3900: 		while (trackdirs != TRACKDIR_BIT_NONE) {
KUDr@3900: 			Trackdir td_new = (Trackdir)FindFirstBit2x64(trackdirs);
KUDr@3900: 			trackdirs = (TrackdirBits)KillFirstBit2x64(trackdirs);
KUDr@3900: 
KUDr@3900: 			Node& n = Yapf().CreateNewNode();
KUDr@3900: 			n.m_key.m_x  =  x_new;
KUDr@3900: 			n.m_key.m_y  =  y_new;
KUDr@3900: 			n.m_key.m_td = td_new;
KUDr@3900: 			n.m_key.m_exitdir = TrackdirToExitdir(n.m_key.m_td);
KUDr@3900: 			n.m_parent   = &org;
KUDr@3900: 			Yapf().AddNewNode(n);
KUDr@3900: 		}
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	FORCEINLINE void PfSetStartupNodes()
KUDr@3900: 	{
KUDr@3900: 		Node& n1 = Yapf().CreateNewNode();
KUDr@3900: 		n1.m_key.m_x = m_x1;
KUDr@3900: 		n1.m_key.m_y = m_y1;
KUDr@3900: 		n1.m_key.m_td = m_td1;
KUDr@3900: 		n1.m_key.m_exitdir = TrackdirToExitdir(n1.m_key.m_td);
KUDr@3900: 		Yapf().AddStartupNode(n1);
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	FORCEINLINE bool PfCalcCost(Node& n)
KUDr@3900: 	{
KUDr@3900: 		// base tile cost depending on distance
KUDr@3900: 		int c = IsDiagonalTrackdir(n.m_key.m_td) ? 10 : 7;
KUDr@3900: 		// additional penalty for curve
KUDr@3900: 		if (n.m_parent != NULL && n.m_key.m_td != n.m_parent->m_key.m_td) c += 3;
KUDr@3900: 		// z-difference cost
KUDr@3900: 		int z_new = Map::MapZ(n.m_key.m_x, n.m_key.m_y);
KUDr@3900: 		int z_old = Map::MapZ(n.m_parent->m_key.m_x, n.m_parent->m_key.m_y);
KUDr@3900: 		if (z_new > z_old) n.m_cost += (z_new - z_old) * 10;
KUDr@3900: 		// apply it
KUDr@3900: 		n.m_cost = n.m_parent->m_cost + c;
KUDr@3900: 		return true;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	FORCEINLINE bool PfCalcEstimate(Node& n)
KUDr@3900: 	{
KUDr@3900: 		int dx = abs(n.m_key.m_x - m_x2);
KUDr@3900: 		int dy = abs(n.m_key.m_y - m_y2);
KUDr@3900: 		int dd = min(dx, dy);
KUDr@3900: 		int dxy = abs(dx - dy);
KUDr@3900: 		int d = 14 * dd + 10 * dxy;
KUDr@3900: 		n.m_estimate = n.m_cost + d /*+ d / 4*/;
KUDr@3900: 		return true;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	FORCEINLINE bool PfDetectDestination(Node& n)
KUDr@3900: 	{
KUDr@3900: 		bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2);
KUDr@3900: 		return bDest;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	static int stTestAstar(bool silent)
KUDr@3900: 	{
KUDr@3900: 		Tpf pf;
KUDr@3900: 		pf.Set(3, 3, 20, 56, TRACKDIR_X_NE);
KUDr@3900: 		int ret = pf.TestAstar(silent);
KUDr@3900: 		return ret;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	int TestAstar(bool silent)
KUDr@3900: 	{
KUDr@3900: 		CPerformanceTimer pc;
KUDr@3900: 
KUDr@3900: 		pc.Start();
KUDr@3900: 		bool bRet = Yapf().FindPath(NULL);
KUDr@3900: 		pc.Stop();
KUDr@3900: 
KUDr@3900: 		if (!bRet) return 1;
KUDr@3900: 
KUDr@3900: 		typedef CFixedSizeArrayT<int, 1024> Row;
KUDr@3900: 		typedef CFixedSizeArrayT<Row, 1024> Box;
KUDr@3900: 
KUDr@3900: 		Box box;
KUDr@3900: 		{
KUDr@3900: 			for (int x = 0; x < Map::xMax; x++) {
KUDr@3900: 				Row& row = box.Add();
KUDr@3900: 				for (int y = 0; y < Map::yMax; y++) {
KUDr@3900: 					row.Add() = Map::MapZ(x, y);
KUDr@3900: 				}
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 		int nPathTiles = 0;
KUDr@3900: 		{
KUDr@3900: 			for (Node* pNode = &Yapf().GetBestNode(); pNode != NULL; pNode = pNode->m_parent) {
KUDr@3900: 				box[pNode->m_key.m_x][pNode->m_key.m_y] = '.';
KUDr@3900: 				nPathTiles++;
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 		{
KUDr@3900: 			printf("\n\n");
KUDr@3900: 			for (int x = 0; x < Map::xMax; x++) {
KUDr@3900: 				for (int y = 0; y < Map::yMax; y++) {
KUDr@3900: 					printf("%c", box[x][y]);
KUDr@3900: 				}
KUDr@3900: 				printf("\n");
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		{
KUDr@3900: 			printf("\n");
KUDr@3900: 			printf("Path Tiles:    %6d\n", nPathTiles);
KUDr@3900: //			printf("Closed nodes:  %6d\n", pf.m_nodes.ClosedCount());
KUDr@3900: //			printf("Open nodes:    %6d\n", pf.m_nodes.OpenCount());
KUDr@3900: //			printf("A-star rounds: %6d\n", pf.m_num_steps);
KUDr@3900: 		}
KUDr@3900: 		int total_time = pc.Get(1000000);
KUDr@3900: 		if (total_time != 0)
KUDr@3900: 			printf("Total time:    %6d us\n", pc.Get(1000000));
KUDr@3900: 
KUDr@3900: 		printf("\n");
KUDr@3900: 
KUDr@3900: 		{
KUDr@3900: 			int nCnt = Yapf().m_nodes.TotalCount();
KUDr@3900: 			for (int i = 0; i < nCnt; i++) {
KUDr@3900: 				Node& n = Yapf().m_nodes.ItemAt(i);
KUDr@3900: 				int& z = box[n.m_key.m_x][n.m_key.m_y];
KUDr@3900: 				z = (z < 'a') ? 'a' : (z + 1);
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 		{
KUDr@3900: 			for (int x = 0; x < Map::xMax; x++) {
KUDr@3900: 				for (int y = 0; y < Map::yMax; y++) {
KUDr@3900: 					printf("%c", box[x][y]);
KUDr@3900: 				}
KUDr@3900: 				printf("\n");
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		return 0;
KUDr@3900: 	}
KUDr@3900: };
KUDr@3900: 
KUDr@3900: struct CDummy1 {};
KUDr@3900: struct CDummy2 {};
KUDr@3900: struct CDummy3 {};
KUDr@3900: 
KUDr@3900: template <class Tpf_, class Tnode_list, class Tmap>
KUDr@3900: struct CYapf_TypesT
KUDr@3900: {
KUDr@3900: 	typedef CYapf_TypesT<Tpf_, Tnode_list, Tmap> Types;
KUDr@3900: 
KUDr@3900: 	typedef Tpf_                              Tpf;
KUDr@3900: 	typedef Tnode_list                        NodeList;
KUDr@3900: 	typedef Tmap                              Map;
KUDr@3900: 	typedef CYapfBaseT<Types>                 PfBase;
KUDr@3900: 	typedef CYapfTestBaseT<Types>             PfFollow;
KUDr@3900: 	typedef CDummy1                           PfOrigin;
KUDr@3900: 	typedef CDummy2                           PfDestination;
KUDr@3900: 	typedef CYapfSegmentCostCacheNoneT<Types> PfCache;
KUDr@3900: 	typedef CDummy3                           PfCost;
KUDr@3900: };
KUDr@3900: 
KUDr@3900: typedef CNodeList_HashTableT<CYapfNode1, 12, 16> CNodeList1;
KUDr@3900: typedef CNodeList_HashTableT<CYapfNode2, 12, 16> CNodeList2;
KUDr@3900: 
KUDr@3900: struct CTestYapf1
KUDr@3900: 	: public CYapfT<CYapf_TypesT<CTestYapf1, CNodeList1, CYapfMap1> >
KUDr@3900: {
KUDr@3900: };
KUDr@3900: 
KUDr@3900: struct CTestYapf2
KUDr@3900: 	: public CYapfT<CYapf_TypesT<CTestYapf2, CNodeList2, CYapfMap1> >
KUDr@3900: {
KUDr@3900: };
KUDr@3900: 
KUDr@3900: