map-tests.cpp

/*
  Some parts adapted from
  http://www.redblobgames.com/pathfinding/a-star/implementation.html
*/

#include <array>
#include <random>
#include <algorithm>
#include <iostream>
#include <queue>
#include <unordered_map>
#include <unistd.h>

using namespace std;

#define SX 127
#define SY 60

struct MapPoint {
    MapPoint();
    MapPoint(int n);
    MapPoint(int_fast16_t nx, int_fast16_t ny);

    int_fast16_t x, y;
};
    
struct Tile {
    Tile();

    MapPoint pos;
    bool visited;
    int_fast16_t tile_id;

};

struct Map {
    array<Tile, SX * SY> tiles;

    void init(void);
    void draw(void);

    bool inBounds(const MapPoint & p);
    int getOffset(int x, int y);
    float distance(const MapPoint & p1, const MapPoint & p2);
    float heuristic(size_t n1, size_t n2);

    void gen(int n, int seed);
    vector<int> path_find(int start, int goal);
    vector<int> path_find(int x1, int y1, int x2, int y2);
    float move_cost(int n);
    int dir_transform(int n, int_fast16_t dir);
    bool valid(int n);
    bool valid(int_fast16_t x, int_fast16_t y);
    void draw_line(int x1, int y1, int x2, int y2);
};

void Map::draw_line(int x1, int y1, int x2, int y2) {
    int dx =   abs(x2 - x1);
    int dy = - abs(y2 - y1);
    int sx = x1 < x2 ? 1 : -1;
    int sy = y1 < y2 ? 1 : -1;
    int err = dx + dy;
    int x = x1;
    int y = y1;

    while(true) {
        tiles[getOffset(x, y)].tile_id = 'L';
        if(x == x2 && y == y2) {
            break;
        }
        int e2 = 2 * err;
        if(e2 >= dy) {
            err = err + dy;
            x = x + sx;
        }
        if(e2 <= dx) {
            err = err + dx;
            y = y + sy;
        }
    }
}

bool Map::valid(int_fast16_t x, int_fast16_t y) {
    return
        x >= 0 &&
        y >= 0 &&
        x <= SX - 1 &&
        y <= SY - 1;
}

bool Map::valid(int n) {
    if(n <= 0 || n >= SX * SY)
        return false;

    int_fast16_t x = tiles[n].pos.x;
    int_fast16_t y = tiles[n].pos.y;

    return valid(x, y);
}

int Map::dir_transform(int n, int_fast16_t dir) {
    int_fast16_t x = tiles[n].pos.x;
    int_fast16_t y = tiles[n].pos.y;
    
    if(dir == 1 && y < SY - 2)
        return getOffset(x, y + 1);
    else if(dir == 2 && y > 0)
        return getOffset(x, y - 1);
    else if(dir == 3 && x < SX - 1)
        return getOffset(x + 1, y);
    else if(dir == 4 && x > 0)
        return getOffset(x - 1, y);
    else if(dir == 5 && x > 0 && y > 0)
        return getOffset(x - 1, y - 1);
    else if(dir == 6 && x > 0 && y < SY - 1)
        return getOffset(x - 1, y + 1);
    else if(dir == 7 && x < SX - 1 && y > 0)
        return getOffset(x + 1, y - 1);
    else if(dir == 8 && x < SX - 1 && y < SY - 1)
        return getOffset(x + 1, y + 1);

    return -1;
}

template<typename T, typename Number=float>
struct PriorityQueue {
    typedef pair<Number, T> PQElement;
    priority_queue<PQElement, vector<PQElement>,
                   std::greater<PQElement>> elements;

    inline bool empty() { return elements.empty(); }

    inline void put(T item, Number priority) {
        elements.emplace(priority, item);
    }

    inline T get() {
        T best_item = elements.top().second;
        elements.pop();
        return best_item;
    }
};

vector<int> Map::path_find(int x1, int y1, int x2, int y2) {
    return path_find(getOffset(x1, y1), getOffset(x2, y2));
}

inline float Map::heuristic(size_t n1, size_t n2) {
    int16_t d1 = abs(tiles[n1].pos.x - tiles[n2].pos.x);
    int16_t d2 = abs(tiles[n1].pos.y - tiles[n2].pos.y);
    constexpr float D = 1;
    constexpr float D2 = sqrt(2.0) * D;
    
    return D * (d1 + d2) + (D2 - 2 * D) * min(d1, d2);
    // return D * max(abs(d1), abs(d2));
}


