flatten 20260225

tjp/core/graphics/soft/Screen+IO.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "Screen.h"
+#include <iostream>
+#include <tjp/core/iterators/range.hpp>
+
+
+namespace tjp::core::graphics::soft {
+
+template<typename T>
+std::ostream &operator <<(std::ostream &o, const Screen<T> &s)
+{
+	using namespace core::iterators;
+	
+	auto D = "";
+
+	for (auto _ : range(s.dimensions.y+2))
+	{
+		(void)_;
+		o << "-" << D;
+	}
+	o << std::endl;
+		
+	for (auto y : range(s.dimensions.y))
+	{
+		o << "|";
+		for (auto x : range(s.dimensions.x))
+		{
+			if (auto v = s.get({x,y}))
+				o << v << D;
+			else
+				o << " " << D;
+		}
+		
+		o << "|" << std::endl;
+	}
+
+	for (auto _ : range(s.dimensions.y+2))
+	{
+		(void)_;
+		o << "-" << D;
+	}
+	o << std::endl;
+	
+	return o;
+}
+
+} // namespace

tjp/core/graphics/soft/Screen+PNG.cpp

@@ -0,0 +1 @@
+#include "Screen+PNG.inl"

tjp/core/graphics/soft/Screen+PNG.inl

@@ -0,0 +1,186 @@
+#pragma once
+
+#include "Screen.h"
+#include "Screen.hpp"
+#include "../STB+Image.h"
+
+#include <tjp/core/math/Vector3.hpp>
+
+#include <cmath>
+
+namespace tjp::core::graphics::soft {
+
+
+template<typename T>
+Vector<char> toPNG(const Screen<T> &screen);
+
+
+template<>
+inline
+Vector<char> toPNG(const Screen<bool> &screen)
+{
+	Vector<u8> data;
+	for (auto v : screen.memory)
+		data.push_back(v ? 255 : 0);
+	
+	auto channels = 1;
+	
+	int len;
+	char *png = (char *)stbi_write_png_to_mem(
+		data.data(),
+		channels * screen.dimensions.x(),
+		screen.dimensions.x(),
+		screen.dimensions.y(),
+		channels,
+		&len
+	);
+
+	Vector<char> result(png, png + len);
+
+	free(png);
+	
+	return result;
+}
+
+template<typename T>
+Vector<char> toPNG(const Screen<math::Vector3<T>> &screen)
+{
+	Vector<u8> data;
+	for (auto v : screen.memory) {
+		data.push_back(v[0] * 255);
+		data.push_back(v[1] * 255);
+		data.push_back(v[2] * 255);
+	}
+	
+	auto channels = 3;
+	
+	int len;
+	char *png = (char *)stbi_write_png_to_mem(
+		data.data(),
+		channels * screen.dimensions.x,
+		screen.dimensions.x,
+		screen.dimensions.y,
+		channels,
+		&len
+	);
+
+	Vector<char> result(png, png + len);
+
+	free(png);
+	
+	return result;
+}
+
+template<typename T>
+Screen<T> fromPNG(const Vector<char> &screen);
+
+template<>
+inline
+Screen<bool> fromPNG(const Vector<char> &data)
+{
+	int width, height, channels;
+	 
+	char *png = (char *)stbi_load_from_memory(
+		(unsigned char *)data.data(), (int)data.size(),
+		&width, &height, &channels, 0
+	);
+
+	Screen<bool> result(
+		Screen<bool>::Point { width, height }
+	);
+	
+	auto *begin = png;
+	auto *end = begin + (width * height * channels);
+
+	int e = 0;
+	for (auto *p=png; p!=end; p+= channels)
+	{
+		bool v = false;
+		for (auto i=0; i<channels; ++i)
+		{
+			if (p[i] != 0)
+			{
+				v = true;
+				break;
+			}
+		}
+
+		result.memory[e++] = v;
+	}
+	
+	free(png);
+	
+	return result;
+}
+
+template<>
+inline
+Screen<u8> fromPNG(const Vector<char> &data)
+{
+	int width, height, channels;
+	 
+	auto *png = (unsigned char *)stbi_load_from_memory(
+		(unsigned char *)data.data(), (int)data.size(),
+		&width, &height, &channels, 0
+	);
+
+	Screen<u8> result(
+		Screen<u8>::Point { width, height }
+	);
+	
+	auto *begin = png;
+	auto *end = begin + (width * height * channels);
+
+	int e = 0;
+	for (auto *p=png; p!=end; p+= channels)
+	{
+		u32 v = 0;
+		for (auto i=0; i<channels; ++i)
+			v += p[i];
+			
+		v /= channels;
+
+		result.memory[e++] = (u8)v;
+	}
+	
+	free(png);
+	
+	return result;
+}
+
+
+template<>
+inline
+Screen<math::Vector3r> fromPNG(const Vector<char> &data)
+{
+	int width, height, channels;
+	 
+	char *png = (char *)stbi_load_from_memory(
+		(unsigned char *)data.data(), (int)data.size(),
+		&width, &height, &channels, 0
+	);
+
+	Screen<math::Vector3r> result(
+		Screen<math::Vector3r>::Point { width, height }
+	);
+	
+	auto *begin = png;
+	auto *end = begin + (width * height * channels);
+	int e = 0;
+	for (auto *p=png; p!=end; p+= channels)
+	{
+		math::Vector3r v = math::Vector3r::Zero;
+		for (auto i=0; i<channels && i<3; ++i)
+			v[i] = p[i]/255.0;
+			
+		result.memory[e++] = v;
+			
+	}
+	
+	free(png);
+	
+	return result;
+}
+
+
+} // namespace

