Add AStar pathfinding

Add Fringe pathfinding
Add PointMap
Add unit testing with snitch-org/snitch@2a59433
Add `distance_squared` function to vectors
Add `dot` function to vectors
Add `hash_murmur3` function to Utility.hpp

.github/workflows/builds.yml

@@ -97,7 +97,7 @@ jobs:
         with:
           platform: ${{ matrix.platform }}
           target: ${{ matrix.target }}
-          sconsflags: arch=${{ matrix.arch }} build_ovsim_library=yes
+          sconsflags: arch=${{ matrix.arch }} build_ovsim_library=yes run_ovsim_tests=yes
 
       - name: Delete compilation files
         if: ${{ matrix.platform == 'windows' }}

.gitignore

@@ -65,6 +65,8 @@ bin/*
 *.idb
 *.exp
 
+tests/bin/*
+
 # Build configuarion.
 /custom.py
 

.gitmodules

@@ -13,3 +13,7 @@
 [submodule "deps/plf_colony"]
 	path = deps/plf_colony
 	url = https://github.com/mattreecebentley/plf_colony
+[submodule "tests/deps/snitch"]
+	path = tests/deps/snitch
+	url = https://github.com/snitch-org/snitch
+	ignore = dirty

SConstruct

@@ -14,6 +14,7 @@ env.PrependENVPath("PATH", os.getenv("PATH"))
 
 opts = env.SetupOptions()
 
+opts.Add(BoolVariable("run_ovsim_tests", "Build and run the openvic simulation unit tests", env.is_standalone))
 opts.Add(BoolVariable(key="build_ovsim_library", help="Build the openvic simulation library.", default=env.get("build_ovsim_library", not env.is_standalone)))
 opts.Add(BoolVariable("build_ovsim_headless", "Build the openvic simulation headless executable", env.is_standalone))
 
@@ -56,6 +57,9 @@ library_name = "libopenvic-simulation{}{}".format(suffix, env["LIBSUFFIX"])
 
 default_args = []
 
+if env["run_ovsim_tests"]:
+    env["build_ovsim_library"] = True
+
 if env["build_ovsim_library"]:
     library = env.StaticLibrary(target=env.File(os.path.join(BINDIR, library_name)), source=sources)
     default_args += [library]
@@ -86,6 +90,9 @@ if env["build_ovsim_headless"]:
     )
     default_args += [headless_program]
 
+if env["run_ovsim_tests"]:
+    SConscript("tests/SCsub", "env")
+
 # Add compiledb if the option is set
 if env.get("compiledb", False):
     default_args += ["compiledb"]

src/openvic-simulation/pathfinding/AStarPathing.cpp

@@ -0,0 +1,93 @@
+#include "openvic-simulation/pathfinding/AStarPathing.hpp"
+
+#include <tsl/ordered_set.h>
+
+#include <fmt/core.h>
+#include <fmt/format.h>
+
+#include <range/v3/algorithm/find.hpp>
+#include <range/v3/algorithm/heap_algorithm.hpp>
+
+#include "openvic-simulation/pathfinding/PointMap.hpp"
+#include "openvic-simulation/types/fixed_point/FixedPoint.hpp"
+
+using namespace OpenVic;
+
+bool AStarPathing::_solve(search_iterator begin_point, search_iterator end_point, uint64_t pass, bool allow_partial_path) {
+	last_closest_point = search.end();
+
+	if (!_is_point_enabled(end_point) && !allow_partial_path) {
+		return false;
+	}
+
+	bool found_route = false;
+
+	std::vector<search_iterator> open_list;
+	open_list.reserve(20);
+
+	begin_point.value().g_score = 0;
+	begin_point.value().f_score = _estimate_cost(begin_point, end_point);
+	begin_point.value().abs_f_score = begin_point.value().f_score;
+	open_list.push_back(begin_point);
+
+	while (!open_list.empty()) {
+		search_iterator p = open_list.front(); // The currently processed point.
+
+		// Find point closer to end_point, or same distance to end_point but closer to begin_point.
+		if (last_closest_point == search.end() || last_closest_point.value().abs_f_score > p.value().abs_f_score ||
+			(last_closest_point.value().abs_f_score >= p.value().abs_f_score && //
+			 last_closest_point.value().g_score > p.value().g_score)) {
+			last_closest_point = p;
+		}
+
+		if (p == end_point) {
+			found_route = true;
+			break;
+		}
+
+		ranges::pop_heap(open_list, PointSort {}); // Remove the current point from the open list.
+		open_list.pop_back();
+		p.value().closed_pass = pass; // Mark the point as closed.
+
+		for (PointMap::points_key_type const& neighbor_id : p.value().point->neighbors) {
+			search_iterator e = search.find(neighbor_id); // The neighbor point.
+			if (e == search.end()) {
+				PointMap::points_value_type const* neighbor_point = point_map->get_point(neighbor_id);
+				if (neighbor_point == nullptr) {
+					continue;
+				}
+				e = search.insert({ neighbor_id, { neighbor_point } }).first;
+			}
+
+			if (!_is_point_enabled(e) || e.value().closed_pass == pass) {
+				continue;
+			}
+
+			fixed_point_t tentative_g_score = p.value().g_score + _compute_cost(p, e) * e.value().point->weight_scale;
+
+			bool new_point = false;
+
+			if (e.value().open_pass != pass) { // The point wasn't inside the open list.
+				e.value().open_pass = pass;
+				new_point = true;
+			} else if (tentative_g_score >= e.value().g_score) { // The new path is worse than the previous.
+				continue;
+			}
+
+			e.value().prev_point = p;
+			e.value().g_score = tentative_g_score;
+			e.value().abs_f_score = _estimate_cost(e, end_point);
+			e.value().f_score = e.value().g_score + e.value().abs_f_score;
+
+			if (new_point) { // The position of the new points is already known.
+				open_list.push_back(e);
+				ranges::push_heap(open_list, PointSort {});
+			} else if (open_list.size() > 1) {
+				decltype(open_list)::iterator temp_it = ranges::find(open_list, e);
+				ranges::push_heap(open_list.begin(), temp_it + 1, PointSort {});
+			}
+		}
+	}
+
+	return found_route;
+}

src/openvic-simulation/pathfinding/AStarPathing.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <cstdint>
+
+#include <tsl/ordered_map.h>
+#include <tsl/ordered_set.h>
+
+#include "openvic-simulation/pathfinding/PathingBase.hpp"
+#include "openvic-simulation/types/fixed_point/FixedPoint.hpp"
+
+namespace OpenVic {
+	struct AStarPathingNode : public PathingNodeBase<AStarPathingNode> {
+		using PathingNodeBase::PathingNodeBase;
+
+		// Used for pathfinding.
+		uint64_t open_pass = 0;
+		uint64_t closed_pass = 0;
+
+		// Used for pathfinding.
+		fixed_point_t g_score = 0;
+		fixed_point_t f_score = 0;
+
+		// Used for getting closest_point_of_last_pathing_call.
+		fixed_point_t abs_f_score = 0;
+	};
+
+	/** A* Pathfinding implementation
+
+		Requires that any pointer invalidation in PointMap must call reset_search()
+	 */
+	struct AStarPathing : public PathingBase<AStarPathingNode> {
+		using PathingBase::PathingBase;
+
+		struct PointSort {
+			// Returns true when the Point A is worse than Point B.
+			bool operator()(search_const_iterator A, search_const_iterator B) const {
+				if (A.value().f_score > B.value().f_score) {
+					return true;
+				} else if (A.value().f_score < B.value().f_score) {
+					return false;
+				} else {
+					// If the f_costs are the same then prioritize the points that
+					// are further away from the start.
+					return A.value().g_score < B.value().g_score;
+				}
+			}
+		};
+
+	protected:
+		virtual bool _solve( //
+			search_iterator begin_point, search_iterator end_point, uint64_t pass, bool allow_partial_path
+		) override;
+	};
+}