vector<int> Map::path_find(int start, int goal) {
    PriorityQueue<int> frontier;
    frontier.put(start, 0);

    unordered_map<int, int> came_from;
    came_from.emplace(start, start);

    unordered_map<int, float> cost_so_far;
    cost_so_far.emplace(start, 0);

    while(frontier.empty() == false) {
        int current = frontier.get();

        if(current == goal)
            break;

        for(int_fast16_t dir = 1; dir <= 8; dir++) {
            int next = dir_transform(current, dir);

            if(next == -1)
                continue;

            constexpr float sqrt2 = sqrt(2.0);
            // directions 1-4 - cardinal
            // directions 5-8 - diagonal
            float factor = dir >= 5 ? sqrt2 : 1.0;
            float new_cost = cost_so_far[current] + move_cost(next) * factor;

            if(cost_so_far.count(next) == 0 || new_cost < cost_so_far[next]) {
                cost_so_far[next] = new_cost;
                tiles[next].visited = true;
                float priority = new_cost + heuristic(next, goal);
                frontier.put(next, priority);
                came_from.emplace(next, current);
                // draw();
                // sleep(1);
            }
        }
    }

    int current = goal;

    vector<int> path = { current };

    while(current != start) {
        current = came_from[current];
        path.push_back(current);
    }

    return path;
}

float Map::move_cost(int n) {
    if(tiles[n].tile_id == '.')
        return 1;
    else if(tiles[n].tile_id == 'T')
        return 5;
    else
        return 127;
}

MapPoint::MapPoint() {
}

Tile::Tile() {
}

MapPoint::MapPoint(int_fast16_t nx, int_fast16_t ny) {
    x = nx; y = ny;
}

MapPoint::MapPoint(int n) {
    x = n % SX;
    y = n / SX;
}

inline bool Map::inBounds(const MapPoint & p) {
    return
        p.x >= 0 &&
        p.x < SX &&
        p.y >= 0 &&
        p.y < SY;
}

inline float Map::distance(const MapPoint & p1, const MapPoint & p2) {
    int16_t d1 = p1.x - p2.x;
    int16_t d2 = p1.y - p2.y;
    return d1 * d1 + d2 * d2;
}

inline int Map::getOffset(int x, int y) {
    return y * SX + x;
}

void Map::init(void) {
    int_fast16_t x = 0;
    int_fast16_t y = 0;
    for(auto&& point : tiles) {
        point.tile_id = ' ';
        point.visited = false;
        point.pos = MapPoint(x, y);
        if(x == SX - 1) {
            y++;
            x = 0;
        } else
            x++;
    }
}

void Map::draw(void) {
    for(int y = 0; y < SY; y++) {
        for(int x = 0; x < SX; x++) {
            int n = getOffset(x, y);
            bool visited = tiles[n].visited;
            char c = tiles[n].tile_id;
            if(c == 'T')
                if(!visited)
                    printf("%c", c);
                else
                    printf("\x1b[32m%c\x1b[0m", c);
            else if(c == '.')
                if(!visited)
                    printf("%c", c);
                else
                    printf("\x1b[33m%c\x1b[0m", c);
            else
                printf("%c", c);

        }
        printf("\n");
    }
}

void Map::gen(int n_points, int seed) {
    vector<MapPoint> land_points(n_points + SX);
    vector<MapPoint> forest_points(n_points + SX);

    random_device rd;
    mt19937 g(seed);
    uniform_int_distribution<int> point_gen(0, SX * SY);

    // add random points
    for(int i = 0; i < n_points; i++) {
        land_points.push_back(MapPoint(point_gen(g)));
        forest_points.push_back(MapPoint(point_gen(g)));
    }

    // add the bottom points from the previous map to the top
    // of the new map
    for(int x = 0; x < SX; x++) {
        int n = getOffset(x, SY - 1);
        if(tiles[n].tile_id == 'T')
            forest_points.push_back(tiles[x].pos);
        else if(tiles[n].tile_id == '.')
            land_points.push_back(tiles[x].pos);
    }

    for(auto&& t : tiles) {

        int nearest_land_point_dist = 99999;
        int nearest_forest_point_dist = 99999;

        // find nearest point for this tile
        for(auto&& lp : land_points) {
            int dist = distance(t.pos, lp);
            if(dist < nearest_land_point_dist)
                nearest_land_point_dist = dist;
        }
        for(auto&& fp : forest_points) {
            int dist = distance(t.pos, fp);
            if(dist < nearest_forest_point_dist)
                nearest_forest_point_dist = dist;
        }

        // the tile is whatever the nearest point is
        if(nearest_forest_point_dist <= nearest_land_point_dist)
            t.tile_id = '.';
        else
            t.tile_id = 'T';
    }
}

int main(int argc, char **argv)
{
    Map m;

    m.init();
    m.gen(atoi(argv[1]), atoi(argv[2]));

    // vector<int> path = m.path_find(SX / 4, SY / 3, 3 * SX / 4, 2 * SY / 3);
    vector<int> path = m.path_find(0, 0, SX - 1, SY - 1);

    for(auto&& n : path) {
        m.tiles[n].tile_id = 'X';
    }

    m.draw();
}