tjp/core/graphics/soft/Screen+and.inl

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Screen.h"
+
+#include <tjp/core/iterators/enumerate.hpp>
+
+#include <cmath>
+
+namespace tjp::core::graphics::soft {
+
+template<typename T, typename U>
+Screen<T> &and_(Screen<T> &lhs, const Screen<U> &rhs)
+{
+	for (auto i=0; i<rhs.memory.size(); ++i)
+		if (!rhs.memory.at(i))
+			lhs.memory[i] = 0;
+
+	return lhs;
+}
+
+} // namespace

tjp/core/graphics/soft/Screen+dilate.inl

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <tjp/core/iterators/range.hpp>
+
+#include <cmath>
+
+namespace tjp::core::graphics::soft {
+
+template<typename T>
+Screen<T> dilate(const Screen<T> &screen)
+{
+	auto copy = screen;
+	for (auto y : range(1, screen.dimensions.y()-1))
+	{
+		for (auto x : range(1, screen.dimensions.x()-1))
+		{
+			bool shouldDilate = false;
+			auto dx_ = 0, dy_ = 0;
+			
+			for (int dy = -1; dy <= 1; ++dy) {
+                for (int dx = -1; dx <= 1; ++dx) {
+                    if (screen.ref({x + dx, y + dy})) {
+						dx_ = dx; dy_ = dy;
+                        shouldDilate = true;
+                        break;
+                    }
+                }
+                if (shouldDilate) break;
+            }
+            
+            if (shouldDilate)
+				copy.set({x, y}, screen.ref({x + dx_, y + dy_}));
+		}
+	}
+	
+	return copy;
+}
+
+} // namespace

tjp/core/graphics/soft/Screen+erode.inl

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "Screen.h"
+#include <tjp/core/iterators/range.hpp>
+
+#include <cmath>
+
+namespace tjp::core::graphics::soft {
+
+template<typename T>
+Screen<T> erode(const Screen<T> &screen)
+{
+	auto copy = screen;
+	for (auto y : range(1, screen.dimensions.y-1))
+	{
+		for (auto x : range(1, screen.dimensions.x-1))
+		{
+			bool shouldErode = true;
+			auto dx_ = 0, dy_ = 0;
+			
+			for (int dy = -1; dy <= 1; ++dy) {
+                for (int dx = -1; dx <= 1; ++dx) {
+                    if (!screen.at({x + dx, y + dy})) {
+						dx_ = dx; dy_ = dy;
+                        shouldErode = false;
+                        break;
+                    }
+                }
+                if (!shouldErode) break;
+            }
+            
+            if (shouldErode)
+				copy.set({x, y}, screen.at({x + dx_, y + dy_}));
+		}
+	}
+	
+	return copy;
+}
+
+} // namespace

