n_astar.c

Go to the documentation of this file.

/*
 * Nilorea Library
 * Copyright (C) 2005-2026 Castagnier Mickael
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied. See the License for the specific language governing
 * permissions and limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
 
#include "nilorea/n_astar.h"
#include <string.h>
#include <math.h>
 
/* Internal helpers */
 
static inline int grid_index(const ASTAR_GRID* grid, int x, int y, int z) {
    return (z * grid->height + y) * grid->width + x;
}
 
static inline int in_bounds(const ASTAR_GRID* grid, int x, int y, int z) {
    return x >= 0 && x < grid->width &&
           y >= 0 && y < grid->height &&
           z >= 0 && z < grid->depth;
}
 
typedef struct ASTAR_BOX {
    int minx, miny, minz; 
    int bw, bh, bd;       
} ASTAR_BOX;
 
static inline int box_index(const ASTAR_BOX* b, int x, int y, int z) {
    return ((z - b->minz) * b->bh + (y - b->miny)) * b->bw + (x - b->minx);
}
 
static inline int in_box(const ASTAR_BOX* b, int x, int y, int z) {
    return x >= b->minx && x < b->minx + b->bw &&
           y >= b->miny && y < b->miny + b->bh &&
           z >= b->minz && z < b->minz + b->bd;
}
 
static inline int iabs(int v) {
    return v < 0 ? -v : v;
}
 
/* Binary Min-Heap (priority queue) */
 
static ASTAR_HEAP* heap_new(int capacity) {
    ASTAR_HEAP* h = (ASTAR_HEAP*)calloc(1, sizeof(ASTAR_HEAP));
    if (!h) return NULL;
 
    h->data = (ASTAR_HEAP_NODE*)malloc((size_t)capacity * sizeof(ASTAR_HEAP_NODE));
    if (!h->data) {
        free(h);
        return NULL;
    }
    h->size = 0;
    h->capacity = capacity;
    return h;
}
 
static void heap_free(ASTAR_HEAP* h) {
    if (!h) return;
    free(h->data);
    free(h);
}
 
static int heap_grow(ASTAR_HEAP* h) {
    int new_cap = h->capacity * 2;
    ASTAR_HEAP_NODE* new_data = (ASTAR_HEAP_NODE*)realloc(
        h->data, (size_t)new_cap * sizeof(ASTAR_HEAP_NODE));
    if (!new_data) return 0;
    h->data = new_data;
    h->capacity = new_cap;
    return 1;
}
 
static void heap_swap(ASTAR_HEAP_NODE* a, ASTAR_HEAP_NODE* b) {
    ASTAR_HEAP_NODE tmp = *a;
    *a = *b;
    *b = tmp;
}
 
static void heap_push(ASTAR_HEAP* h, int x, int y, int z, int f) {
    if (h->size >= h->capacity) {
        if (!heap_grow(h)) return;
    }
 
    int i = h->size++;
    h->data[i].x = x;
    h->data[i].y = y;
    h->data[i].z = z;
    h->data[i].f = f;
 
    /* Sift up */
    while (i > 0) {
        int parent = (i - 1) / 2;
        if (h->data[i].f < h->data[parent].f) {
            heap_swap(&h->data[i], &h->data[parent]);
            i = parent;
        } else {
            break;
        }
    }
}
 
static ASTAR_HEAP_NODE heap_pop(ASTAR_HEAP* h) {
    ASTAR_HEAP_NODE top = h->data[0];
    h->data[0] = h->data[--h->size];
 
    /* Sift down */
    int i = 0;
    for (;;) {
        int left = 2 * i + 1;
        int right = 2 * i + 2;
        int smallest = i;
 
        if (left < h->size && h->data[left].f < h->data[smallest].f)
            smallest = left;
        if (right < h->size && h->data[right].f < h->data[smallest].f)
            smallest = right;
 
        if (smallest != i) {
            heap_swap(&h->data[i], &h->data[smallest]);
            i = smallest;
        } else {
            break;
        }
    }
 
    return top;
}
 
