Ascent-of-Man/pathfinder.cpp

#include "pathfinder.h"
#include "terrainmovementmanager.h"
#include "unitconfig.h"
#include <cmath>
#include <queue>
#include <unordered_map>
#include <algorithm>

QVector2D Pathfinder::hexToWorld(int x, int y)
{
    float wx = x * (LOD_0_SIZE * 0.75f);
    float wy = y * (LOD_0_SIZE_Y * 2.0f) + ((x % 2 == 0) ? 0.0f : LOD_0_SIZE_Y);
    return QVector2D(wx, wy);
}

void Pathfinder::worldToHex(float wx, float wy, int &outX, int &outY)
{
    float stepX = LOD_0_SIZE * 0.75f;
    float stepY = LOD_0_SIZE_Y * 2.0f;

    int approxX = (int) std::round(wx / stepX);
    int approxY = (int) std::round(wy / stepY);

    float minDistSq = 1e9f;
    int bestX = 0;
    int bestY = 0;

    for (int dx = -2; dx <= 2; ++dx) {
        for (int dy = -2; dy <= 2; ++dy) {
            int cx = approxX + dx;
            int cy = approxY + dy;
            if (cx >= 0 && cx < MAP_WIDTH && cy >= 0 && cy < MAP_HEIGHT) {
                QVector2D center = hexToWorld(cx, cy);
                float distSq = (wx - center.x()) * (wx - center.x()) + (wy - center.y()) * (wy - center.y());
                if (distSq < minDistSq) {
                    minDistSq = distSq;
                    bestX = cx;
                    bestY = cy;
                }
            }
        }
    }

    outX = bestX;
    outY = bestY;
}

int Pathfinder::hexDistance(int x1, int y1, int x2, int y2)
{
    int q1 = x1;
    int r1 = y1 - (x1 - (x1 & 1)) / 2;
    int s1 = -q1 - r1;

    int q2 = x2;
    int r2 = y2 - (x2 - (x2 & 1)) / 2;
    int s2 = -q2 - r2;

    return (std::abs(q1 - q2) + std::abs(r1 - r2) + std::abs(s1 - s2)) / 2;
}

namespace {
    struct Node {
        int x, y;
        float g, f;
        bool operator>(const Node &other) const {
            return f > other.f;
        }
    };

    inline int packCoord(int x, int y) {
        return (y << 16) | (x & 0xFFFF);
    }
}

std::vector<std::pair<int, int>> Pathfinder::findPath(
    int startX, int startY,
    int goalX, int goalY,
    Matrix2D<TerrainPoint> *mapPoints,
    int numPoints05,
    MovementDomain domain,
    const UnitConfig *unitConfig)
{
    std::vector<std::pair<int, int>> path;
    if (startX == goalX && startY == goalY) {
        path.emplace_back(startX, startY);
        return path;
    }

    if (goalX < 0 || goalX >= MAP_WIDTH || goalY < 0 || goalY >= MAP_HEIGHT) {
        return path;
    }

    // Goal hex must be passable for this unit/domain
    if (!TerrainMovementManager::instance().isHexPassable(goalX, goalY, domain, mapPoints, numPoints05, unitConfig)) {
        return path;
    }

    std::priority_queue<Node, std::vector<Node>, std::greater<Node>> openSet;
    std::unordered_map<int, float> gScore;
    std::unordered_map<int, int> cameFrom;

    int startPacked = packCoord(startX, startY);
    gScore[startPacked] = 0.0f;
    float minCost = TerrainMovementManager::instance().getConfig().minStepCost;
    openSet.push({startX, startY, 0.0f, (float)hexDistance(startX, startY, goalX, goalY) * minCost});

    int maxIterations = 5000;
    int iterations = 0;

    bool found = false;

    while (!openSet.empty() && iterations++ < maxIterations) {
        Node current = openSet.top();
        openSet.pop();

        if (current.x == goalX && current.y == goalY) {
            found = true;
            break;
        }

        int currPacked = packCoord(current.x, current.y);
        if (current.g > gScore[currPacked]) continue;

        for (int edge = 0; edge < 6; ++edge) {
            int nx, ny;
            getHexNeighbor(current.x, current.y, edge, nx, ny);

            if (nx < 0 || nx >= MAP_WIDTH || ny < 0 || ny >= MAP_HEIGHT) continue;

            // Calculate step cost taking into account terrain penalty and slope (uphill penalty or downhill bonus)
            float stepCost = TerrainMovementManager::instance().calculateStepCost(
                current.x, current.y, nx, ny, domain, mapPoints, numPoints05, unitConfig
            );

            // If impassable or cliff too steep
            if (stepCost < 0.0f) {
                continue;
            }

            float tentativeG = current.g + stepCost;
            int nPacked = packCoord(nx, ny);

            if (!gScore.count(nPacked) || tentativeG < gScore[nPacked]) {
                gScore[nPacked] = tentativeG;
                cameFrom[nPacked] = currPacked;
                float h = (float) hexDistance(nx, ny, goalX, goalY) * minCost;
                openSet.push({nx, ny, tentativeG, tentativeG + h});
            }
        }
    }

    if (found) {
        int curr = packCoord(goalX, goalY);
        while (curr != startPacked) {
            int cx = curr & 0xFFFF;
            int cy = curr >> 16;
            path.emplace_back(cx, cy);
            curr = cameFrom[curr];
        }
        path.emplace_back(startX, startY);
        std::reverse(path.begin(), path.end());
    }

    return path;
}