tjp/core/graphics/soft/Screen+find_path.inl

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "pathfinders.h"
+
+namespace tjp::core::graphics::soft {
+
+template<typename U>
+Vector<int> find_path(const Screen<u8> &screen, const U &start, const U &end)
+{
+	Vector<int> path;
+	path.resize(16384);
+
+	auto pathSize =
+//		BFSFindPathDiag(
+		AStarFindPathDiag(
+			(int)start.x(), (int)start.y(),
+			(int)end.x(), (int)end.y(),
+			screen.memory.data(),
+			screen.dimensions.x(),
+			screen.dimensions.y(),
+			path.data(),
+			(int)path.size()
+		);
+	
+	if (pathSize < 0)
+		pathSize = 0;
+		
+	path.resize(pathSize);
+	return path;
+}
+
+template<typename T, typename U>
+Vector<int> find_path(const Screen<T> &screen, const U &start, const U &end)
+{
+	Screen<u8> map(screen.dimensions);
+	
+	for (auto i=0; i<screen.memory.size(); ++i)
+		map.memory[i] = screen.memory.at(i);
+		
+	return find_path(map, start, end);
+}
+
+} // namespace

tjp/core/graphics/soft/Screen.h

@@ -0,0 +1,8 @@
+#pragma once
+
+namespace tjp::core::graphics::soft {
+
+template<typename T>
+struct Screen;
+
+} // namespace

tjp/core/graphics/soft/Screen.hpp

@@ -0,0 +1,169 @@
+#pragma once
+
+#include "Screen.h"
+
+#include <tjp/core/math/Vector2.hpp>
+#include <tjp/core/math/Vector2.inl>
+#include <tjp/core/assert/debug_assert.h>
+#include <tjp/core/containers/Vector.h>
+
+#include <cmath>
+
+namespace tjp::core::graphics::soft {
+
+template<typename T>
+struct Screen {
+	using Point = core::math::Vector2i;
+	using Real = core::math::Real;
+	using Interpolation = core::math::Vector2<Real>;
+	
+	using Memory = Vector<T>;
+	using const_reference = typename Memory::const_reference;
+	using reference = typename Memory::reference;
+	using value_type = T;
+	
+	Memory memory;
+	Point dimensions;
+	
+	Screen() :
+		dimensions({0,0})
+	{}
+	
+	Screen(const Screen &rhs) :
+		memory(rhs.memory),
+		dimensions(rhs.dimensions)
+	{
+	}
+	
+	Screen(const Point &dimensions_) : Screen()
+	{
+		resize(dimensions_);
+	}
+	
+	Screen(Screen &&rhs) :
+		memory(std::move(rhs.memory)),
+		dimensions(std::move(rhs.dimensions))
+	{
+	}
+	
+	void resize(const Point &dimensions_)
+	{
+		dimensions = dimensions_;
+		memory.resize(dimensions[0] * dimensions[1]);
+	}
+	
+	bool empty() const
+	{
+		return memory.empty();
+	}
+
+	auto &operator=(Screen<T> &&copy)
+	{
+		memory = std::move(copy.memory);
+		dimensions = std::move(copy.dimensions);
+		
+		return *this;
+	}
+
+	auto &operator=(const Screen<T> &copy)
+	{
+		memory = copy.memory;
+		dimensions = copy.dimensions;
+		
+		return *this;
+	}
+	
+	auto index(const Point &p) const
+	{
+		return p[1] * dimensions[0] + p[0];
+	}
+
+	reference ref(const Point &p)
+	{
+		debug_assert(has(p));
+
+		auto i = index(p);
+		return memory[i];
+	}
+
+	const_reference ref(const Point &p) const
+	{
+		debug_assert(has(p));
+
+		auto i = index(p);
+		return memory[i];
+	}
+
+	value_type get(const Point &p) const
+	{
+		debug_assert(has(p));
+
+		return memory[p[1] * dimensions[0] + p[0]];
+	}
+
+	Point clamp(const Point &p) const
+	{
+		return Point {
+			(p[0] < 0) ? 0 :
+				(p[0] >= dimensions[0]) ? dimensions[0] - 1 :
+					p[0],
+			(p[1] < 0) ? 0 :
+				(p[1] >= dimensions[1]) ? dimensions[1] - 1 :
+					p[1]
+		} ;
+	}
+
+	auto interpolate(const Interpolation &p_) const
+	{
+		auto p = mul(p_, dimensions.asType<Real>());
+		auto p0 = Point { (int)std::floor(p[0]), (int)std::floor(p[1]) };
+		
+		Point ps[4] = {
+			p0,
+			{ p0[0] + 1, p0[1] },
+			{ p0[0], p0[1] + 1 },
+			{ p0[0] + 1, p0[1] + 1 }
+		};
+		
+		auto sqrt_2 = std::sqrt(2);
+
+		Real wv = 0;
+		Real w = 0;
+		for (auto i=0; i<4; ++i)
+		{
+			auto pv = get(clamp(ps[i]));
+			
+			auto pw = sqrt_2 - (p - ps[i].asType<Real>()).length();
+			auto pwv = pw * pv;
+
+			w += pw;
+			wv += pwv;
+		}
+		
+		auto v = wv / w;
+		
+		return v;
+	}
+
+	void set(const Point &p, const T &t)
+	{
+		debug_assert(has(p));
+		
+		auto i = index(p);
+		memory[i] = t;
+	}
+
+	bool has(const Point &p) const
+	{
+		return
+			p[0] >= 0 && p[0] < dimensions[0] &&
+			p[1] >= 0 && p[1] < dimensions[1];
+	}
+
+	void clear(const T &value)
+	{
+		memory.assign(memory.size(), value);
+	}
+} ;
+
+} // namespace

