1. Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010

2. What is Motion Planning? Given a set of obstacles, a start position, and a destination position, find a path from start to destination that doesn’t go through any obstacles This project seeks shortest path Many variations on the basic problem

4. Program Two parts: Course builder and simulator Course Builder: made in C# using XNA and Windows forms User-friendly interface Simulator: approximately 2000 lines of code OpenGL rendering

5. Potential Project Goals Point object path finding Polygonal object finding Robot rotation Real-time calculation Non-convex obstacles Moving obstacles Curves No global vision 3D problem Path weighting Group travel

6. Procedure Define input: obstacles, “robot”, starting position, and destination Create a visibility graph Find the shortest path from start to destination in the graph

7. Required Geometry Point Vector Line Line Segment Ray Poly Line Polygon

8. Defining a Polygon Ordered set vertices Edges implicitly defined by adjacency in the vertex list May be clockwise or counter-clockwise wound

9. Open or Closed Polygons? OpenClosed Does it matter?

10. Open vs Closed Polygon No Intersection Intersection

11. Open vs Closed Polygons Cannot achieve shortest possible path with closed polygons Always can get slightly closer Open polygons cannot get any closer to the polygon without penetrating it

12. Course Builder Create 2D polygons and lay them out as a course Saved to file, loaded by simulator

13. Convex Hull Algorithm #1 Trivial O(n^3) algorithm Every pair of vertices is checked to see if it can be an edge of the convex set

14. Convex Hull Algorithm #2 Gift wrapping technique O(n*h) n: # of vertices h: # of vertices in the convex hull

15. Convex Hull Algorithm #3 Graham-Schmidt algorithm O(n log n) n: # of vertices Sorts the points Didn’t attempt algorithm #4: mix of gift wrapping with Graham-Schmidt

16. Polygonal Robot Instead of a point robot, use a polygon By calculating the Configuration Space of the polygonal robot, the robot can be represented by a point C-Space is the areas that the point robot cannot move

17. Configuration Space Calculate C-Space version of each obstacle using Minkowski sum of the polygon and the robot

18. Minkowski Sum Algorithm #1 Trivial algorithm - O(n^3) For each vertex a in polygon A For each vertex b in polygon B Add point (a+b) to P Minkowski sum is the convex hull of points P

19. Minkowski Sum Algorithm #2 Sweep method – O(n + m) n: # of vertices in polygon A m: # of vertices in polygon B Sorts the points in each polygon in counter- clockwise order Next point is the point with smallest angle

20. Visibility Graph Graph vertices represent vertices of the C-Space obstacles and the start and destination points Edges represent possible paths If a line segment from any vertex in the graph to any other vertex in the graph doesn’t penetrate an obstacle, there’s an edge between those two vertices Edge weights represent distance between points Shortest path in the graph is shortest path through the obstacles

21. Graph

22. Line Segment-Polygon Intersection

23. Visibility Graph Algorithm #1 O(n^3) Create every possible line segment from pairs of vertices in the graph Any line segment that doesn’t penetrate an obstacle gets added as an edge in the graph

24. Visibility Graph Algorithm #2 O(n^2 log n) Sweep algorithm Uses the fact that some edges will obscure other edges from being visible Requires a line segment search tree data structure

25. Ray-Line Segment AVG Tree Balanced binary search tree Given a ray, find the line segment in the tree that it will intersect first

26. Shortest Path Now that we have an obstacle course and a visibility graph for it, we can calculate the shortest path using graph algorithms

27. Dijkstra’s Algorithm Breadth first search with weights Only non-negative edges Requires priority queue

28. Path Finding in Video Games Generally discretised (grid based) 2D path finding useful in many 3D games Speed important (1/60 th of a second/frame) Accuracy less important Much of the environment is static Precomputation often available

29. Precomputation Precalculate visibility graph of C-Space obstacles only (don’t include any start or end point) To create the full visibility graph, insert start and destination points (calculating visibility for each polygon vertex for them) Use a copy of precalculated visibility graph so it can be reused Needs to be recomputed as often as the environment changes

30. Statistics TO FILL IN WHEN I HAVE THE FINAL STATISTICS AVAILABLE

31. More Speed? Multi-threading CUDA Performance optimizations (length squared, etc) Minimizing geometry where possible Sacrifice accuracy Amortize path finding cost over time

32. Non-Convex Polygons

33. Robot Rotation Unlike translation, rotating the robot changes the C-Space Even if rotation angles are discretized, greatly enlarges the graph size, and thus the computation time Gave up on this problem to pursue other aspects of motion planning

34. Goals Revisited Point object path finding Polygonal object finding Robot rotation Real-time calculation Non-convex obstacles Moving obstacles Curves No global vision 3D problem Path weighting Group travel Too computationally expensive for real-time Not as useful in video games, slow Didn’t get to it Beyond the scope of this project AI Problem

35. Further Exploration Rotation Better precomputation Non-convex performance Sacrificing accuracy for performance Regional partitioning

36. Implicit Line Equation from 2 Points Given two points on the line, calculate a, b, c  Pick any a and b that make this true  Use any point (x,y) on the line * Any multiple k ≠ 0 applied to (a, b, c) will result in the same line

37. Half Space (2D) Input: Line and a Point Output: Real (+,-,0) Geometric Interpretation: 0 means the point is on the line + means on one side of the line - means on the other side Plug point into implicit line equation Result is the half space that contains the point (+,-,0)

38. Half Space (2D) cont. + -

39. Line-Line Intersection (2D) Line 0: Line 1: Solve for (x,y): two equations, two unknowns Three cases: - No intersection (parallel lines) - Always intersect (same line) - One intersection

40. Line-Line Intersection (2D) cont. No intersection (parallel lines) Always intersect (same line) Otherwise, solve for (x,y) Constant time Slope of the two lines are equal: For k ≠ 0: But not the same line:

41. Line Segment-Line Segment Intersection Find intersection point using line-line intersection (if it exists, if it doesn’t the line segments don’t intersect) Create a ray for each line segment Ray ranges from 0 to 1 along the line segment Calculate t of the intersection point for each line segment If both t’s are in [0,1], line segments intersect

42. Point-Line Intersection Calculate the halfspace the point lies in If halfspace equals 0, point is on the line Floating point error may play into this Use an epsilon, point is near close enough to being on the line

43. Point-Polygon Intersection Calculate the half space the point lies in for each edge of the polygon If the polygon is clockwise wound, the point must always be in - half space If the polygon is counter-clockwise wound, the point must always be in + half space O(# of vertices in the polygon)

44. + + + + - - - - Clockwise Winding + + + +- - - - Counter-Clockwise Winding

45. Polygon-Polygon Intersection Possibly intersecting or fully contained Check each line segment in polygon A for intersection with each line segment in polygon B Check for containment: pick a point from A and see if it’s in B (and do same check to see if B is in A)