path finder for a 2d game

pathfind.cpp

#include <iostream>
#include <cassert>
#include <cstdint>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <utility>
#include <string>




static constexpr int GAMEBOARD[] = {
    0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
    1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};

enum class PathDirect : char {
    Up = 'U',
    Down = 'D',
    Left = 'L',
    Right = 'R'
};

struct PathExecutor {
	float spriteX = 0.0f;
	float spriteY = 0.0f;
	float targetX = 0.0f;
	float targetY = 0.0f;
	float sqaureSize = 0.0f;

	float increment = 0.0f;

	const char* curPath = nullptr;

	void setTargetDirection() {
		if (curPath == nullptr) {
			return;
		}
		PathDirect pdir = static_cast<PathDirect>(*curPath);
		 switch (pdir) {
		 case PathDirect::Up:
		 	targetY = spriteY - sqaureSize;
		 	targetX = spriteX;
		 	break;
		 case PathDirect::Down:
		 	targetY = spriteY + sqaureSize;
		 	targetX = spriteX;
		 	break;
		 case PathDirect::Left:
		 	targetX = spriteX - sqaureSize;
		 	targetY = spriteY;
		 	break;
		 case PathDirect::Right:
		 	targetX = spriteX + sqaureSize;
		 	targetY = spriteY;
		 	break;
		 }
	}

	void begin() {
		setTargetDirection();
	}

	void moveSprite() {
		// called at every frame
		if (curPath == nullptr)
			return;
		PathDirect pdir = static_cast<PathDirect>(*curPath);

		switch (pdir) {
		 case PathDirect::Up:
		 	if (targetY < spriteY) {
		 		spriteY -= increment;
		 		return;
		 	}
		 	break;
		 case PathDirect::Down:
		 	if (targetY > spriteY) {
		 		spriteY += increment;
		 		return;
		 	}
		 	break;
		 case PathDirect::Left:
		 	if (targetX < spriteX) {
		 		spriteX -= increment;
		 		return;
		 	}
		 	break;
		 case PathDirect::Right:
		 	if (targetX > spriteX) {
		 		spriteX += increment;
		 		return;
		 	}
		 	break;
		}
		++curPath;
		if (*curPath == '\0') {
			curPath = nullptr;
			return;
		}
		// todo this lags by one iteration, think if we should fix it or not
		setTargetDirection();
	}
};

enum class MoveResult {
	Moved,
	Finished,
	Blocked
};

/**
 * The end condition is when one is at start, and any adjacent space is already visited.
 * Backward move is only possible if pathlen is non zero
 * at any point, there is UDLR moves, or B for backward.
 * */
struct PathFinder {
	int* gameBoard = nullptr;
	char* path = nullptr;
	size_t boardSize = 0;
	size_t pathLen = 0;
	int boardX = -1;
	int boardY = -1;
	int curX = -1;
	int curY = -1;
	int endX = -1;
	int endY = -1;
	int startX = -1;
	int startY = -1;

	PathFinder(const int* board, int boardx, int boardy, 
		       int startx, int starty,
		       int endx, int endy) {
		boardX = boardx;
		boardY = boardy;
		boardSize = boardX * boardY;
		startX = startx;
		startY = starty;
		endX = endx;
		endY = endy;

		curX = startX;
		curY = startY;

		gameBoard = new int[boardSize];
		std::memcpy(gameBoard, board, boardSize * sizeof(int)); // copy so  we can just mark in same space what is visited

		path = new char[boardSize + 1];
		std::memset(path, 0, boardSize + 1);
	}


	PathFinder(const int* board, int boardx, int boardy) {
		boardX = boardx;
		boardY = boardy;
		boardSize = boardX * boardY;

		gameBoard = new int[boardSize];
		std::memcpy(gameBoard, board, boardSize * sizeof(int)); // copy so  we can just mark in same space what is visited

		path = new char[boardSize + 1];
		std::memset(path, 0, boardSize + 1);
	}

	~PathFinder() {
		delete[] gameBoard;
		delete[] path;
	}