/* Heuristic Functions */
 
int n_astar_heuristic(int x1, int y1, int z1, int x2, int y2, int z2, ASTAR_HEURISTIC heuristic) {
    int dx = iabs(x2 - x1);
    int dy = iabs(y2 - y1);
    int dz = iabs(z2 - z1);
 
    switch (heuristic) {
        case ASTAR_HEURISTIC_MANHATTAN:
            return (dx + dy + dz) * ASTAR_COST_CARDINAL;
 
        case ASTAR_HEURISTIC_EUCLIDEAN: {
            double dist = sqrt((double)(dx * dx + dy * dy + dz * dz));
            return (int)(dist * ASTAR_COST_CARDINAL);
        }
 
        case ASTAR_HEURISTIC_CHEBYSHEV:
        default: {
            int vals[3] = {dx, dy, dz};
            /* Sort ascending */
            if (vals[0] > vals[1]) {
                int t = vals[0];
                vals[0] = vals[1];
                vals[1] = t;
            }
            if (vals[1] > vals[2]) {
                int t = vals[1];
                vals[1] = vals[2];
                vals[2] = t;
            }
            if (vals[0] > vals[1]) {
                int t = vals[0];
                vals[0] = vals[1];
                vals[1] = t;
            }
 
            int cost = vals[2] * ASTAR_COST_CARDINAL;
            cost += vals[0] * (ASTAR_COST_DIAGONAL3D - ASTAR_COST_CARDINAL);
            cost += (vals[1] - vals[0]) * (ASTAR_COST_DIAGONAL - ASTAR_COST_CARDINAL);
            return cost;
        }
    }
}
 
/* Grid Management */
 
ASTAR_GRID* n_astar_grid_new(int width, int height, int depth) {
    if (width <= 0 || height <= 0 || depth <= 0) return NULL;
 
    ASTAR_GRID* grid = (ASTAR_GRID*)calloc(1, sizeof(ASTAR_GRID));
    if (!grid) return NULL;
 
    grid->width = width;
    grid->height = height;
    grid->depth = depth;
 
    size_t total = (size_t)width * (size_t)height * (size_t)depth;
 
    grid->walkable = (uint8_t*)malloc(total);
    if (!grid->walkable) {
        free(grid);
        return NULL;
    }
    memset(grid->walkable, 1, total);
 
    grid->cost = (int*)malloc(total * sizeof(int));
    if (!grid->cost) {
        free(grid->walkable);
        free(grid);
        return NULL;
    }
    for (size_t i = 0; i < total; i++) {
        grid->cost[i] = ASTAR_COST_CARDINAL;
    }
 
    return grid;
}
 
void n_astar_grid_free(ASTAR_GRID* grid) {
    if (!grid) return;
    free(grid->walkable);
    free(grid->cost);
    free(grid);
}
 
void n_astar_grid_set_walkable(ASTAR_GRID* grid, int x, int y, int z, uint8_t walkable) {
    if (!grid || !in_bounds(grid, x, y, z)) return;
    grid->walkable[grid_index(grid, x, y, z)] = walkable;
}
 
uint8_t n_astar_grid_get_walkable(const ASTAR_GRID* grid, int x, int y, int z) {
    if (!grid || !in_bounds(grid, x, y, z)) return 0;
    return grid->walkable[grid_index(grid, x, y, z)];
}
 
void n_astar_grid_set_cost(ASTAR_GRID* grid, int x, int y, int z, int cost) {
    if (!grid || !in_bounds(grid, x, y, z)) return;
    grid->cost[grid_index(grid, x, y, z)] = cost;
}
 
int n_astar_grid_get_cost(const ASTAR_GRID* grid, int x, int y, int z) {
    if (!grid || !in_bounds(grid, x, y, z)) return 0;
    return grid->cost[grid_index(grid, x, y, z)];
}
 
