A* pathing in preallocated memory

path.c

/*Boost Software License - Version 1.0 - August 17th, 2003

Copyright (c) 2015 Freddy A Cubas "Superstar64"

Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:

The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.*/
#include "stdlib.h"
#include "stdio.h"
// the grid element
typedef struct node node;
struct node {
  int walkable;
  int fcost;
  // used for reconstructing the path
  node* parent;
  // the open list
  node* next;
  // optional used to prevent older searches from interfering with newer ones
  int reset_id;
};

typedef struct {
  node* grid;
  int w;
  int h;
} grid;

node* index_to_node(grid* grid, int x, int y) {
  return grid->grid + x + y * grid->w;
}

void node_to_index(grid* grid, node* node, int* x, int* y) {
  int id = node - grid->grid;
  *x = id % grid->w;
  *y = id / grid->w;
}

// add a node to the singly linked list, ordered by fcost
void add_node(node** head, node* node) {
  int fcost = node->fcost;
  while (*head) {
    // use <= if you want to check older nodes first
    // use < if you want to check newer nodes first
    if (fcost < (*head)->fcost) {
      node->next = *head;
      *head = node;
      return;
    }
    head = &(*head)->next;
  }
  *head = node;
  node->next = 0;
}

// remove a node from the singly linked list
// linear search
void remove_node(node** head, node* node) {
  while (*head) {
    if (*head == node) {
      *head = (*head)->next;
      return;
    }
    head = &(*head)->next;
  }
}

// tries to mark a node open
// ignores the node if it's closed or unwalkable
// recalculates a node if it's already open
void try_open(grid* grid, int x, int y, int end_x, int end_y, int reset_id,
              node* parent, int gcost, node** head) {
  int hcost = abs(end_x - x) + abs(end_y - y);
  int fcost = gcost + hcost;
  node* node = index_to_node(grid, x, y);

  if (!node->walkable) {
    // the node is unwalkable
    return;
  }

  if (node->reset_id != reset_id) {
    // node has data from old search
    node->fcost = fcost;
    node->parent = 0;
    node->next = 0;
    node->reset_id = reset_id;
  }

  if (node->parent && fcost == 0) {
    // the node is closed
    return;
  }
  if (node->parent) {
    // the node is open
    if (fcost < node->fcost) {
      // better path to the open node
      // reorder list with new fcost
      remove_node(head, node);
      node->fcost = fcost;
      node->parent = parent;
      add_node(head, node);
    }
    return;
  }
  // the node is unchecked
  node->fcost = fcost;
  node->parent = parent;
  add_node(head, node);
}

// expects the caller to remove it from the open list
// tries to open the nodes around it
void close_node(grid* grid, int x, int y, int end_x, int end_y, int reset_id,
                int gcost, node** head) {
  node* parent = index_to_node(grid, x, y);
  parent->fcost = 0;
  if (x != 0)
    try_open(grid, x - 1, y, end_x, end_y, reset_id, parent, gcost + 1, head);
  if (x != grid->w - 1)
    try_open(grid, x + 1, y, end_x, end_y, reset_id, parent, gcost + 1, head);
  if (y != 0)
    try_open(grid, x, y - 1, end_x, end_y, reset_id, parent, gcost + 1, head);
  if (y != grid->h - 1)
    try_open(grid, x, y + 1, end_x, end_y, reset_id, parent, gcost + 1, head);
}

// path finds from start to end
// the end node will contain a single linked list back to the start though
// end_node->parent
void path_find(grid* grid, int start_x, int start_y, int end_x, int end_y,
               int* reset_id) {
  node* head = 0;
  close_node(grid, start_x, start_y, end_x, end_y, *reset_id, 0, &head);
  node* start = index_to_node(grid, start_x, start_y);

  // mark start as closed
  start->fcost = 0;
  start->parent = start;
  start->next = 0;
  start->reset_id = *reset_id;

  while (head) {
    int x;
    int y;
    node_to_index(grid, head, &x, &y);
    if (x == end_x && y == end_y) {
      start->parent = 0;
      (*reset_id)++;
      return;
    }
    int hcost = abs(end_x - x) + abs(end_y - y);
    int gcost = head->fcost - hcost;
    head = head->next;
    close_node(grid, x, y, end_x, end_y, *reset_id, gcost, &head);
  }
  // could not find a path
  (*reset_id)++;
}

int main() {
  int data[8][8] = {{1, 1, 1, 1, 1, 0, 1, 1},
                    {1, 1, 1, 1, 1, 0, 1, 1},
                    {1, 1, 1, 1, 1, 0, 1, 1},
                    {1, 1, 1, 0, 0, 0, 1, 1},
                    {1, 1, 1, 1, 1, 0, 1, 1},
                    {0, 0, 0, 0, 1, 0, 1, 1},
                    {1, 1, 1, 1, 1, 1, 1, 1},
                    {1, 1, 1, 1, 1, 0, 1, 1}};

  node raw_grid[8][8];
  grid grid;
  grid.grid = &raw_grid[0][0];
  grid.w = 8;
  grid.h = 8;
  for (int x = 0; x < grid.w; x++) {
    for (int y = 0; y < grid.h; y++) {
      node* node = index_to_node(&grid, x, y);
      node->walkable = data[y][x];
      node->fcost = 0;
      node->parent = 0;
      node->next = 0;
      node->reset_id = 0;
    }
  }
  int reset_id = 0;
  path_find(&grid, 7, 7, 1, 1, &reset_id);
  node* node = index_to_node(&grid, 1, 1);
  while (node) {
    int x;
    int y;
    node_to_index(&grid, node, &x, &y);
    printf("x:%i y:%i\n", x, y);
    node = node->parent;
  }
}