tjp/core/graphics/soft/_tests/Screen.cpp

@@ -0,0 +1,56 @@
+#include "../Screen.hpp"
+
+#include <catch2/catch.hpp>
+#include <tjp/core/iterators/range.hpp>
+
+namespace tjp::core::graphics::soft {
+
+using namespace core::iterators;
+
+SCENARIO("screen")
+{
+	GIVEN("a screen")
+	{
+		Screen<bool> s({10,10});
+		
+		REQUIRE(s.has({-1, 0}) == false);
+		REQUIRE(s.has({0, 0}) == true);
+		REQUIRE(s.has({10, 0}) == false);
+		REQUIRE(s.has({9,9}) == true);
+	}
+
+	GIVEN("a screen")
+	{
+		Screen<bool> s({10,10});
+		
+		WHEN("clears")
+		{
+			s.clear(false);
+			for (auto i : range(s.dimensions.x))
+			{
+				for (auto j : range(s.dimensions.y))
+				{
+					REQUIRE(s.get({i,j}) == false);
+				}
+			}
+		}
+		
+		WHEN("clear and set")
+		{
+			s.clear(false);
+			REQUIRE(s.get({5,5}) == false);
+			s.set({5,5}, true);
+			
+			REQUIRE(s.get({5,5}) == true);
+			REQUIRE(s.get({4,5}) == false);
+		}
+	}
+	
+	GIVEN("a screen")
+	{
+
+	}
+}
+
+
+} // namespace

tjp/core/graphics/soft/pathfinders.cpp