	void resetPath(const int* board, int startx, int starty,
		       int endx, int endy) {
		startX = startx;
		startY = starty;
		endX = endx;
		endY = endy;

		curX = startX;
		curY = startY;
		std::memset(path, 0, boardSize + 1);

		std::memcpy(gameBoard, board, boardSize * sizeof(int));
		pathLen = 0;
	}

	static std::string createPath(const int* board, int boardx, int boardy, 
		       int startx, int starty,
		       int endx, int endy) {
		PathFinder finder(board, boardx, boardy, startx, starty, endx, endy);
		finder.findPath();
		return std::string(finder.path);
	}

	bool isAtEnd() const {
		return endX == curX && endY == curY;
	}

	bool isWalkable(int x, int y) const {
		return gameBoard[(y * boardX) + x] == 0;
	}

	void markSpotNonWalkable(int x, int y) {
		gameBoard[(y * boardX) + x] = 1;
	} 

	bool canMoveUp() const {
		return curY != 0 && isWalkable(curX, curY - 1);
	}

	bool canMoveDown() const {
		return (curY != (boardY - 1)) && isWalkable(curX, curY + 1);
	}

	bool canMoveLeft() const {
		return curX != 0 && isWalkable(curX - 1, curY);
	}

	bool canMoveRight() const {
		return (curX != (boardX - 1)) && isWalkable(curX + 1, curY);
	}

	void moveUp() {
		markSpotNonWalkable(curX, curY);
		--curY;
		path[pathLen++] = (char)PathDirect::Up;
	}

	void moveDown() {
		markSpotNonWalkable(curX, curY);
		++curY;
		path[pathLen++] = (char)PathDirect::Down;
	}

	void moveLeft() {
		markSpotNonWalkable(curX, curY);
		--curX;
		path[pathLen++] = (char)PathDirect::Left;
	}

	void moveRight() {
		markSpotNonWalkable(curX, curY);
		++curX;
		path[pathLen++] = (char)PathDirect::Right;
	}

	bool canReverse() const {
		return pathLen > 0;
	}

	void reverse() {
		printf("Reversing!\n");
		markSpotNonWalkable(curX, curY);
		size_t prevDirIndex = --pathLen;
		char prevDir = path[prevDirIndex];
		PathDirect d = static_cast<PathDirect>(prevDir);
		switch (d) {
		case PathDirect::Up:
			curY += 1;
			break;
		case PathDirect::Down:
			curY -= 1;
			break;
		case PathDirect::Left:
			curX += 1;
			break;
		case PathDirect::Right:
			curX -= 1;
			break;
		}
		path[prevDirIndex] = '\0';
	}

	bool attemptAnyMove() {
		if (canMoveUp()) {
			moveUp();
			return true;
		}
		if (canMoveDown()) {
			moveDown();
			return true;
		}
		if (canMoveLeft()) {
			moveLeft();
			return true;
		}
		if (canMoveRight()) {
			moveRight();
			return true;
		}
		return false;
	}

	MoveResult move() {
		if (isAtEnd()) {
			return MoveResult::Finished;
		}
		int xdist = endX - curX;
		int ydist = endY - curY;

		if (xdist > 0) {
			// move right
			if (canMoveRight()) {
				moveRight();
				return MoveResult::Moved;
			}
		} 
		if (xdist < 0) {
			// move left
			if (canMoveLeft()) {
				moveLeft();
				return MoveResult::Moved;
			}
		}

		if (ydist > 0) {
			// move down
			if (canMoveDown()) {
				moveDown();
				return MoveResult::Moved;
			}
		}
		if (ydist < 0) {
			// move up
			if (canMoveUp()) {
				moveUp();
				return MoveResult::Moved;
			}
		}

		if (attemptAnyMove()) {
			return MoveResult::Moved;
		}

		if (canReverse()) {
			reverse();
			return MoveResult::Moved;
		}
		return MoveResult::Blocked;

	}

	size_t findPath() {
		if (isAtEnd())
			return 0;
		size_t foundLen = 1;
		auto res = move();
		while (res == MoveResult::Moved) {
			++foundLen;
			res = move();
		}
		return foundLen;
	}

};