void n_astar_grid_set_rect_blocked(ASTAR_GRID* grid,
                                   int x1,
                                   int y1,
                                   int z1,
                                   int x2,
                                   int y2,
                                   int z2) {
    if (!grid) return;
    if (x1 > x2) {
        int t = x1;
        x1 = x2;
        x2 = t;
    }
    if (y1 > y2) {
        int t = y1;
        y1 = y2;
        y2 = t;
    }
    if (z1 > z2) {
        int t = z1;
        z1 = z2;
        z2 = t;
    }
 
    for (int z = z1; z <= z2; z++)
        for (int y = y1; y <= y2; y++)
            for (int x = x1; x <= x2; x++)
                n_astar_grid_set_walkable(grid, x, y, z, 0);
}
 
/* Path Reconstruction */
 
static ASTAR_PATH* reconstruct_path(ASTAR_CELL* cells, const ASTAR_BOX* box, int sx, int sy, int sz, int gx, int gy, int gz) {
    /* Count path length */
    int length = 0;
    int cx = gx, cy = gy, cz = gz;
    while (cx != sx || cy != sy || cz != sz) {
        length++;
        int idx = box_index(box, cx, cy, cz);
        int px = cells[idx].parent_x;
        int py = cells[idx].parent_y;
        int pz = cells[idx].parent_z;
        cx = px;
        cy = py;
        cz = pz;
        if (length > box->bw * box->bh * box->bd) {
            return NULL;
        }
    }
    length++;
 
    ASTAR_PATH* path = (ASTAR_PATH*)calloc(1, sizeof(ASTAR_PATH));
    if (!path) return NULL;
 
    path->nodes = (ASTAR_NODE*)malloc((size_t)length * sizeof(ASTAR_NODE));
    if (!path->nodes) {
        free(path);
        return NULL;
    }
    path->length = length;
    path->cost = cells[box_index(box, gx, gy, gz)].g;
 
    int i = length - 1;
    cx = gx;
    cy = gy;
    cz = gz;
    while (i >= 0) {
        path->nodes[i].x = cx;
        path->nodes[i].y = cy;
        path->nodes[i].z = cz;
        if (cx == sx && cy == sy && cz == sz) break;
        int idx = box_index(box, cx, cy, cz);
        int px = cells[idx].parent_x;
        int py = cells[idx].parent_y;
        int pz = cells[idx].parent_z;
        cx = px;
        cy = py;
        cz = pz;
        i--;
    }
 
    return path;
}
 
/* 2D Neighbor Directions */
 
static const int dir2d_cardinal[][2] = {
    {0, -1},
    {0, 1},
    {1, 0},
    {-1, 0}};
static const int dir2d_diagonal[][2] = {
    {1, -1},
    {-1, -1},
    {1, 1},
    {-1, 1}};
 
/* 3D Neighbor Directions */
 
static const int dir3d_cardinal[][3] = {
    {1, 0, 0},
    {-1, 0, 0},
    {0, 1, 0},
    {0, -1, 0},
    {0, 0, 1},
    {0, 0, -1}};
static const int dir3d_diagonal[][3] = {
    {1, 1, 0},
    {1, -1, 0},
    {-1, 1, 0},
    {-1, -1, 0},
    {1, 0, 1},
    {1, 0, -1},
    {-1, 0, 1},
    {-1, 0, -1},
    {0, 1, 1},
    {0, 1, -1},
    {0, -1, 1},
    {0, -1, -1},
    {1, 1, 1},
    {1, 1, -1},
    {1, -1, 1},
    {1, -1, -1},
    {-1, 1, 1},
    {-1, 1, -1},
    {-1, -1, 1},
    {-1, -1, -1}};
 
/* A* Core Algorithm */
 
