Ross Hatton

# ``Inside" and ``Outside" vectors

Each line segment in my functions is inherently oriented, in that it is defined by an ordered pair of points. I consider the line segment as a vector departing from the first point and arriving at the second.

The boundaries on all my objects (robot and obstacles) are positively oriented (when moving around the object in the direction of its component line segments, the interior of the object is always on the left). I use this property extensively to determine whether a given line segment is inside or outside of any given object.

At the intersection of two line segments, one line segment will have come from ``inside" the other if its vector has a positive dot product with the outward pointing normal vector of the other segment. This rule can be expanded to handle special cases, such as segments which meet at endpoints, though the rules are more complex.

# Obstacle expansion

I expand convex obstacles by placing the obstacle at the identity (such that it's world geometry is equal to its local, or body-coordinate, geometry) and then placing each each vertex of the robot at each vertex of the obstacle. A placement is deemed to be ``valid" if it results in the robot being entirely outside the obstacle. The (local coordinates) vertices of the expanded obstacle boundary, referred to as its ``sense" are the positions of the robot's reference point at the valid placements.

The primary test for valid placements is to intersect the robot's line segments with those of the obstacle. If there are any intersections not at the collocated vertices, then the robot is definitely overlapping the obstacle, and the placement being tested is not valid. If there are no intersections, then the robot is either entirely inside or entirely outside of the obstacle. To check this, I find whether the line segment on the robot which ends at the vertex is inside or outside the obstacle. If it is outside, then the robot is outside, and the placement is valid.

# Obstacle merging

While non-convex obstacles can be effectively created by placing convex obstacles near each other, I added an obstacle-merge function, which joins concave, expanded obstacles into single objects. For the boundary and sense vertex sets, the merge algorithm follows the boundary of the first obstacle's vertex set, adding each vertex to the set for the merged obstacle, until it comes to an intersection. If the intersection was approached from outside the second obstacle (and thus was part of the true outer boundary), then the algorithm determines which line segment leaving the intersection is outermost, and continues following boundary, switching primary obstacles if necessary.

I have included support for disjoint merged objects, which is useful for merging objects which have overlapping sense fields, but whose base structures do not intersect, and for building complex objects from multiple objects without having to ensure a fixed order for adding objects.

If the algorithm determines that the intersection was approached from the inside, then it starts over from that intersection.

The merge algorithm terminates when it reaches its most recent starting point.

The special cases for this algorithm were tricky to define, and partially responsible for the excessive amounts of time I put into getting this homework finished.

# Visibility graph generation and search

The visibility graph constructor uses the naive algorithm. Two vertices of expanded obstacles are visible to each other if the line between them intersects obstacles only at those vertices and is outside the expanded obstacles at both ends.

The graph search uses a fairly standard search. The only feature of the search algorithm that may be of special interest is that I allow for multi-dimensional node indexing, such that the nodes can be referred to with (obstacle, subobstacle, vertex) triples, and the visibility graph can be constructed without precalculating the number of nodes that it will contain.

# Matlab files

The matlab files I have included are

cyclic.m
1-indexed mod function
detect_collisions.m
find intersections between the boundaries of two objects. For this exercise, I also included finding collisions between the expanded boundaries of two objects, for the merge function.
distance.m
Find the Euclidean distance between two points.
draw_object.m
Draw an object.
draw_set.m
Draw all objects in an array.
expand_obstacle.
Given an obstacle and a robot, expand the obstacle.
expand.m
Node expansion for search.
line_eq.m
Get an algebraic expression for each line segment of the object.
max2d.m
Get the maximum value and subscripts of that value for a two dimensional matrix.
meetpoint.m
Find the intersection, if any, between two sets of line segments.
motion_planning_hw5_failure.m
Main program for the failure case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
motion_planning_hw5_success.m
Main program for the success case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
obstacle_merge.m
Merge the boundaries and sense boundaries of two obstacles.
outboard_test.m
Test whether a vector is inside or outside a pair of vectors.
pop_n_best.m
Get `n_best` for the search.
update_vertices.m
Update the geometry of an object to reflect its current local geometry and reference state.
visibility_graph.m
Generate the visibility graph for a set of objects.