WSP-Navigate: An Optimal 2D Grid Navigation Increment Calculation Algorithm
William Stafford Parsons developed a grid navigation increment calculation algorithm as a substantial improvement to all similar algorithms for navigating rectangular grids in a 2D plane.
Library
Source
struct wsp_navigate_s {
unsigned long grid_width;
unsigned long grid_height;
unsigned long source;
unsigned long destination;
long increment;
long increment_bottom_left;
long increment_bottom_right;
long increment_top_center;
long increment_top_left;
long increment_top_right;
char increment_x;
char increment_y;
unsigned long source_x;
unsigned long source_y;
unsigned long destination_x;
unsigned long destination_y;
unsigned long repetitions_count;
};
void wsp_navigate_initialize_bounds(unsigned long width, unsigned long height,
struct wsp_navigate_s *s) {
s->grid_width = width;
s->grid_height = height;
s->increment_bottom_left = width - 1;
s->increment_bottom_right = width + 1;
s->increment_top_center = -width;
s->increment_top_left = -1 - width;
s->increment_top_right = 1 - width;
}
void wsp_navigate_initialize_points(unsigned long source,
unsigned long destination,
struct wsp_navigate_s *s) {
s->source = source;
s->destination = destination;
s->increment = 0;
s->source_y = source / s->grid_width;
s->source_x = source - (s->grid_width * s->source_y);
s->destination_y = destination / s->grid_width;
s->destination_x = destination - (s->grid_width * s->destination_y);
s->repetitions_count = 0;
}
void wsp_navigate_increment(struct wsp_navigate_s *s) {
if (s->repetitions_count == 0) {
if (s->source_x == s->destination_x) {
s->increment_x = 0;
if (s->source_y != s->destination_y) {
if (s->source_y < s->destination_y) {
s->increment = s->grid_width;
s->increment_y = 1;
s->repetitions_count = s->destination_y - s->source_y;
goto apply_increment;
}
s->increment = s->increment_top_center;
s->increment_y = -1;
s->repetitions_count = s->source_y - s->destination_y;
goto apply_increment;
}
s->increment = 0;
s->increment_y = 0;
goto apply_increment;
}
if (s->source_y == s->destination_y) {
s->increment_y = 0;
if (s->source_x < s->destination_x) {
s->increment = 1;
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
goto apply_increment;
}
s->increment = -1;
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
goto apply_increment;
}
if (s->source_x < s->destination_x) {
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
if (s->source_y < s->destination_y) {
s->increment = s->increment_bottom_right;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
goto apply_increment;
}
s->increment = s->increment_top_right;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
goto apply_increment;
}
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
if (s->source_y < s->destination_y) {
s->increment = s->increment_bottom_left;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
goto apply_increment;
}
s->increment = s->increment_top_left;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
}
apply_increment:
s->source += s->increment;
s->source_x += s->increment_x;
s->source_y += s->increment_y;
s->repetitions_count--;
}Reference
wsp_navigate_initialize_bounds() is the grid width and height bounds initialization function that accepts the following 3 arguments.
width is the width of the grid.
height is the height of the grid.
The result of width * height must be less than or equal to ULONG_MAX - 3.
s is a struct wsp_navigate_s pointer.
The return value data type is void.
wsp_navigate_initialize_points() is the grid source and destination points initialization function that accepts the following 3 arguments.
source is the starting position as an index in the grid.
destination is the ending position as an index in the grid.
s is a struct wsp_navigate_s pointer.
The return value data type is void.
wsp_navigate_increment() is the navigation increment function that accepts the following argument.
s is the struct wsp_navigate_s pointer.
The return value data type is void.
Requirements
C compiler with C89 (ISO/IEC 9899:1990) standard compatibility.
Explanation
WSP-Navigate is designed to calculate the next step in 1 of 8 possible directions within the bounds of a rectangular grid by knowing the grid width, grid height, source index position and destination index position.
It's portable for both 32-bit and 64-bit systems.
It meets strict compliance, portability and code security requirements.
For example, the following grid array uses a source 1 at index position 0 and a destination 2 at index position 95.
unsigned char grid[100] = {
1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 2, 0, 0, 0, 0
};The next step forward towards the destination 2 from the source 1 is at index position 11.
In this case, the wsp_navigate_increment() function increments s.source to advance to this position.
This is applied recursively until s.source reaches s.destination as demonstrated in the following code example.
#include <stdio.h>
#include "wsp_navigate.h"
int main(void) {
struct wsp_navigate_s s;
unsigned char grid[100] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
unsigned char i = 0;
wsp_navigate_initialize_bounds(10, 10, &s);
wsp_navigate_initialize_points(0, 95, &s);
grid[s.source] = 1;
while (s.source != s.destination) {
wsp_navigate_increment(&s);
grid[s.source] = 3;
}
grid[s.destination] = 2;
i = 0;
while (i != 100) {
printf("%u ", grid[i]);
if (((i + 1) % 10) == 0) {
printf("\n");
}
i++;
}
return 0;
}The following grid output is the traversal from the source to the destination with each step marked as 3.
1 0 0 0 0 0 0 0 0 0
0 3 0 0 0 0 0 0 0 0
0 0 3 0 0 0 0 0 0 0
0 0 0 3 0 0 0 0 0 0
0 0 0 0 3 0 0 0 0 0
0 0 0 0 0 3 0 0 0 0
0 0 0 0 0 3 0 0 0 0
0 0 0 0 0 3 0 0 0 0
0 0 0 0 0 3 0 0 0 0
0 0 0 0 0 2 0 0 0 0This simple calculation could be processed billions of times per minute, so it's necessary to micro-optimize wherever possible.
There are plenty of ways to do this wrong by calculating steps incorrectly and wasting CPU cycles and memory.
For example, the following code is the first attempt at creating this algorithm without optimization.
long navigate_bad(const unsigned long grid_width,
const unsigned long grid_height, const unsigned long source,
const unsigned long destination) {
long map[10] = {0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
const unsigned long source_x = source % grid_width;
const unsigned long source_y = source / grid_width;
const unsigned long destination_x = destination % grid_width;
const unsigned long destination_y = destination / grid_width;
unsigned char i = source_y < destination_y;
map[0] = -(grid_width + 1);
map[1] = grid_width - 1;
map[2] = -1;
map[3] = 0;
map[4] = -(grid_width - 1);
map[5] = grid_width + 1;
map[8] = -grid_width;
map[9] = grid_width;
if (source_y == destination_y) {
i += 2;
}
if (source_x < destination_x) {
i += 4;
}
if (source_x == destination_x) {
i += 8;
}
return map[i];
}Although the aforementioned solution works, map isn't required and wastes 40–80 bytes of memory per instance.
In addition to the 4 required conditional statements, map uses 8 dereferences, 8 assignments and 7 arithmetic operations.
These 4 conditional statements can be optimized further by nesting conditional statements and using = operations instead of += operations.
The following code is a second attempt without mapping the traversal steps.
long navigate_bad(const unsigned long grid_width,
const unsigned long grid_height, const unsigned long source,
const unsigned long destination) {
const unsigned long source_x = source % grid_width;
const unsigned long source_y = source / grid_width;
const unsigned long destination_x = destination % grid_width;
const unsigned long destination_y = destination / grid_width;
long increment = 0;
if (source != destination) {
if (source_x == destination_x) {
if (source_y < destination_y) {
increment = grid_width;
} else {
increment = -grid_width;
}
} else {
if (source_y == destination_y) {
if (source_x < destination_x) {
increment = 1;
} else {
increment = -1;
}
} else {
if (source_x < destination_x) {
if (source_y < destination_y) {
increment = grid_width + 1;
} else {
increment = -grid_width + 1;
}
} else {
if (source_y < destination_y) {
increment = grid_width - 1;
} else {
increment = -grid_width - 1;
}
}
}
}
}
return increment;
}The aforementioned code is passable, but still not optimal.
The initialized value for step is overwritten in a majority of cases and the % operator uses too many CPU cycles.
Arithmetic division with the / operator is necessary to calculate both source_y and destination_y, but source_x and destination_x can use cheaper operations with arithmetic * and - instead of %.
The following code tests the processing time of % operations in a loop without compiler optimization enabled.
#include <stdio.h>
int main(void) {
unsigned short a = 11111;
unsigned long b = 0;
unsigned short c = 0;
while (b != 111111111) {
c += a % 111;
a += c;
b++;
}
return 0;
}Arithmetic * and - were more efficient by a few seconds when testing against both / and % with more than 100m iterations as shown in the following table.
Operators Expression Average Time
* - c += c - (a * 111); 0m6.105s
/ c += a / 111; 0m6.780s
% c += a % 111; 0m8.282sThe following code improves upon the previous attempts, but could still be further improved with code structure changes.
struct wsp_navigate_bad_s {
unsigned long grid_width;
unsigned long grid_height;
unsigned long source;
unsigned long destination;
long increment;
char increment_x;
char increment_y;
unsigned long source_x;
unsigned long source_y;
unsigned long destination_x;
unsigned long destination_y;
unsigned long repetitions_count;
unsigned char has_source_coordinates;
unsigned char has_destination_coordinates;
};
void wsp_navigate_bad(struct wsp_navigate_bad_s *s) {
if (s->repetitions_count == 0) {
if (s->has_source_coordinates == 0) {
s->source_y = s->source / s->grid_width;
s->source_x = s->source - (s->grid_width * s->source_y);
s->has_source_coordinates = 1;
}
if (s->has_destination_coordinates == 0) {
s->destination_y = s->destination / s->grid_width;
s->destination_x = s->destination - (s->grid_width * s->destination_y);
s->has_destination_coordinates = 1;
}
if (s->source_x == s->destination_x) {
s->increment_x = 0;
if (s->source_y < s->destination_y) {
s->increment = s->grid_width;
s->increment_y = 1;
s->repetitions_count = s->destination_y - s->source_y;
} else {
if (s->source_y > s->destination_y) {
s->increment = -s->grid_width;
s->increment_y = -1;
s->repetitions_count = s->source_y - s->destination_y;
} else {
s->increment = 0;
s->increment_y = 0;
s->repetitions_count = 0;
}
}
} else {
if (s->source_y == s->destination_y) {
s->increment_y = 0;
if (s->source_x < s->destination_x) {
s->increment = 1;
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
} else {
s->increment = -1;
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
}
} else {
if (s->source_x < s->destination_x) {
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
if (s->source_y < s->destination_y) {
s->increment = s->grid_width + 1;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
} else {
s->increment = -s->grid_width + 1;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
}
} else {
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
if (s->source_y < s->destination_y) {
s->increment = s->grid_width - 1;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
} else {
s->increment = -s->grid_width - 1;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
}
}
}
}
}
s->source += s->increment;
s->source_x += s->increment_x;
s->source_y += s->increment_y;
s->repetitions_count--;
}The following optimal code result is similar to the aforementioned code, but with segmented initialization functions, pre-calculated increments and goto statements instead of branching with else statements.
#include <stdio.h>
struct wsp_navigate_s {
unsigned long grid_width;
unsigned long grid_height;
unsigned long source;
unsigned long destination;
long increment;
long increment_bottom_left;
long increment_bottom_right;
long increment_top_center;
long increment_top_left;
long increment_top_right;
char increment_x;
char increment_y;
unsigned long source_x;
unsigned long source_y;
unsigned long destination_x;
unsigned long destination_y;
unsigned long repetitions_count;
};
void wsp_navigate_initialize_bounds(unsigned long width, unsigned long height,
struct wsp_navigate_s *s) {
s->grid_width = width;
s->grid_height = height;
s->increment_bottom_left = width - 1;
s->increment_bottom_right = width + 1;
s->increment_top_center = -width;
s->increment_top_left = -1 - width;
s->increment_top_right = 1 - width;
}
void wsp_navigate_initialize_points(unsigned long source,
unsigned long destination,
struct wsp_navigate_s *s) {
s->source = source;
s->destination = destination;
s->increment = 0;
s->source_y = source / s->grid_width;
s->source_x = source - (s->grid_width * s->source_y);
s->destination_y = destination / s->grid_width;
s->destination_x = destination - (s->grid_width * s->destination_y);
s->repetitions_count = 0;
}
void wsp_navigate_increment(struct wsp_navigate_s *s) {
if (s->repetitions_count == 0) {
if (s->source_x == s->destination_x) {
s->increment_x = 0;
if (s->source_y != s->destination_y) {
if (s->source_y < s->destination_y) {
s->increment = s->grid_width;
s->increment_y = 1;
s->repetitions_count = s->destination_y - s->source_y;
goto apply_increment;
}
s->increment = s->increment_top_center;
s->increment_y = -1;
s->repetitions_count = s->source_y - s->destination_y;
goto apply_increment;
}
s->increment = 0;
s->increment_y = 0;
goto apply_increment;
}
if (s->source_y == s->destination_y) {
s->increment_y = 0;
if (s->source_x < s->destination_x) {
s->increment = 1;
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
goto apply_increment;
}
s->increment = -1;
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
goto apply_increment;
}
if (s->source_x < s->destination_x) {
s->increment_x = 1;
s->repetitions_count = s->destination_x - s->source_x;
if (s->source_y < s->destination_y) {
s->increment = s->increment_bottom_right;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
goto apply_increment;
}
s->increment = s->increment_top_right;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
goto apply_increment;
}
s->increment_x = -1;
s->repetitions_count = s->source_x - s->destination_x;
if (s->source_y < s->destination_y) {
s->increment = s->increment_bottom_left;
s->increment_y = 1;
if ((s->destination_y - s->source_y) < s->repetitions_count) {
s->repetitions_count = s->destination_y - s->source_y;
}
goto apply_increment;
}
s->increment = s->increment_top_left;
s->increment_y = -1;
if ((s->source_y - s->destination_y) < s->repetitions_count) {
s->repetitions_count = s->source_y - s->destination_y;
}
}
apply_increment:
s->source += s->increment;
s->source_x += s->increment_x;
s->source_y += s->increment_y;
s->repetitions_count--;
}
int main(void) {
struct wsp_navigate_s s;
unsigned char grid[100] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
unsigned char i = 0;
wsp_navigate_initialize_bounds(10, 10, &s);
wsp_navigate_initialize_points(0, 95, &s);
grid[s.source] = 1;
while (s.source != s.destination) {
wsp_navigate_increment(&s);
grid[s.source] = 3;
}
grid[s.destination] = 2;
i = 0;
while (i != 100) {
printf("%u ", grid[i]);
if (((i + 1) % 10) == 0) {
printf("\n");
}
i++;
}
return 0;
}In addition to saving CPU time and memory, the required conditional statements for calculating navigation are nested properly to minimize conditional checks for each navigational direction, as shown in the following table with optimized repetition calculations excluded.
Instance Direction Conditionals Direction Conditional
Before Optimization After Optimization
0 1 3
1 3 2
2 3 3
3 4 3
4 4 3
5 5 4
6 5 4
7 5 4
8 5 4
__________ __________
35 30The minority case when the source is the same as the destination requires more conditional checks to decrease the overall count from 35 to 30 and the average count from 3.89 to 3.33.
Furthermore, wsp_navigate_increment() is iterative, meaning it traverses 1 adjacent grid point at a time in 1 of 8 directions without repeated calculations in the same direction.
To navigate to a new destination after reaching the current destination, wsp_navigate_initialize_points() must be used to initialize new source and destination points with new s.destination_x, s.destination_y, s.source_x and s.source_y coordinates.
Whenever the grid size changes, wsp_navigate_initialize_bounds() must be used before initializing the source and destination points with wsp_navigate_initialize_points().
4-directional navigation is omitted because it's a trivial adjustment from the 8-directional result and it's dependent on the position of obstacles.
For example, the 8-directional calculation from position 21 to 32 in the following grid with obstacles marked as 4 doesn't need to consider adjacent obstacles, but the 4-directional adjustment considers a possible obstacle at either position 22 or 31.
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
0 4 3 0 0 0 0 0 0 0
0 0 4 3 0 0 0 0 0 0
0 0 4 3 0 0 0 0 0 0
0 0 4 3 0 0 0 0 0 0
0 0 0 2 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0In this case, a 4-directional implementation should adjust the calculation step from 11 to 1 instead of 10 based on the obstacle at position 31.