@@ -0,0 +1,604 @@
+#include <vector>
+#include <queue>
+#include <tuple>
+#include <cstdlib>
+#include <climits>
+
+#include <random>
+//#include <boost/random/random_device.hpp>
+//#include <boost/random/uniform_int_distribution.hpp>
+
+
+// https://github.com/XinosZeng/pathfinding-AStarVar
+
+#include "pathfinders.h"
+
+using namespace std;
+
+static int ExploredNodes;
+static vector<int> Landmarks;
+static vector<vector<int>> LD;
+
+int BFSFindPath(const int nStartX, const int nStartY,
+		const int nTargetX, const int nTargetY,
+		const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  d[startPos] = 0;
+  queue<int> q;
+  q.push(startPos);
+  while (!q.empty()) {
+    int u = q.front(); q.pop(); ExploredNodes++;
+    for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] == INT_MAX && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	q.push(v);
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos];
+}
+
+int BFSFindPathDiag(const int nStartX, const int nStartY,
+		    const int nTargetX, const int nTargetY,
+		    const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		    int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  queue<int> q;
+  d[startPos] = 0;
+  q.push(startPos);
+  while (!q.empty()) {
+    int u = q.front(); q.pop(); ExploredNodes++;
+    for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	  +1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	  || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] == INT_MAX && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	q.push(v);
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+int AStarFindPath(const int nStartX, const int nStartY,
+		  const int nTargetX, const int nTargetY,
+		  const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		  int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  auto h = [=](int u) -> int { // lower bound distance to target from u
+    int x = u % nMapWidth, y = u / nMapWidth;
+    return abs(x-nTargetX) + abs(y-nTargetY);
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  int discovered = 0; ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<tuple<int, int, int>,
+		 vector<tuple<int, int, int>>,
+		 greater<tuple<int, int, int>>> pq; // A* with tie breaking
+  d[startPos] = 0;
+  pq.push(make_tuple(0 + h(startPos), 0, startPos));
+  while (!pq.empty()) {
+    int u = get<2>(pq.top()); pq.pop(); ExploredNodes++;
+    for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_tuple(d[v] + h(v), ++discovered, v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+int AStarFindPathDiag(const int nStartX, const int nStartY,
+		      const int nTargetX, const int nTargetY,
+		      const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		      int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+//  auto h = [=](int u) -> int { // lower bound distance to target from u
+//    int x = u % nMapWidth, y = u / nMapWidth;
+//    return max(abs(x-nTargetX), abs(y-nTargetY));
+//  };
+  
+auto h = [=](int u) -> int {
+	int x = u % nMapWidth, y = u / nMapWidth;
+	int dx = abs(x - nTargetX), dy = abs(y - nTargetY);
+	return (dx + dy) + (sqrt(2) - 2) * min(dx, dy);
+};
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  int discovered = 0; ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<tuple<int, int, int>,
+		 vector<tuple<int, int, int>>,
+		 greater<tuple<int, int, int>>> pq; // A* with tie breaking
+  d[startPos] = 0;
+  pq.push(make_tuple(0 + h(startPos), 0, startPos));
+  while (!pq.empty()) {
+    int u = get<2>(pq.top()); pq.pop(); ExploredNodes++;
+    for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	  +1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	  || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_tuple(d[v] + h(v), ++discovered, v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+void InitializeLandmarks(int k, const unsigned char* pMap, const int nMapWidth, const int nMapHeight) {
+
+  vector<int> traversable;
+  for (int i = 0; i < nMapWidth; i++)
+    for (int j = 0; j < nMapHeight; j++)
+      if (pMap[nMapWidth*j + i])
+	traversable.push_back(nMapWidth*j + i);
+
+  while (Landmarks.size() < k) {
+
+    if (Landmarks.empty()) {
+	std::mt19937 rng;
+      uniform_int_distribution<> uniform(0, traversable.size() - 1);
+      Landmarks.push_back(traversable[uniform(rng)]);
+      continue;
+    }
+
+    const int n = nMapWidth*nMapHeight;
+    vector<int> p(n), d(n, INT_MAX);
+    queue<int> q;
+    for (auto s : Landmarks) {
+      d[s] = 0;
+      q.push(s);
+    }
+    int farthest = -1, maxdist = -1;
+    while (!q.empty()) {
+      int u = q.front(); q.pop();
+      if (d[u] > maxdist)
+	maxdist = d[u], farthest = u;
+      for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+	int v = u + e;
+	if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	  continue;
+	if (0 <= v && v < n && d[v] == INT_MAX && pMap[v]) {
+	  p[v] = u;
+	  d[v] = d[u] + 1;
+	  q.push(v);
+	}
+      }
+    }
+
+    Landmarks.push_back(farthest); // works well when the graph is not too disconnected
+  }
+
+  LD.resize(Landmarks.size());
+  for (int i = 0; i < Landmarks.size(); i++) {
+    const int n = nMapWidth*nMapHeight;
+    vector<int> p(n); LD[i].resize(n, INT_MAX);
+    queue<int> q;
+    int s = Landmarks[i];
+    LD[i][s] = 0;
+    q.push(s);
+    while (!q.empty()) {
+      int u = q.front(); q.pop();
+      for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+	int v = u + e;
+	if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	  continue;
+	if (0 <= v && v < n && LD[i][v] == INT_MAX && pMap[v]) {
+	  p[v] = u;
+	  LD[i][v] = LD[i][u] + 1;
+	  q.push(v);
+	}
+      }
+    }
+  }
+}
+
+int AStarFindPathLandmarks(const int nStartX, const int nStartY,
+			   const int nTargetX, const int nTargetY,
+			   const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			   int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  auto h = [=](int u) { // lower bound distance to target from u
+    int m = 0;
+    for (int i = 0; i < Landmarks.size(); i++) // global vector<int> Landmarks
+      m = max(m, LD[i][targetPos] - LD[i][u]); // global vector<vector<int>> LD
+    return m;
+  };
+
+  int discovered = 0; ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<tuple<int, int, int>,
+		 vector<tuple<int, int, int>>,
+		 greater<tuple<int, int, int>>> pq; // A* with tie breaking
+  d[startPos] = 0;
+  pq.push(make_tuple(0 + h(startPos), 0, startPos));
+  while (!pq.empty()) {
+    int u = get<2>(pq.top()); pq.pop(); ExploredNodes++;
+    for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_tuple(d[v] + h(v), ++discovered, v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+void InitializeLandmarksDiag(int k, const unsigned char* pMap, const int nMapWidth, const int nMapHeight) {
+
+  vector<int> traversable;
+  for (int i = 0; i < nMapWidth; i++)
+    for (int j = 0; j < nMapHeight; j++)
+      if (pMap[nMapWidth*j + i])
+	traversable.push_back(nMapWidth*j + i);
+
+  while (Landmarks.size() < k) {
+
+    if (Landmarks.empty()) {
+	std::mt19937 rng;
+      uniform_int_distribution<> uniform(0, traversable.size() - 1);
+      Landmarks.push_back(traversable[uniform(rng)]);
+      continue;
+    }
+
+    const int n = nMapWidth*nMapHeight;
+    vector<int> p(n), d(n, INT_MAX);
+    queue<int> q;
+    for (auto s : Landmarks) {
+      d[s] = 0;
+      q.push(s);
+    }
+    int farthest = -1, maxdist = -1;
+    while (!q.empty()) {
+      int u = q.front(); q.pop();
+      if (d[u] > maxdist)
+	maxdist = d[u], farthest = u;
+      for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	    +1, -1, +nMapWidth, -nMapWidth}) {
+	int v = u + e;
+	if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	    || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	  continue;
+	if (0 <= v && v < n && d[v] == INT_MAX && pMap[v]) {
+	  p[v] = u;
+	  d[v] = d[u] + 1;
+	  q.push(v);
+	}
+      }
+    }
+
+    Landmarks.push_back(farthest); // works well when the graph is not too disconnected
+  }
+
+  LD.resize(Landmarks.size());
+  for (int i = 0; i < Landmarks.size(); i++) {
+    const int n = nMapWidth*nMapHeight;
+    vector<int> p(n); LD[i].resize(n, INT_MAX);
+    queue<int> q;
+    int s = Landmarks[i];
+    LD[i][s] = 0;
+    q.push(s);
+    while (!q.empty()) {
+      int u = q.front(); q.pop();
+      for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	    +1, -1, +nMapWidth, -nMapWidth}) {
+	int v = u + e;
+	if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	    || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	  continue;
+	if (0 <= v && v < n && LD[i][v] == INT_MAX && pMap[v]) {
+	  p[v] = u;
+	  LD[i][v] = LD[i][u] + 1;
+	  q.push(v);
+	}
+      }
+    }
+  }
+}
+
+int AStarFindPathLandmarksDiag(const int nStartX, const int nStartY,
+			       const int nTargetX, const int nTargetY,
+			       const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			       int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  auto h = [=](int u) { // lower bound distance to target from u
+    int m = 0;
+    for (int i = 0; i < Landmarks.size(); i++) // global vector<int> Landmarks
+      m = max(m, LD[i][targetPos] - LD[i][u]); // global vector<vector<int>> LD
+    return m;
+  };
+
+  int discovered = 0; ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<tuple<int, int, int>,
+		 vector<tuple<int, int, int>>,
+		 greater<tuple<int, int, int>>> pq; // A* with tie breaking
+  d[startPos] = 0;
+  pq.push(make_tuple(0 + h(startPos), 0, startPos));
+  while (!pq.empty()) {
+    int u = get<2>(pq.top()); pq.pop(); ExploredNodes++;
+    for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	  +1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	  || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_tuple(d[v] + h(v), ++discovered, v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+int AStarFindPathNoTie(const int nStartX, const int nStartY,
+		       const int nTargetX, const int nTargetY,
+		       const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		       int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  auto h = [=](int u) { // lower bound distance to target from u
+    int x = u % nMapWidth, y = u / nMapWidth;
+    return abs(x-nTargetX) + abs(y-nTargetY);
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<pair<int, int>,
+		 vector<pair<int, int>>,
+		 greater<pair<int, int>>> pq;
+  d[startPos] = 0;
+  pq.push(make_pair(0 + h(startPos), startPos));
+  while (!pq.empty()) {
+    int u = pq.top().second; pq.pop(); ExploredNodes++;
+    for (auto e : {+1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if ((e == 1 && (v % nMapWidth == 0)) || (e == -1 && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_pair(d[v] + h(v), v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}
+
+int AStarFindPathNoTieDiag(const int nStartX, const int nStartY,
+			   const int nTargetX, const int nTargetY,
+			   const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			   int* pOutBuffer, const int nOutBufferSize) {
+
+  auto idx = [nMapWidth](int x, int y) {
+    return x + y*nMapWidth;
+  };
+
+  auto h = [=](int u) { // lower bound distance to target from u
+    int x = u % nMapWidth, y = u / nMapWidth;
+    return max(abs(x-nTargetX), abs(y-nTargetY));
+  };
+
+  const int n = nMapWidth*nMapHeight;
+  const int startPos = idx(nStartX, nStartY), targetPos = idx(nTargetX, nTargetY);
+
+  ExploredNodes = 0;
+  vector<int> p(n), d(n, INT_MAX);
+  priority_queue<pair<int, int>,
+		 vector<pair<int, int>>,
+		 greater<pair<int, int>>> pq;
+  d[startPos] = 0;
+  pq.push(make_pair(0 + h(startPos), startPos));
+  while (!pq.empty()) {
+    int u = pq.top().second; pq.pop(); ExploredNodes++;
+    for (auto e : {-nMapWidth-1, -nMapWidth+1, +nMapWidth-1, +nMapWidth+1,
+	  +1, -1, +nMapWidth, -nMapWidth}) {
+      int v = u + e;
+      if (((e == 1 || e == -nMapWidth+1 || e == nMapWidth+1) && (v % nMapWidth == 0))
+	  || ((e == -1 || e == -nMapWidth-1 || e == nMapWidth-1) && (u % nMapWidth == 0)))
+	continue;
+      if (0 <= v && v < n && d[v] > d[u] + 1 && pMap[v]) {
+	p[v] = u;
+	d[v] = d[u] + 1;
+	if (v == targetPos)
+	  goto end;
+	pq.push(make_pair(d[v] + h(v), v));
+      }
+    }
+  }
+ end:
+
+  if (d[targetPos] == INT_MAX) {
+    return -1;
+  } else if (d[targetPos] <= nOutBufferSize) {
+    int curr = targetPos;
+    for (int i = d[targetPos] - 1; i >= 0; i--) {
+      pOutBuffer[i] = curr;
+      curr = p[curr];
+    }
+    return d[targetPos];
+  }
+
+  return d[targetPos]; // buffer size too small
+}

tjp/core/graphics/soft/pathfinders.h

@@ -0,0 +1,48 @@
+#ifndef PATHFINDERS_H
+#define PATHFINDERS_H
+
+int BFSFindPath(const int nStartX, const int nStartY,
+		const int nTargetX, const int nTargetY,
+		const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		int* pOutBuffer, const int nOutBufferSize);
+
+int BFSFindPathDiag(const int nStartX, const int nStartY,
+		    const int nTargetX, const int nTargetY,
+		    const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		    int* pOutBuffer, const int nOutBufferSize);
+
+int AStarFindPath(const int nStartX, const int nStartY,
+		  const int nTargetX, const int nTargetY,
+		  const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		  int* pOutBuffer, const int nOutBufferSize);
+
+int AStarFindPathDiag(const int nStartX, const int nStartY,
+		      const int nTargetX, const int nTargetY,
+		      const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		      int* pOutBuffer, const int nOutBufferSize);
+
+void InitializeLandmarks(int k, const unsigned char* pMap, const int nMapWidth, const int nMapHeight);
+
+int AStarFindPathLandmarks(const int nStartX, const int nStartY,
+			   const int nTargetX, const int nTargetY,
+			   const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			   int* pOutBuffer, const int nOutBufferSize);
+
+void InitializeLandmarksDiag(int k, const unsigned char* pMap, const int nMapWidth, const int nMapHeight);
+
+int AStarFindPathLandmarksDiag(const int nStartX, const int nStartY,
+			       const int nTargetX, const int nTargetY,
+			       const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			       int* pOutBuffer, const int nOutBufferSize);
+
+int AStarFindPathNoTie(const int nStartX, const int nStartY,
+		       const int nTargetX, const int nTargetY,
+		       const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+		       int* pOutBuffer, const int nOutBufferSize);
+
+int AStarFindPathNoTieDiag(const int nStartX, const int nStartY,
+			   const int nTargetX, const int nTargetY,
+			   const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
+			   int* pOutBuffer, const int nOutBufferSize);
+
+#endif