src/openvic-simulation/pathfinding/FringePathing.cpp

@@ -0,0 +1,116 @@
+#include "openvic-simulation/pathfinding/FringePathing.hpp"
+
+#include <vector>
+
+#include <tsl/ordered_map.h>
+#include <tsl/ordered_set.h>
+
+#include <fmt/core.h>
+#include <fmt/format.h>
+
+#include <range/v3/algorithm/find.hpp>
+#include <range/v3/algorithm/heap_algorithm.hpp>
+
+#include "openvic-simulation/pathfinding/PointMap.hpp"
+#include "openvic-simulation/types/fixed_point/FixedPoint.hpp"
+
+using namespace OpenVic;
+
+bool FringePathing::_solve(search_iterator begin_point, search_iterator end_point, uint64_t pass, bool allow_partial_path) {
+	last_closest_point = search.end();
+
+	if (!_is_point_enabled(end_point) && !allow_partial_path) {
+		return false;
+	}
+
+	bool found_route = false;
+
+	std::vector<search_iterator> fringe;
+	fringe.reserve(20);
+
+	begin_point.value().g_score = 0;
+	begin_point.value().f_score = _estimate_cost(begin_point, end_point);
+	begin_point.value().open_pass = pass;
+	fringe.push_back(begin_point);
+
+	fixed_point_t f_limit = begin_point.value().f_score;
+	fixed_point_t next_f_limit = fixed_point_t::max();
+
+	while (!fringe.empty() && !found_route) {
+		ranges::pop_heap(fringe, FringeSort {});
+		search_iterator p = fringe.back();
+		fringe.pop_back();
+
+		if (p.value().open_pass != pass) {
+			continue;
+		}
+
+		if (p.value().f_score > f_limit) {
+			f_limit = next_f_limit;
+			next_f_limit = fixed_point_t::max();
+			fringe.push_back(p);
+			ranges::push_heap(fringe, FringeSort {});
+			continue;
+		}
+
+		if (p.value().f_score > f_limit && p.value().f_score < next_f_limit) {
+			next_f_limit = p.value().f_score;
+		}
+
+		// Find point closer to end_point, or same distance to end_point but closer to begin_point.
+		if (last_closest_point == search.end() || last_closest_point.value().abs_f_score > p.value().abs_f_score ||
+			(last_closest_point.value().abs_f_score >= p.value().abs_f_score && //
+			 last_closest_point.value().g_score > p.value().g_score)) {
+			last_closest_point = p;
+		}
+
+		if (p == end_point) {
+			found_route = true;
+			break;
+		}
+
+		p.value().closed_pass = pass;
+
+		for (PointMap::points_key_type const& neighbor_id : p.value().point->neighbors) {
+			search_iterator e = search.find(neighbor_id); // The neighbor point.
+			if (e == search.end()) {
+				PointMap::points_value_type const* neighbor_point = point_map->get_point(neighbor_id);
+				if (neighbor_point == nullptr) {
+					continue;
+				}
+				e = search.insert({ neighbor_id, { neighbor_point } }).first;
+			}
+
+			if (!_is_point_enabled(e) || e.value().closed_pass == pass) {
+				continue;
+			}
+
+			fixed_point_t tentative_g_score = p.value().g_score + _compute_cost(p, e) * e.value().point->weight_scale;
+			bool new_point = e.value().open_pass != pass;
+
+			if (new_point) { // The point wasn't inside the open list.
+				e.value().open_pass = pass;
+				e.value().abs_f_score = _estimate_cost(p, e);
+				e.value().f_score = tentative_g_score + e.value().abs_f_score;
+			} else if (tentative_g_score >= e.value().g_score) { // The new path is worse than the previous.
+				continue;
+			} else {
+				e.value().abs_f_score = e.value().f_score - e.value().g_score;
+				e.value().f_score = tentative_g_score + e.value().abs_f_score;
+			}
+
+			e.value().g_score = tentative_g_score;
+			e.value().prev_point = p;
+
+			if (new_point) {
+				fringe.push_back(e);
+				ranges::push_heap(fringe, FringeSort {});
+			} else {
+				decltype(fringe)::iterator temp_it = ranges::find(fringe, e);
+				ranges::push_heap(fringe.begin(), temp_it + 1, FringeSort {});
+			}
+		}
+	}
+
+	return found_route;
+}