ASTAR_PATH* n_astar_find_path(const ASTAR_GRID* grid,
                              int sx,
                              int sy,
                              int sz,
                              int gx,
                              int gy,
                              int gz,
                              int diagonal,
                              ASTAR_HEURISTIC heuristic) {
    if (!grid) return NULL;
    if (!in_bounds(grid, sx, sy, sz) || !in_bounds(grid, gx, gy, gz)) return NULL;
    if (!grid->walkable[grid_index(grid, sx, sy, sz)]) return NULL;
    if (!grid->walkable[grid_index(grid, gx, gy, gz)]) return NULL;
 
    /* Trivial case */
    if (sx == gx && sy == gy && sz == gz) {
        ASTAR_PATH* path = (ASTAR_PATH*)calloc(1, sizeof(ASTAR_PATH));
        if (!path) return NULL;
        path->nodes = (ASTAR_NODE*)malloc(sizeof(ASTAR_NODE));
        if (!path->nodes) {
            free(path);
            return NULL;
        }
        path->nodes[0].x = sx;
        path->nodes[0].y = sy;
        path->nodes[0].z = sz;
        path->length = 1;
        path->cost = 0;
        return path;
    }
 
    int is_3d = (grid->depth > 1);
 
    /* Window the per-call working set to the bounding box of (start, goal)
     * expanded by ASTAR_SEARCH_MARGIN and clamped to the grid, instead of
     * allocating one ASTAR_CELL per map tile. A 2400x2400 grid is 5.76M
     * cells (~161 MB) per calloc/free — fired on every chase repath, this
     * was the cause of multi-hundred-ms tick spikes. The grid's persistent
     * walkable[] / cost[] planes stay full-size and keep using the absolute
     * grid_index(); only the transient `cells` scratch is windowed, indexed
     * by box_index(). So per-call cost scales with the path length, not the
     * map size: a few KB for a short path. Trade-off: an optimal route that
     * detours more than ASTAR_SEARCH_MARGIN tiles outside the start-goal box
     * is not found through this window (returns NULL). For bounded game
     * paths that never happens; raise ASTAR_SEARCH_MARGIN if a caller needs
     * wider detours.
     *
     * calloc keeps zero-initialised cells (ASTAR_NODE_NONE == 0 == the
     * "unvisited" state); the search only touches the handful of cells it
     * explores, and reconstruct_path walks parents it has already written. */
    ASTAR_BOX box;
    {
        int lo = (sx < gx) ? sx : gx, hi = (sx > gx) ? sx : gx;
        box.minx = lo - ASTAR_SEARCH_MARGIN;
        if (box.minx < 0) box.minx = 0;
        hi += ASTAR_SEARCH_MARGIN;
        if (hi > grid->width - 1) hi = grid->width - 1;
        box.bw = hi - box.minx + 1;
 
        lo = (sy < gy) ? sy : gy;
        hi = (sy > gy) ? sy : gy;
        box.miny = lo - ASTAR_SEARCH_MARGIN;
        if (box.miny < 0) box.miny = 0;
        hi += ASTAR_SEARCH_MARGIN;
        if (hi > grid->height - 1) hi = grid->height - 1;
        box.bh = hi - box.miny + 1;
 
        if (is_3d) {
            lo = (sz < gz) ? sz : gz;
            hi = (sz > gz) ? sz : gz;
            box.minz = lo - ASTAR_SEARCH_MARGIN;
            if (box.minz < 0) box.minz = 0;
            hi += ASTAR_SEARCH_MARGIN;
            if (hi > grid->depth - 1) hi = grid->depth - 1;
            box.bd = hi - box.minz + 1;
        } else {
            box.minz = 0;
            box.bd = 1;
        }
    }
 
    size_t btotal = (size_t)box.bw * (size_t)box.bh * (size_t)box.bd;
    ASTAR_CELL* cells = (ASTAR_CELL*)calloc(btotal, sizeof(ASTAR_CELL));
    if (!cells) return NULL;
 
    ASTAR_HEAP* open = heap_new(256);
    if (!open) {
        free(cells);
        return NULL;
    }
 
    int si = box_index(&box, sx, sy, sz);
    cells[si].g = 0;
    cells[si].h = n_astar_heuristic(sx, sy, sz, gx, gy, gz, heuristic);
    cells[si].f = cells[si].h;
    cells[si].status = ASTAR_NODE_OPEN;
    heap_push(open, sx, sy, sz, cells[si].f);
 
    ASTAR_PATH* result = NULL;
 
    while (open->size > 0) {
        ASTAR_HEAP_NODE current = heap_pop(open);
        int cx = current.x, cy = current.y, cz = current.z;
        int ci = grid_index(grid, cx, cy, cz); /* absolute: cost[] */
        int lci = box_index(&box, cx, cy, cz); /* window-local: cells[] */
 
        if (cells[lci].status == ASTAR_NODE_CLOSED)
            continue;
 
        if (cx == gx && cy == gy && cz == gz) {
            result = reconstruct_path(cells, &box, sx, sy, sz, gx, gy, gz);
            break;
        }
 
        cells[lci].status = ASTAR_NODE_CLOSED;
 
        if (is_3d) {
            /* 3D: 6 cardinal directions */
            for (int d = 0; d < 6; d++) {
                int nx = cx + dir3d_cardinal[d][0];
                int ny = cy + dir3d_cardinal[d][1];
                int nz = cz + dir3d_cardinal[d][2];
                if (!in_box(&box, nx, ny, nz)) continue;
 
                int ni = grid_index(grid, nx, ny, nz);
                int lni = box_index(&box, nx, ny, nz);
                if (!grid->walkable[ni] || cells[lni].status == ASTAR_NODE_CLOSED)
                    continue;
 
                int move_cost = (grid->cost[ci] + grid->cost[ni]) / 2;
                int tentative_g = cells[lci].g + move_cost;
 
                if (cells[lni].status == ASTAR_NODE_NONE || tentative_g < cells[lni].g) {
                    cells[lni].g = tentative_g;
                    cells[lni].h = n_astar_heuristic(nx, ny, nz, gx, gy, gz, heuristic);
                    cells[lni].f = tentative_g + cells[lni].h;
                    cells[lni].parent_x = cx;
                    cells[lni].parent_y = cy;
                    cells[lni].parent_z = cz;
                    cells[lni].status = ASTAR_NODE_OPEN;
                    heap_push(open, nx, ny, nz, cells[lni].f);
                }
            }
 
            /* 3D: 20 diagonal directions */
            if (diagonal) {
                for (int d = 0; d < 20; d++) {
                    int ddx = dir3d_diagonal[d][0];
                    int ddy = dir3d_diagonal[d][1];
                    int ddz = dir3d_diagonal[d][2];
                    int nx = cx + ddx;
                    int ny = cy + ddy;
                    int nz = cz + ddz;
                    if (!in_box(&box, nx, ny, nz)) continue;
 
                    int ni = grid_index(grid, nx, ny, nz);
                    int lni = box_index(&box, nx, ny, nz);
                    if (!grid->walkable[ni] || cells[lni].status == ASTAR_NODE_CLOSED)
                        continue;
 
                    /* Corner-cutting check */
                    int blocked = 0;
                    if (ddx != 0 && ddy != 0 && !grid->walkable[grid_index(grid, cx + ddx, cy, cz)])
                        blocked = 1;
                    if (ddx != 0 && ddy != 0 && !grid->walkable[grid_index(grid, cx, cy + ddy, cz)])
                        blocked = 1;
                    if (ddx != 0 && ddz != 0 && !grid->walkable[grid_index(grid, cx + ddx, cy, cz)])
                        blocked = 1;
                    if (ddx != 0 && ddz != 0 && !grid->walkable[grid_index(grid, cx, cy, cz + ddz)])
                        blocked = 1;
                    if (ddy != 0 && ddz != 0 && !grid->walkable[grid_index(grid, cx, cy + ddy, cz)])
                        blocked = 1;
                    if (ddy != 0 && ddz != 0 && !grid->walkable[grid_index(grid, cx, cy, cz + ddz)])
                        blocked = 1;
                    if (blocked) continue;
 
                    int axes = (ddx != 0) + (ddy != 0) + (ddz != 0);
                    int base_cost;
                    if (axes == 3)
                        base_cost = ASTAR_COST_DIAGONAL3D;
                    else
                        base_cost = ASTAR_COST_DIAGONAL;
 
                    int cell_cost = (grid->cost[ci] + grid->cost[ni]) / 2;
                    int move_cost = (base_cost * cell_cost) / ASTAR_COST_CARDINAL;
                    int tentative_g = cells[lci].g + move_cost;
 
                    if (cells[lni].status == ASTAR_NODE_NONE || tentative_g < cells[lni].g) {
                        cells[lni].g = tentative_g;
                        cells[lni].h = n_astar_heuristic(nx, ny, nz, gx, gy, gz, heuristic);
                        cells[lni].f = tentative_g + cells[lni].h;
                        cells[lni].parent_x = cx;
                        cells[lni].parent_y = cy;
                        cells[lni].parent_z = cz;
                        cells[lni].status = ASTAR_NODE_OPEN;
                        heap_push(open, nx, ny, nz, cells[lni].f);
                    }
                }
            }
        } else {
            /* 2D: 4 cardinal directions */
            for (int d = 0; d < 4; d++) {
                int nx = cx + dir2d_cardinal[d][0];
                int ny = cy + dir2d_cardinal[d][1];
                if (!in_box(&box, nx, ny, 0)) continue;
 
                int ni = grid_index(grid, nx, ny, 0);
                int lni = box_index(&box, nx, ny, 0);
                if (!grid->walkable[ni] || cells[lni].status == ASTAR_NODE_CLOSED)
                    continue;
 
                int move_cost = (grid->cost[ci] + grid->cost[ni]) / 2;
                int tentative_g = cells[lci].g + move_cost;
 
                if (cells[lni].status == ASTAR_NODE_NONE || tentative_g < cells[lni].g) {
                    cells[lni].g = tentative_g;
                    cells[lni].h = n_astar_heuristic(nx, ny, 0, gx, gy, 0, heuristic);
                    cells[lni].f = tentative_g + cells[lni].h;
                    cells[lni].parent_x = cx;
                    cells[lni].parent_y = cy;
                    cells[lni].parent_z = 0;
                    cells[lni].status = ASTAR_NODE_OPEN;
                    heap_push(open, nx, ny, 0, cells[lni].f);
                }
            }
 
            /* 2D: 4 diagonal directions */
            if (diagonal) {
                for (int d = 0; d < 4; d++) {
                    int ddx = dir2d_diagonal[d][0];
                    int ddy = dir2d_diagonal[d][1];
                    int nx = cx + ddx;
                    int ny = cy + ddy;
                    if (!in_box(&box, nx, ny, 0)) continue;
 
                    int ni = grid_index(grid, nx, ny, 0);
                    int lni = box_index(&box, nx, ny, 0);
                    if (!grid->walkable[ni] || cells[lni].status == ASTAR_NODE_CLOSED)
                        continue;
 
                    if (!grid->walkable[grid_index(grid, cx + ddx, cy, 0)] ||
                        !grid->walkable[grid_index(grid, cx, cy + ddy, 0)])
                        continue;
 
                    int cell_cost = (grid->cost[ci] + grid->cost[ni]) / 2;
                    int move_cost = (ASTAR_COST_DIAGONAL * cell_cost) / ASTAR_COST_CARDINAL;
                    int tentative_g = cells[lci].g + move_cost;
 
                    if (cells[lni].status == ASTAR_NODE_NONE || tentative_g < cells[lni].g) {
                        cells[lni].g = tentative_g;
                        cells[lni].h = n_astar_heuristic(nx, ny, 0, gx, gy, 0, heuristic);
                        cells[lni].f = tentative_g + cells[lni].h;
                        cells[lni].parent_x = cx;
                        cells[lni].parent_y = cy;
                        cells[lni].parent_z = 0;
                        cells[lni].status = ASTAR_NODE_OPEN;
                        heap_push(open, nx, ny, 0, cells[lni].f);
                    }
                }
            }
        }
    }
 
    heap_free(open);
    free(cells);
    return result;
}
 
/* Path Cleanup */
 
void n_astar_path_free(ASTAR_PATH* path) {
    if (!path) return;
    free(path->nodes);
    free(path);
}