├── ship.png ├── ship_burn.png ├── make_gif.sh ├── .gitignore ├── render.py ├── demo.py ├── obstacle.py ├── pathing.py ├── pathing_tests.py ├── interpolate.py ├── README.md ├── draw.py └── newt.py /ship.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matthew-piziak/spacepath/HEAD/ship.png -------------------------------------------------------------------------------- /ship_burn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matthew-piziak/spacepath/HEAD/ship_burn.png -------------------------------------------------------------------------------- /make_gif.sh: -------------------------------------------------------------------------------- 1 | convert -delay 2 -loop 0 *screen.png $1 2 | convert $1 -resize %50 $1 3 | gnome-open $1 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *screen.jpg 3 | *screen.png 4 | __pycache__ 5 | .#* 6 | *.gif 7 | *.pyc 8 | *.profile 9 | *.org 10 | *.org_archive 11 | -------------------------------------------------------------------------------- /render.py: -------------------------------------------------------------------------------- 1 | """Rendering functions""" 2 | 3 | import os 4 | import time 5 | import subprocess 6 | 7 | import pygame 8 | 9 | def save_image(window, i): 10 | """save screenshot to disk""" 11 | pygame.image.save(window, str(i).zfill(4) + "screen.png") 12 | 13 | def make_gif(): 14 | """generate a gif from the accumulated screenshots""" 15 | label = str(int(time.time())) 16 | print("label: " + label) 17 | gif_command = "bash make_gif.sh maneuver" + label + ".gif" 18 | gif_process = subprocess.Popen(gif_command.split(), stdout=subprocess.PIPE) 19 | gif_process.wait() 20 | _clear_images() 21 | 22 | def _clear_images(): 23 | """delete all PNGs from program directory""" 24 | filelist = [f for f in os.listdir(".") if f.endswith("screen.png")] 25 | for filename in filelist: 26 | os.remove(filename) 27 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | """Game runner""" 2 | 3 | import time 4 | 5 | import pygame 6 | 7 | import newt 8 | import pathing 9 | import draw 10 | import obstacle 11 | 12 | START = newt.Node(0, 0, 0, 0, 1) 13 | GOAL = newt.Node(100, 100, 0, 0, 0) 14 | 15 | def get_path(obstacles, bounds): 16 | """return nodes in path from A*""" 17 | heuristic = lambda n, g: newt.heuristic(n, g, obstacles, bounds) 18 | return pathing.a_star(START, GOAL, newt.adjacent, heuristic, newt.success) 19 | 20 | def main(): 21 | """generate path and render""" 22 | obstacles = obstacle.get_obstacles(START.x, START.y, GOAL.x, GOAL.y) 23 | bounds = (GOAL.x, GOAL.y) 24 | window = draw.init(obstacles, GOAL) 25 | pygame.display.flip() 26 | begin = time.time() 27 | path = get_path(obstacles, bounds) 28 | end = time.time() 29 | print("time: " + str(end - begin)) 30 | draw.animate_path(window, obstacles, GOAL, path) 31 | 32 | if __name__ == "__main__": 33 | while True: 34 | main() 35 | -------------------------------------------------------------------------------- /obstacle.py: -------------------------------------------------------------------------------- 1 | """Obstacle generator""" 2 | 3 | import random 4 | import collections 5 | 6 | Obstacle = collections.namedtuple('Obstacle', ['x', 'y', 'radius']) 7 | 8 | def get_obstacles(min_x, min_y, max_x, max_y): 9 | """return an allotment of obstacles""" 10 | num_obstacles = 6 11 | generate = lambda: _generate_random_obstacle(min_x, min_y, max_x, max_y) 12 | return [generate() for _ in range(0, random.randint(1, num_obstacles))] 13 | 14 | def contains_node(x, y, obstacle): 15 | """returns whether the node is contained in the obstacle""" 16 | dx = abs(x - obstacle.x) 17 | dy = abs(y - obstacle.y) 18 | if dx > obstacle.radius: 19 | return False 20 | if dy > obstacle.radius: 21 | return False 22 | if dx + dy <= obstacle.radius: 23 | return True 24 | return (dx ** 2) + (dy ** 2) <= obstacle.radius ** 2 25 | 26 | def _generate_random_obstacle(min_x, min_y, max_x, max_y): 27 | """generate random obstacle""" 28 | maximum_obstacle_radius = 18 29 | minimum_obstacle_radius = 12 30 | radius = random.randint(minimum_obstacle_radius, maximum_obstacle_radius) 31 | x = random.randint(min_x + radius, max_x - radius) 32 | y = random.randint(min_y + radius, max_y - radius) 33 | return Obstacle(x, y, radius) 34 | -------------------------------------------------------------------------------- /pathing.py: -------------------------------------------------------------------------------- 1 | """Generalized pathing functions.""" 2 | 3 | import heapq 4 | 5 | def a_star(start, goal, adjacent, heuristic, success): 6 | """A* pathing algorithm""" 7 | open_heap = [] 8 | seen = set() 9 | came_from = {} 10 | g_score = {} 11 | f_score = {} 12 | g_score[start] = 0 13 | f_score[start] = g_score[start] + heuristic(start, goal) 14 | open_heap.append((f_score[start], start)) 15 | seen.add(start) 16 | while True: 17 | node = heapq.heappop(open_heap)[1] 18 | if success(node, goal): 19 | print("heap: " + str(len(open_heap))) 20 | return _reconstruct_path(came_from, node) 21 | for adj, action in adjacent(node): 22 | if adj in seen: 23 | continue 24 | seen.add(adj) 25 | came_from[adj] = (node, action) 26 | # adjacency is based on a constant time step 27 | g_score[adj] = g_score[node] + 1 28 | h_score = heuristic(adj, goal) 29 | f_score[adj] = g_score[adj] + h_score 30 | heapq.heappush(open_heap, (f_score[adj], adj)) 31 | 32 | def _reconstruct_path(came_from, node): 33 | """trace back the path built by A*""" 34 | if node in came_from: 35 | path = _reconstruct_path(came_from, came_from[node][0]) 36 | return path + [(node, came_from[node][1])] 37 | else: 38 | return [node] 39 | -------------------------------------------------------------------------------- /pathing_tests.py: -------------------------------------------------------------------------------- 1 | """Unit tests for pathing module""" 2 | 3 | import pathing 4 | import unittest 5 | import math 6 | import time 7 | 8 | class TestPathing(unittest.TestCase): 9 | """Test suite for pathing module""" 10 | # pylint: disable=R0904 11 | 12 | def setUp(self): 13 | self.start = (0, 0) 14 | self.goal = (4, 4) 15 | def manhattan(node): 16 | """returns a list of adjacent nodes in mahattan distance""" 17 | adjacency_list = [] 18 | adjacency_list.append(((node[0] + 1, node[1]), None)) 19 | adjacency_list.append(((node[0] - 1, node[1]), None)) 20 | adjacency_list.append(((node[0], node[1] + 1), None)) 21 | adjacency_list.append(((node[0], node[1] - 1), None)) 22 | return adjacency_list 23 | self.adjacent = manhattan 24 | def euclidean_distance(node, goal): 25 | """returns crows-flight distance between node and goal""" 26 | return math.sqrt((goal[0]-node[0])**2 + (goal[1]-node[1])**2) 27 | self.heuristic = euclidean_distance 28 | self.success = lambda node, goal: node == goal 29 | 30 | def test_a_star(self): 31 | """test that path is optimal and obeys heuristic""" 32 | start = time.time() 33 | path = pathing.a_star(self.start, 34 | self.goal, 35 | self.adjacent, 36 | self.heuristic, 37 | self.success) 38 | stop = time.time() 39 | elapsed_ms = (stop - start) * 1000 40 | self.assertEqual(len(path), 9) 41 | self.assertIn(((2, 2), None), path) 42 | print("Path found. Time elapsed: %f milliseconds.", elapsed_ms) 43 | 44 | if __name__ == '__main__': 45 | unittest.main() 46 | -------------------------------------------------------------------------------- /interpolate.py: -------------------------------------------------------------------------------- 1 | """Draw interpolation""" 2 | 3 | from collections import namedtuple 4 | from scipy import interpolate 5 | import numpy 6 | 7 | INTERPOLATE = True 8 | INTERPOLATION_FACTOR = 12 9 | 10 | InterpolationNode = namedtuple('InterpolationNode', ['x', 'y', 'angle']) 11 | 12 | def interpolate_path(path): 13 | """Generate a higher resolution path using cubic spline interpolation.""" 14 | def get_interpolation_node(node): 15 | """Simplify the node so that interpolation is easier.""" 16 | return InterpolationNode(node[0].x, node[0].y, node[0].angle) 17 | interpolaton_nodes = [get_interpolation_node(n) for n in path] 18 | if not INTERPOLATE: 19 | return interpolaton_nodes 20 | t = numpy.arange(0, len(interpolaton_nodes)) 21 | f_x = interpolate.interp1d(t, [p.x for p in interpolaton_nodes], 'cubic') 22 | f_y = interpolate.interp1d(t, [p.y for p in interpolaton_nodes], 'cubic') 23 | t_new = numpy.arange(0, 24 | len(interpolaton_nodes) - 2, 25 | 1.0 / INTERPOLATION_FACTOR) 26 | interpolated_x = f_x(t_new) 27 | interpolated_y = f_y(t_new) 28 | interpolated_angle = _angles([p.angle for p in interpolaton_nodes]) 29 | actionless_path = zip(interpolated_x, interpolated_y, interpolated_angle) 30 | def get_actions(path): 31 | """Get actions for each node in the interpolated path. 32 | 33 | Each interpolated node is assigned the same action as the 34 | previous real node. 35 | 36 | """ 37 | actions = [] 38 | for node in path: 39 | actions += [node[1] for _ in range(INTERPOLATION_FACTOR)] 40 | return actions 41 | interpolated_path = zip(actionless_path, get_actions(path)) 42 | return interpolated_path 43 | 44 | def _angles(angles): 45 | """Perform linear modulus interpolation for angles.""" 46 | interpolated_angles = [] 47 | fraction = 1.0 / INTERPOLATION_FACTOR 48 | for i in range(len(angles) - 1): 49 | start_angle = float(angles[i]) 50 | end_angle = float(angles[i + 1]) 51 | if abs(end_angle - start_angle) <= 4: 52 | for j in range(INTERPOLATION_FACTOR): 53 | start_factor = (INTERPOLATION_FACTOR - j) * start_angle 54 | end_factor = j * end_angle 55 | interpolated_angle = (start_factor + end_factor) * fraction 56 | interpolated_angles.append(interpolated_angle) 57 | else: 58 | for j in range(INTERPOLATION_FACTOR): 59 | if start_angle < 4: 60 | start_angle += 8 61 | if end_angle < 4: 62 | end_angle += 8 63 | start_factor = (INTERPOLATION_FACTOR - j) * start_angle 64 | end_factor = j * end_angle 65 | interpolated_angle = start_factor + end_factor 66 | interpolated_angle = (interpolated_angle * fraction) % 8 67 | interpolated_angles.append(interpolated_angle) 68 | return interpolated_angles 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Spacepath 2 | 3 | Spacepath is a demonstration of A\* pathfinding applied to Newtonian 4 | physics. 5 | 6 | ![demo](https://i.imgur.com/8ClC7Tr.gif) ![demo2](https://i.imgur.com/DCVFNwP.gif) 7 | 8 | To run the demo, invoke `python demo.py` 9 | 10 | The demo spawns a spaceship in the upper left, a goal region in the 11 | bottom right, and a random number of gray obstacles. 12 | 13 | The spaceship paths to the goal region using a time-optimal path, with 14 | two constraints: 15 | - the ship must come to a complete stop in the goal region 16 | - the ship must avoid any obstacles en route 17 | 18 | Note that the ship obeys conservation of momentum and takes this fact 19 | into account while calculating the optimal path. 20 | 21 | The pathfinding algorithm used is standard A\*. All domain information 22 | about Newtonian physics is entirely contained in the `newt` heuristic 23 | function. This demonstrates that A\* can efficiently perform Newtonian 24 | pathfinding. For a representative simulation the `newt` heuristic 25 | searches only 0.1% of the space that would be explored by 26 | breadth-first search. 27 | 28 | 31 prerendered demos can be seen [here](http://imgur.com/a/K8XfM). 29 | 30 | ## Implementation Details 31 | 32 | ### The Graph Model 33 | 34 | Each search node is five dimensional: 35 | - location X 36 | - location Y 37 | - velocity X 38 | - velocity Y 39 | - angle 40 | 41 | At each search step, the velocity is applied to the location. The ship also makes two choices: 42 | - burn or cruise 43 | - fly straight or turn left or turn right 44 | 45 | This corresponds to a branching factor of 6. Angular resolution is not 46 | currently conserved, but this is a planned future improvement. 47 | 48 | #### Angular resolution 49 | 50 | The ship sometimes wobbles as it navigates. This is because the 51 | angular resolution is currently set to 8, which corresponds to turning 52 | angles of 45 degrees. Increasing the angular resolution is a planned 53 | future improvement. 54 | 55 | #### Turning bias 56 | 57 | When two paths are equivalent, the ship breaks ties deterministically: 58 | - cruising > burning 59 | - straight > clockwise > counterclockwise 60 | 61 | #### Fast sine and cosine 62 | 63 | Fast sine and cosine approximations are applied to make all locations 64 | and velocities integral. This avoids floating-point errors in node 65 | equality checking. 66 | 67 | ### Bounded relaxation 68 | 69 | Bounded relaxation is a pathfinding technique where the heuristic 70 | sacrifices some optimality in favor of letting the pathfinder search 71 | deeper along the best-guess path. Spacepath uses a bounded relaxation 72 | factor of 2%, which means that generated paths are only guaranteed to 73 | be 98% optimal. 74 | 75 | ## Thanks 76 | 77 | ### Physics 78 | 79 | Thanks to Physics StackExchange users [Kurtovic](http://physics.stackexchange.com/users/44104/kurtovic) and [nivag](http://physics.stackexchange.com/users/44576/nivag) for helping 80 | me determine the kinematic equations necessary to solve for the heuristic. 81 | 82 | [Original Question](http://physics.stackexchange.com/questions/112687/how-long-does-it-take-to-optimally-change-position-and-velocity) 83 | 84 | ### Math 85 | 86 | Thanks to Mathematics StackExchange users [ah-huh-moment](http://math.stackexchange.com/users/101504/ah-huh-moment) and 87 | [hypergeometric](http://math.stackexchange.com/users/168053/hypergeometric) for helping me solve the resulting algebraic system. 88 | 89 | [Original Question](http://math.stackexchange.com/questions/1021921/solve-system-of-kinematics-equation) 90 | 91 | ### Art 92 | 93 | Thank you zxBranden from OpenGameArt for the spaceship sprite. 94 | 95 | [Original source](http://opengameart.org/users/zxbranden) 96 | -------------------------------------------------------------------------------- /draw.py: -------------------------------------------------------------------------------- 1 | """Game rendering functions""" 2 | 3 | import pygame 4 | import math 5 | import interpolate 6 | import time 7 | 8 | # drawing constants 9 | NODE_RADIUS = 3 10 | DRAW_SCALE = 6 11 | NODE_COLOR = (255, 255, 255) 12 | OBSTACLE_COLOR = (100, 100, 100) 13 | GOAL_COLOR = (100, 100, 255) 14 | BACKGROUND_COLOR = (0, 0, 0) 15 | ANGLE_LENGTH = 1.5 * NODE_RADIUS 16 | 17 | # animation 18 | FRAME_STEP = 0.01 19 | 20 | class Ship(pygame.sprite.Sprite): 21 | """Represents the ship""" 22 | # pylint: disable=R0903 23 | # This is a necessary extension of the sprite class, so the 'Too 24 | # few public methods' pylint warning can be ignored. 25 | 26 | def __init__(self, x, y, angle, action): 27 | scale = NODE_RADIUS * DRAW_SCALE * 2 28 | images = {"cruise": "ship.png", "burn": "ship_burn.png"} 29 | self.image = pygame.image.load(images[action]) 30 | self.image = pygame.transform.smoothscale(self.image, 31 | (scale, scale)) 32 | def rot_center(angle): 33 | """rotate an image while keeping its center and size""" 34 | orig_rect = self.image.get_rect() 35 | rot_image = pygame.transform.rotate(self.image, angle) 36 | rot_rect = orig_rect.copy() 37 | rot_rect.center = rot_image.get_rect().center 38 | rot_image = rot_image.subsurface(rot_rect).copy() 39 | self.image = rot_image 40 | rot_center(-90) 41 | rot_center(-1 * angle * 45) 42 | self.rect = self.image.get_rect() 43 | self.x = x * DRAW_SCALE 44 | self.y = y * DRAW_SCALE 45 | 46 | def draw(self, surface): 47 | """Draw ship sprite to surface.""" 48 | surface.blit(self.image, (self.x, self.y)) 49 | 50 | def init(obstacles, goal): 51 | """draw initial scene and obstacles""" 52 | window_dimensions = ((goal.x + 10) * DRAW_SCALE, (goal.y + 10) * DRAW_SCALE) 53 | window = pygame.display.set_mode(window_dimensions) 54 | _scene(window, obstacles) 55 | _goal(window, goal) 56 | return window 57 | 58 | def animate_path(window, obstacles, goal, path): 59 | """animate path""" 60 | path[0] = (path[0], "cruise") 61 | interpolated_path = interpolate.interpolate_path(path) 62 | for node, action in interpolated_path: 63 | _scene(window, obstacles) 64 | _goal(window, goal) 65 | ship = Ship(node[0], node[1], node[2], action) 66 | ship.draw(window) 67 | pygame.display.flip() 68 | time.sleep(0.01) 69 | 70 | def _obstacle(window, obstacle): 71 | """draw obstacle""" 72 | location = (obstacle.x * DRAW_SCALE, obstacle.y * DRAW_SCALE) 73 | node_overlap = int(NODE_RADIUS * DRAW_SCALE * 2) 74 | radius = (obstacle.radius * DRAW_SCALE) - node_overlap 75 | pygame.draw.circle(window, OBSTACLE_COLOR, location, radius) 76 | 77 | def _goal(window, goal): 78 | """draw goal""" 79 | position = (goal.x * DRAW_SCALE, goal.y * DRAW_SCALE) 80 | goal_draw_radius = NODE_RADIUS * DRAW_SCALE * 4 81 | pygame.draw.circle(window, GOAL_COLOR, position, goal_draw_radius) 82 | 83 | def _node(window, x, y, angle, action): 84 | """draw node""" 85 | node_x = int(x * DRAW_SCALE) 86 | node_y = int(y * DRAW_SCALE) 87 | node_draw_radius = NODE_RADIUS * DRAW_SCALE 88 | pygame.draw.circle(window, NODE_COLOR, (node_x, node_y), node_draw_radius) 89 | node_angle = (math.pi / 4) * float(angle) 90 | origin = (node_x, node_y) 91 | angle_length_multiplier = DRAW_SCALE * ANGLE_LENGTH 92 | angle_x = (-1 * math.cos(node_angle) * angle_length_multiplier) + node_x 93 | angle_y = (-1 * math.sin(node_angle) * angle_length_multiplier) + node_y 94 | angle_tip = (angle_x, angle_y) 95 | angle_color = NODE_COLOR 96 | if action == "burn": 97 | angle_color = GOAL_COLOR 98 | pygame.draw.line(window, angle_color, origin, angle_tip, 10) 99 | 100 | def _scene(window, obstacles): 101 | """draw scene""" 102 | window.fill(BACKGROUND_COLOR) 103 | for obstacle in obstacles: 104 | _obstacle(window, obstacle) 105 | -------------------------------------------------------------------------------- /newt.py: -------------------------------------------------------------------------------- 1 | """Newtonian physics specificiations""" 2 | 3 | import math 4 | import obstacle 5 | from collections import namedtuple 6 | 7 | Node = namedtuple('Node', ['x', 'y', 'v_x', 'v_y', 'angle']) 8 | 9 | def adj_position(node): 10 | """determines position for the next time step""" 11 | return [(node.x + node.v_x, node.y + node.v_y)] 12 | 13 | def adj_velocities(node): 14 | """assumes acceleration = 2 to use fast sine and cosine""" 15 | angle = node.angle 16 | sin = {0: 0, 1: 1, 2: 1, 3: 1, 4: 0, 5: -1, 6: -1, 7: -1} 17 | cos = {0: 1, 1: 1, 2: 0, 3: -1, 4: -1, 5: -1, 6: 0, 7: 1} 18 | delta_v_x = cos[angle] 19 | delta_v_y = sin[angle] 20 | cruise = (node.v_x, node.v_y) 21 | burn = (node.v_x + delta_v_x, node.v_y + delta_v_y) 22 | velocities = [burn, cruise] 23 | return velocities 24 | 25 | def adj_angles(node): 26 | """determines adjacent angles based on turning choice""" 27 | adj_angles_unnormalized = [node.angle - 1, 28 | node.angle + 1, 29 | node.angle] 30 | adj_angles_normalized = [a % 8 for a in adj_angles_unnormalized] 31 | return adj_angles_normalized 32 | 33 | def adjacent(node): 34 | """finds possible adjacent nodes based on burn and turn choices""" 35 | adj_nodes = [] 36 | for pos in adj_position(node): 37 | for vel in adj_velocities(node): 38 | for angle in adj_angles(node): 39 | adj_node = Node(pos[0], pos[1], vel[0], vel[1], angle) 40 | if not vel == (node.v_x, node.v_y): 41 | adj_nodes.append((adj_node, "burn")) 42 | else: 43 | adj_nodes.append((adj_node, "cruise")) 44 | return adj_nodes 45 | 46 | def heuristic(node, goal, obstacles, bounds): 47 | """newtonian physics heuristic for A*""" 48 | acceleration = 2 # hardcoded for sine and cosine optimization 49 | def outside_arena(): 50 | """determines if the node is outside permissible bounds""" 51 | return not (0 < node.x < bounds[0] and 0 < node.y < bounds[1]) 52 | def leaving_arena(): 53 | """determines if the node is moving too fast to remain in the arena""" 54 | braking_time_x = abs(node.v_x) // acceleration 55 | braking_time_y = abs(node.v_y) // acceleration 56 | v_component_x = abs(node.v_x) * braking_time_x 57 | v_component_y = abs(node.v_y) * braking_time_y 58 | a_component_x = 0.5 * acceleration * braking_time_x 59 | a_component_y = 0.5 * acceleration * braking_time_y 60 | braking_distance_x = v_component_x + a_component_x 61 | braking_distance_y = v_component_y + a_component_y 62 | if node.v_x > 0: 63 | if braking_distance_x > bounds[0] - node.x: 64 | return True 65 | if node.v_y > 0: 66 | if braking_distance_y > bounds[0] - node.y: 67 | return True 68 | if node.v_x < -1: 69 | if braking_distance_x > node.x: 70 | return True 71 | if node.v_y < -1: 72 | if braking_distance_y > node.y: 73 | return True 74 | def in_obstacle(): 75 | """determines if the node is inside an obstacle""" 76 | x = node.x 77 | y = node.y 78 | return any([obstacle.contains_node(x, y, o) for o in obstacles]) 79 | h_max = 1000000 80 | if outside_arena() or leaving_arena() or in_obstacle(): 81 | return h_max 82 | heuristic_x = (- node.v_x + 83 | math.sqrt((2 * (node.v_x ** 2)) + 84 | (4 * acceleration * abs(goal.x - node.x)))) / 2 85 | heuristic_y = (- node.v_y + 86 | math.sqrt((2 * (node.v_y ** 2)) + 87 | (4 * acceleration * abs(goal.y - node.y)))) / 2 88 | return 1.02 * (heuristic_x + heuristic_y) 89 | 90 | def success(node, goal): 91 | """success function for A*""" 92 | success_radius = 6 93 | success_region = obstacle.Obstacle(goal.x, goal.y, success_radius) 94 | location = obstacle.contains_node(node.x, node.y, success_region) 95 | speed = abs(node.v_x - goal.v_x) + abs(node.v_y - goal.v_y) == 0 96 | return location and speed 97 | --------------------------------------------------------------------------------