obstacle_finder.py

############################################################################
# Christian Paul Dehli
#
# This .py file demonstrates functionality that can be used to determine 
# how much of a scene is visible to a robot (where certain obstacles block
# its line of sight). The functionality was needed for a game, so pinpoint
# accuracy was not required.
#
# The algorithm works by shooting rays out of the robot into the scene.
# Whenever a ray strikes an obstacle, the ray is deleted and the robot's 
# vision is known to be blocked going further along that ray.
#
# This simulation has a grid, with the robot located in the center. Random
# obstacles are placed in the scene. Every 1 second, the output is updated
# to show what the robot sees going one direction forward. A '*' indicates
# that the robot should know what is at the location. A '.' is an empty tile,
# a 'R' represents the robot, and 'o' represents obstacles that the robot
# cannot see over.
#
# Can be executed by calling: `python obstacle_finder.py`
#
############################################################################

from random import randint
import time
import os
import math
from sys import exit
clear = lambda: os.system('cls')

# This class is used to keep track of the direction of the robot's rays.
class Slope:
    rise = 0
    run = 0

    def __init__(self, rise, run):
        self.rise = rise
        self.run = run

    # normalizes rise and run such that the length == 1
    def normalize(self):
        if self.run  == 0:
            self.rise = self.rise / math.fabs(self.rise)
        else:
            length = (self.rise * self.rise + self.run * self.run) ** 0.5
            self.rise = self.rise / length
            self.run = self.run / length

    def __repr__(self):
        return "Rise: " + str(self.rise) + ", Run: " + str(self.run)

# This method goes around the perimeter of the robot's vision in order to calculate all of the angles needed to 
# hit every square the robot sees. This diagram shows the result for a robot with a view_radius of 3
#
#          *******
#          *.....*
#          *.....* 
#          *..R..*
#          *.....*
#          *.....*
#          *******
#
# An array is returned that contains a slope that goes from 'R' to each of the '*' (for this example there would be a total
# of 24 slopes). There's a total of 8 * view_radius angles returned.
#
# Ideally, this will only be calculated once and then stored in memory for the lifetime of the simulation.
def getRayAngles(view_radius):
    angles = []

    # push the 4 opposite sides
    angles.append(Slope(1, 0))  # top
    angles.append(Slope(-1, 0)) # bottom
    angles.append(Slope(0, 1))  # right
    angles.append(Slope(0, -1)) # left

    def appendAngles(deltaX, deltaY):   
        slope = Slope(deltaY, deltaX)
        slope.normalize()

        # append the angle for all 4 quadrants
        angles.append(Slope(slope.rise, slope.run))
        angles.append(Slope(slope.rise, -slope.run))
        angles.append(Slope(-slope.rise, slope.run))
        angles.append(Slope(-slope.rise, -slope.run))

    # push top (or bottom) side of the corner
    for x in range(1, view_radius + 1):
        appendAngles(x, view_radius)

    # push left (or right) side of the corner
    for y in range(1, view_radius):
        appendAngles(view_radius, y)

    return angles

# This class is used to keep track of the state of the map (and what the robot sees)
class Grid:
    data = None

    def __init__(self, gridWidth, gridHeight, robotX, robotY, obstacleCount):
        # Handle invalid arguments
        if gridWidth < 1 or gridHeight < 1:
            print "Grid created with invalid dimensions."
            exit()
        if robotX < 0 or robotX >= gridWidth or robotY < 0 or robotY >= gridHeight:
            print "Grid created with invalid robot location."
            exit()
        if obstacleCount >= gridWidth * gridHeight:
            print "Grid created with too many obstacles."
            exit()

        self.data = [["." for _ in range(gridWidth)] for _ in range(gridHeight)]

        # set robot location
        self.data[robotY][robotX] = "R"

        # add obstacles
        while obstacleCount > 0:
            rand_x = randint(0, gridWidth - 1)
            rand_y = randint(0, gridHeight - 1)
            if self.data[rand_y][rand_x] == ".":
                self.data[rand_y][rand_x] = "o"
                obstacleCount -= 1

    def hasObstacle(self, x, y):
        return self.data[y][x] == "o"

    def setVisible(self, x, y):
        self.data[y][x] = "*"

    # used to display the grid in the command line
    def __repr__(self):
        result = ""
        for row in self.data:
            result += "".join(row) + "\n"
        return result

# Main method for file
def main():
    # the simulation environment size
    GRID_WIDTH = 51
    GRID_HEIGHT = 51

    # set robot to be at the center of grid
    ROBOT_X = GRID_WIDTH // 2
    ROBOT_Y = GRID_HEIGHT // 2

    # number of obstacles to add
    NUM_OBSTACLES = 50

    grid = Grid(GRID_WIDTH, GRID_HEIGHT, ROBOT_X, ROBOT_Y, NUM_OBSTACLES)

    # how far the robot can see
    ROBOT_RADIUS = 20
    angles = getRayAngles(ROBOT_RADIUS)

    # robot_vision is how far the robot is currently seeing (ROBOT_RADIUS is the max distance it can see)
    for robot_vision in range(ROBOT_RADIUS):
        time.sleep(1)
        clear()

        # go through all of the rays and go one length of grid square each time step
        i = 0
        while i < len(angles):
            angle = angles[i]

            # find the next point for the robot if it goes the direction of the angle by the dimensions of the box
            if angle.run == 0:
                rise = (robot_vision + 1) * angle.rise
                new_x = ROBOT_X
                new_y = ROBOT_Y + rise

            # not undefined slope
            else:
                new_x = round(ROBOT_X + angle.run * (robot_vision + 1), 0)
                new_y = round(ROBOT_Y + angle.rise * (robot_vision + 1), 0)

            # show the vision update by changing the grid to *
            intX = int(new_x)
            intY = int(new_y)

            # If the ray goes out of bounds or hits an obstacle, delete it
            if intX >= GRID_WIDTH or intX < 0 or intY >= GRID_HEIGHT or intY < 0 or grid.hasObstacle(intX, intY):
                del angles[i]
                # decrement the counter so that the next element will be the same as if you didn't delete angles[i]
                i -= 1
            else:
                grid.setVisible(intX, intY)

            i += 1

        print grid

if __name__ == '__main__':
    main()