static constexpr size_t SQ_MAX_X = 16;
static constexpr size_t SQ_MAX_Y = 8;
static constexpr size_t SQ_TOTAL_CNT = sizeof(GAMEBOARD) / sizeof(int);
static constexpr size_t SQ_LAST_CNT = SQ_TOTAL_CNT - 1;
static_assert(SQ_TOTAL_CNT == (SQ_MAX_X * SQ_MAX_Y), "Board dimension not equal to array size");

static size_t coordToPos(size_t x, size_t y) {
    return (y * SQ_MAX_X) + x;
}

static void posToCoord(size_t pos, size_t* x, size_t* y) {
    *x = pos & (SQ_MAX_X - 1);
    *y = pos / SQ_MAX_X;
}

static size_t manhattanDist(size_t x1, size_t y1, size_t x2, size_t y2) {
	return std::abs((long)x1 - (long)x2) + std::abs((long)y1 - (long)y2);
}


static void canvasTranslate(float focusX, float focusY, 
	                        float screenX, float screenY,
	                        float inputX, float inputY,
	                        float* outputX, float* outputY) {
	float cameraX = focusX - (screenX / 2.0f);
	float cameraY = focusY - (screenY / 2.0f);

	// todo consider clamp

	*outputX = inputX - cameraX;
	*outputY = inputY - cameraY;
}

static void canvasWarp(float focusX, float focusY, 
	                        float screenX, float screenY,
	                        float inputX, float inputY,
	                        float* outputX, float* outputY) {
	float cameraX = focusX - (screenX / 2.0f);
	float cameraY = focusY - (screenY / 2.0f);

	// todo consider clamp

	*outputX = inputX + cameraX;
	*outputY = inputY + cameraY;
}

static void printCanvasWarp(float focusX, float focusY, 
	                        float screenX, float screenY,
	                        float inputX, float inputY) {
	float outx = 0.0f;
	float outy = 0.0f;
	canvasWarp(focusX, focusY, screenX, screenY, inputX, inputY, &outx, &outy);
	printf("WARP inputX,Y=(%.1f, %.1f) outputX,Y=(%.1f, %.1f)\n", inputX, inputY, outx, outy);
}


static void printCanvasTranslate(float focusX, float focusY, 
	                        float screenX, float screenY,
	                        float inputX, float inputY) {
	float outx = 0.0f;
	float outy = 0.0f;
	canvasTranslate(focusX, focusY, screenX, screenY, inputX, inputY, &outx, &outy);
	printf("inputX,Y=(%.1f, %.1f) outputX,Y=(%.1f, %.1f)\n", inputX, inputY, outx, outy);
}


int main(int argc, char const *argv[])
{
	PathFinder foo(GAMEBOARD, SQ_MAX_X, SQ_MAX_Y, 5, 1, 13, 0);
	printf("can move up (%s)\n", foo.canMoveUp() ? "1" : "0");

	size_t counted = foo.findPath();
	printf("path (%s), len (%zu)\n", foo.path, counted);

	PathExecutor exct;
	exct.curPath = foo.path;
	exct.sqaureSize = 16.0f;
	exct.spriteX = 5.0f * 16.0f;
	exct.spriteY = 1.0f * 16.0f;
	exct.increment = 4.0f;

	exct.begin();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	exct.moveSprite();
	printf("sprite_x=%.1f, sprite_y=%.1f, targ_x=%.1f, targ_y=%.1f\n", exct.spriteX, exct.spriteY, exct.targetX, exct.targetY);
	printf("currentPath=%s\n", exct.curPath);

	foo.resetPath(GAMEBOARD, 5, 1, 13, 0);
	printf("can move up (%s)\n", foo.canMoveUp() ? "1" : "0");

	counted = foo.findPath();
	printf("path (%s), len (%zu)\n", foo.path, counted);

	const std::string created = PathFinder::createPath(GAMEBOARD, SQ_MAX_X, SQ_MAX_Y, 5, 1, 13, 0);
	printf("pathCreated (%s)\n", created.c_str());

	printCanvasTranslate(79, 79, 80, 80, 20, 20);
	printCanvasWarp(79, 79, 80, 80, 35, 35);

	return 0;
}