KUDr@3900: /* $Id$ */
KUDr@3900: 
KUDr@3900: #ifndef  YAPF_BASE_HPP
KUDr@3900: #define  YAPF_BASE_HPP
KUDr@3900: 
KUDr@3900: EXTERN_C_BEGIN
KUDr@3900: #include "../debug.h"
KUDr@3900: EXTERN_C_END
KUDr@3900: 
KUDr@3900: #include "fixedsizearray.hpp"
KUDr@3900: #include "blob.hpp"
KUDr@3900: #include "nodelist.hpp"
KUDr@3900: 
KUDr@3900: extern int _total_pf_time_us;
KUDr@3900: 
KUDr@3900: /** CYapfBaseT - A-star type path finder base class.
rubidium@4549:  *  Derive your own pathfinder from it. You must provide the following template argument:
rubidium@4549:  *    Types      - used as collection of local types used in pathfinder
rubidium@4549:  *
rubidium@4549:  * Requirements for the Types struct:
rubidium@4549:  *  ----------------------------------
rubidium@4549:  *  The following types must be defined in the 'Types' argument:
rubidium@4549:  *    - Types::Tpf - your pathfinder derived from CYapfBaseT
rubidium@4549:  *    - Types::NodeList - open/closed node list (look at CNodeList_HashTableT)
rubidium@4549:  *  NodeList needs to have defined local type Titem - defines the pathfinder node type.
rubidium@4549:  *  Node needs to define local type Key - the node key in the collection ()
rubidium@4549:  *
rubidium@4549:  *  For node list you can use template class CNodeList_HashTableT, for which
rubidium@4549:  *  you need to declare only your node type. Look at test_yapf.h for an example.
rubidium@4549:  *
rubidium@4549:  *
rubidium@4549:  *  Requrements to your pathfinder class derived from CYapfBaseT:
rubidium@4549:  *  -------------------------------------------------------------
rubidium@4549:  *  Your pathfinder derived class needs to implement following methods:
rubidium@4549:  *    FORCEINLINE void PfSetStartupNodes()
rubidium@4549:  *    FORCEINLINE void PfFollowNode(Node& org)
rubidium@4549:  *    FORCEINLINE bool PfCalcCost(Node& n)
rubidium@4549:  *    FORCEINLINE bool PfCalcEstimate(Node& n)
rubidium@4549:  *    FORCEINLINE bool PfDetectDestination(Node& n)
rubidium@4549:  *
rubidium@4549:  *  For more details about those methods, look at the end of CYapfBaseT
rubidium@4549:  *  declaration. There are some examples. For another example look at
rubidium@4549:  *  test_yapf.h (part or unittest project).
rubidium@4549:  */
KUDr@3900: template <class Types>
KUDr@3900: class CYapfBaseT {
KUDr@3900: public:
KUDr@3914: 	typedef typename Types::Tpf Tpf;           ///< the pathfinder class (derived from THIS class)
KUDr@3900: 	typedef typename Types::NodeList NodeList; ///< our node list
KUDr@3900: 	typedef typename NodeList::Titem Node;     ///< this will be our node type
KUDr@3900: 	typedef typename Node::Key Key;            ///< key to hash tables
KUDr@3900: 
KUDr@3900: 
KUDr@3914: 	NodeList             m_nodes;              ///< node list multi-container
KUDr@3900: protected:
KUDr@3914: 	Node*                m_pBestDestNode;      ///< pointer to the destination node found at last round
KUDr@3914: 	Node*                m_pBestIntermediateNode; ///< here should be node closest to the destination if path not found
KUDr@3914: 	const YapfSettings  *m_settings;           ///< current settings (_patches.yapf)
KUDr@3914: 	int                  m_max_search_nodes;   ///< maximum number of nodes we are allowed to visit before we give up
KUDr@3915: 	const Vehicle*       m_veh;                ///< vehicle that we are trying to drive
KUDr@3900: 
KUDr@3914: 	int                  m_stats_cost_calcs;   ///< stats - how many node's costs were calculated
KUDr@3914: 	int                  m_stats_cache_hits;   ///< stats - how many node's costs were reused from cache
KUDr@3900: 
KUDr@3900: public:
KUDr@3914: 	CPerformanceTimer    m_perf_cost;          ///< stats - total CPU time of this run
KUDr@3914: 	CPerformanceTimer    m_perf_slope_cost;    ///< stats - slope calculation CPU time
KUDr@3914: 	CPerformanceTimer    m_perf_ts_cost;       ///< stats - GetTrackStatus() CPU time
KUDr@3914: 	CPerformanceTimer    m_perf_other_cost;    ///< stats - other CPU time
KUDr@3900: 
KUDr@3900: public:
KUDr@3914: 	int                  m_num_steps;          ///< this is there for debugging purposes (hope it doesn't hurt)
KUDr@3900: 
KUDr@3900: public:
KUDr@3914: 	/// default constructor
KUDr@3900: 	FORCEINLINE CYapfBaseT()
KUDr@3900: 		: m_pBestDestNode(NULL)
KUDr@3900: 		, m_pBestIntermediateNode(NULL)
KUDr@3900: 		, m_settings(&_patches.yapf)
KUDr@3900: 		, m_max_search_nodes(PfGetSettings().max_search_nodes)
KUDr@3900: 		, m_veh(NULL)
KUDr@3900: 		, m_stats_cost_calcs(0)
KUDr@3900: 		, m_stats_cache_hits(0)
KUDr@3900: 		, m_num_steps(0)
KUDr@3900: 	{
KUDr@3900: 	}
KUDr@3900: 
KUDr@3914: 	/// default destructor
KUDr@3900: 	~CYapfBaseT() {}
KUDr@3900: 
KUDr@3900: protected:
KUDr@3914: 	/// to access inherited path finder
KUDr@3900: 	FORCEINLINE Tpf& Yapf() {return *static_cast<Tpf*>(this);}
KUDr@3900: 
KUDr@3900: public:
KUDr@3914: 	/// return current settings (can be custom - player based - but later)
KUDr@3900: 	FORCEINLINE const YapfSettings& PfGetSettings() const
KUDr@3900: 	{
KUDr@3900: 		return *m_settings;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Main pathfinder routine:
rubidium@4549: 	 *   - set startup node(s)
rubidium@4549: 	 *   - main loop that stops if:
rubidium@4549: 	 *      - the destination was found
rubidium@4549: 	 *      - or the open list is empty (no route to destination).
rubidium@4549: 	 *      - or the maximum amount of loops reached - m_max_search_nodes (default = 10000)
rubidium@4549: 	 * @return true if the path was found */
KUDr@3915: 	inline bool FindPath(const Vehicle* v)
KUDr@3900: 	{
KUDr@3900: 		m_veh = v;
KUDr@3900: 
KUDr@3900: 		CPerformanceTimer perf;
KUDr@3900: 		perf.Start();
KUDr@3900: 		Yapf().PfSetStartupNodes();
KUDr@3900: 
KUDr@3900: 		while (true) {
KUDr@3900: 			m_num_steps++;
KUDr@3900: 			Node& n = m_nodes.GetBestOpenNode();
KUDr@3900: 			if (&n == NULL)
KUDr@3900: 				break;
KUDr@3900: 
KUDr@3900: 			// if the best open node was worse than the best path found, we can finish
KUDr@3900: 			if (m_pBestDestNode != NULL && m_pBestDestNode->GetCost() < n.GetCostEstimate())
KUDr@3900: 				break;
KUDr@3900: 
KUDr@3900: 			Yapf().PfFollowNode(n);
KUDr@3900: 			if (m_max_search_nodes == 0 || m_nodes.ClosedCount() < m_max_search_nodes) {
KUDr@3900: 				m_nodes.PopOpenNode(n.GetKey());
KUDr@3900: 				m_nodes.InsertClosedNode(n);
KUDr@3900: 			} else {
KUDr@3900: 				m_pBestDestNode = m_pBestIntermediateNode;
KUDr@3900: 				break;
KUDr@3900: 			}
KUDr@3900: 		}
KUDr@3900: 		bool bDestFound = (m_pBestDestNode != NULL);
KUDr@3900: 
KUDr@3900: 		int16 veh_idx = (m_veh != NULL) ? m_veh->unitnumber : 0;
KUDr@3900: 
KUDr@3900: //		if (veh_idx != 433) return bDestFound;
KUDr@3900: 
KUDr@3900: 		perf.Stop();
KUDr@3900: 		int t = perf.Get(1000000);
KUDr@3900: 		_total_pf_time_us += t;
KUDr@3900: 		char ttc = Yapf().TransportTypeChar();
KUDr@3900: 		float cache_hit_ratio = (float)m_stats_cache_hits / (float)(m_stats_cache_hits + m_stats_cost_calcs) * 100.0f;
KUDr@3900: 		int cost = bDestFound ? m_pBestDestNode->m_cost : -1;
KUDr@3900: 		int dist = bDestFound ? m_pBestDestNode->m_estimate - m_pBestDestNode->m_cost : -1;
KUDr@3947: 		DEBUG(yapf, 3)("[YAPF][YAPF%c]%c%4d- %d us - %d rounds - %d open - %d closed - CHR %4.1f%% - c%d(sc%d, ts%d, o%d) -- ", ttc, bDestFound ? '-' : '!', veh_idx, t, m_num_steps, m_nodes.OpenCount(), m_nodes.ClosedCount(), cache_hit_ratio, cost, dist, m_perf_cost.Get(1000000), m_perf_slope_cost.Get(1000000), m_perf_ts_cost.Get(1000000), m_perf_other_cost.Get(1000000));
KUDr@3900: 		return bDestFound;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** If path was found return the best node that has reached the destination. Otherwise
rubidium@4549: 	 *  return the best visited node (which was nearest to the destination).
rubidium@4549: 	 */
KUDr@3900: 	FORCEINLINE Node& GetBestNode()
KUDr@3900: 	{
KUDr@3900: 		return (m_pBestDestNode != NULL) ? *m_pBestDestNode : *m_pBestIntermediateNode;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Calls NodeList::CreateNewNode() - allocates new node that can be filled and used
rubidium@4549: 	 *  as argument for AddStartupNode() or AddNewNode()
rubidium@4549: 	 */
KUDr@3900: 	FORCEINLINE Node& CreateNewNode()
KUDr@3900: 	{
KUDr@3900: 		Node& node = *m_nodes.CreateNewNode();
KUDr@3900: 		return node;
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Add new node (created by CreateNewNode and filled with data) into open list */
KUDr@3900: 	FORCEINLINE void AddStartupNode(Node& n)
KUDr@3900: 	{
KUDr@3900: 		Yapf().PfNodeCacheFetch(n);
KUDr@4413: 		// insert the new node only if it is not there
KUDr@4413: 		if (&m_nodes.FindOpenNode(n.m_key) == NULL) {
KUDr@4413: 			m_nodes.InsertOpenNode(n);
KUDr@4413: 		} else {
KUDr@4413: 			// if we are here, it means that node is already there - how it is possible?
KUDr@4413: 			//   probably the train is in the position that both its ends point to the same tile/exit-dir
KUDr@4413: 			//   very unlikely, but it happened
KUDr@4413: 		}
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** add multiple nodes - direct children of the given node */
KUDr@3900: 	FORCEINLINE void AddMultipleNodes(Node* parent, TileIndex tile, TrackdirBits td_bits)
KUDr@3900: 	{
KUDr@3978: 		bool is_choice = (KillFirstBit2x64(td_bits) != 0);
KUDr@3900: 		for (TrackdirBits rtds = td_bits; rtds != TRACKDIR_BIT_NONE; rtds = (TrackdirBits)KillFirstBit2x64(rtds)) {
KUDr@3900: 			Trackdir td = (Trackdir)FindFirstBit2x64(rtds);
KUDr@3900: 			Node& n = Yapf().CreateNewNode();
KUDr@3978: 			n.Set(parent, tile, td, is_choice);
KUDr@3900: 			Yapf().AddNewNode(n);
KUDr@3900: 		}
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** AddNewNode() - called by Tderived::PfFollowNode() for each child node.
rubidium@4549: 	 *  Nodes are evaluated here and added into open list */
KUDr@3900: 	void AddNewNode(Node& n)
KUDr@3900: 	{
KUDr@3900: 		// evaluate the node
KUDr@3900: 		bool bCached = Yapf().PfNodeCacheFetch(n);
KUDr@3900: 		if (!bCached) {
KUDr@3900: 			m_stats_cost_calcs++;
KUDr@3900: 		} else {
KUDr@3900: 			m_stats_cache_hits++;
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		bool bValid = Yapf().PfCalcCost(n);
KUDr@3900: 
KUDr@3900: 		if (bCached) {
KUDr@3900: 			Yapf().PfNodeCacheFlush(n);
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		if (bValid) bValid = Yapf().PfCalcEstimate(n);
KUDr@3900: 
KUDr@3900: 		// have the cost or estimate callbacks marked this node as invalid?
KUDr@3900: 		if (!bValid) return;
KUDr@3900: 
KUDr@3900: 		// detect the destination
KUDr@3900: 		bool bDestination = Yapf().PfDetectDestination(n);
KUDr@3900: 		if (bDestination) {
KUDr@3900: 			if (m_pBestDestNode == NULL || n < *m_pBestDestNode) {
KUDr@3900: 				m_pBestDestNode = &n;
KUDr@3900: 			}
KUDr@3900: 			m_nodes.FoundBestNode(n);
KUDr@3900: 			return;
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		if (m_max_search_nodes > 0 && (m_pBestIntermediateNode == NULL || (m_pBestIntermediateNode->GetCostEstimate() - m_pBestIntermediateNode->GetCost()) > (n.GetCostEstimate() - n.GetCost()))) {
KUDr@3900: 			m_pBestIntermediateNode = &n;
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		// check new node against open list
KUDr@3900: 		Node& openNode = m_nodes.FindOpenNode(n.GetKey());
KUDr@3900: 		if (&openNode != NULL) {
KUDr@3900: 			// another node exists with the same key in the open list
KUDr@3900: 			// is it better than new one?
KUDr@3900: 			if (n.GetCostEstimate() < openNode.GetCostEstimate()) {
KUDr@3900: 				// update the old node by value from new one
KUDr@3900: 				m_nodes.PopOpenNode(n.GetKey());
KUDr@3900: 				openNode = n;
KUDr@3900: 				// add the updated old node back to open list
KUDr@3900: 				m_nodes.InsertOpenNode(openNode);
KUDr@3900: 			}
KUDr@3900: 			return;
KUDr@3900: 		}
KUDr@3900: 
KUDr@3900: 		// check new node against closed list
KUDr@3900: 		Node& closedNode = m_nodes.FindClosedNode(n.GetKey());
KUDr@3900: 		if (&closedNode != NULL) {
KUDr@3900: 			// another node exists with the same key in the closed list
KUDr@3900: 			// is it better than new one?
KUDr@3900: 			int node_est = n.GetCostEstimate();
KUDr@3900: 			int closed_est = closedNode.GetCostEstimate();
KUDr@3900: 			if (node_est < closed_est) {
KUDr@3900: 				// If this assert occurs, you have probably problem in
KUDr@3900: 				// your Tderived::PfCalcCost() or Tderived::PfCalcEstimate().
KUDr@3900: 				// The problem could be:
KUDr@3900: 				//  - PfCalcEstimate() gives too large numbers
KUDr@3900: 				//  - PfCalcCost() gives too small numbers
KUDr@3900: 				//  - You have used negative cost penalty in some cases (cost bonus)
KUDr@3900: 				assert(0);
KUDr@3900: 
KUDr@3900: 				return;
KUDr@3900: 			}
KUDr@3900: 			return;
KUDr@3900: 		}
KUDr@3900: 		// the new node is really new
KUDr@3900: 		// add it to the open list
KUDr@3900: 		m_nodes.InsertOpenNode(n);
KUDr@3900: 	}
KUDr@3900: 
KUDr@3915: 	const Vehicle* GetVehicle() const {return m_veh;}
KUDr@3900: 
KUDr@3900: 	// methods that should be implemented at derived class Types::Tpf (derived from CYapfBaseT)
KUDr@3900: 
KUDr@3900: #if 0
KUDr@3900: 	/** Example: PfSetStartupNodes() - set source (origin) nodes */
KUDr@3900: 	FORCEINLINE void PfSetStartupNodes()
KUDr@3900: 	{
KUDr@3900: 		// example:
KUDr@3900: 		Node& n1 = *base::m_nodes.CreateNewNode();
KUDr@3900: 		.
KUDr@3900: 		. // setup node members here
KUDr@3900: 		.
KUDr@3900: 		base::m_nodes.InsertOpenNode(n1);
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Example: PfFollowNode() - set following (child) nodes of the given node */
KUDr@3900: 	FORCEINLINE void PfFollowNode(Node& org)
KUDr@3900: 	{
KUDr@3900: 		for (each follower of node org) {
KUDr@3900: 			Node& n = *base::m_nodes.CreateNewNode();
KUDr@3900: 			.
KUDr@3900: 			. // setup node members here
KUDr@3900: 			.
KUDr@3900: 			n.m_parent   = &org; // set node's parent to allow back tracking
KUDr@3900: 			AddNewNode(n);
KUDr@3900: 		}
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Example: PfCalcCost() - set path cost from origin to the given node */
KUDr@3900: 	FORCEINLINE bool PfCalcCost(Node& n)
KUDr@3900: 	{
KUDr@3900: 		// evaluate last step cost
KUDr@3900: 		int cost = ...;
KUDr@3900: 		// set the node cost as sum of parent's cost and last step cost
KUDr@3900: 		n.m_cost = n.m_parent->m_cost + cost;
KUDr@3900: 		return true; // true if node is valid follower (i.e. no obstacle was found)
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Example: PfCalcEstimate() - set path cost estimate from origin to the target through given node */
KUDr@3900: 	FORCEINLINE bool PfCalcEstimate(Node& n)
KUDr@3900: 	{
KUDr@3900: 		// evaluate the distance to our destination
KUDr@3900: 		int distance = ...;
KUDr@3900: 		// set estimate as sum of cost from origin + distance to the target
KUDr@3900: 		n.m_estimate = n.m_cost + distance;
KUDr@3900: 		return true; // true if node is valid (i.e. not too far away :)
KUDr@3900: 	}
KUDr@3900: 
KUDr@3900: 	/** Example: PfDetectDestination() - return true if the given node is our destination */
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: #endif
KUDr@3900: };
KUDr@3900: 
KUDr@3900: #endif /* YAPF_BASE_HPP */