src/openvic-simulation/pathfinding/FringePathing.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "openvic-simulation/pathfinding/PathingBase.hpp"
+#include "openvic-simulation/types/fixed_point/FixedPoint.hpp"
+
+namespace OpenVic {
+	struct FringePathingNode : public PathingNodeBase<FringePathingNode> {
+		using PathingNodeBase::PathingNodeBase;
+
+		// Used for pathfinding.
+		uint64_t open_pass = 0;
+		uint64_t closed_pass = 0;
+
+		// Used for pathfinding.
+		fixed_point_t g_score = 0;
+		fixed_point_t f_score = -1;
+
+		// Used for getting closest_point_of_last_pathing_call.
+		fixed_point_t abs_f_score = 0;
+	};
+
+	/** Fringe Pathfinding implementation
+
+		Requires that any pointer invalidation in PointMap must call reset_search()
+	 */
+	struct FringePathing : public PathingBase<FringePathingNode> {
+		using PathingBase::PathingBase;
+
+		struct FringeSort {
+			// Returns true when the Point A is worse than Point B.
+			bool operator()(search_const_iterator A, search_const_iterator B) const {
+				if (A.value().f_score > B.value().f_score) {
+					return true;
+				} else if (A.value().f_score < B.value().f_score) {
+					return false;
+				} else {
+					// If the f_costs are the same then prioritize the points that
+					// are further away from the start.
+					return A.value().g_score < B.value().g_score;
+				}
+			}
+		};
+
+	protected:
+		virtual bool _solve( //
+			search_iterator begin_point, search_iterator end_point, uint64_t pass, bool allow_partial_path
+		) override;
+	};
+}