Personally this is what I do - although I've never thought of the problem in a 3d plane to be honest, this is what I found to be my 'simplest' efficient self hauled design, which turns out to be a little like an A* search pattern... I'll write it in pseudo-code to make it shorter and easier to understand
Code:
The records I use are as follows:
A 'path' which contains -
A boolean array the same size as that of the area to navigate but empty at the start.
The final number of 'steps'/waypoints used to reach the destination
Array of X,Y cooridnates of each waypoint
An array of bits the same size as the area to navigate. 0/False signify space that can be moved into, 1/True designate an obstruction.
Another array of bits the same size as the area to navigate.
Thus the algorithm goes:
Code:
proc algorithm.run()
1. Input sensor data into the area grid (ie. Flip the bits so that the array now contains the obstructions.
2. Determine how many options the algorithm should consider - dependant on avilable memory/cpu time. Call this integer MaxRoutes
3. while c < MaxRoutes do //where c is the current route being processed
3.0 c += 1 //forgot to add at first, so thats why its 3.0 XD
3.1 AddNewRouteToArrayOfRouteOptions() //Adds a new, empty route data type to the array
3.2 NewRoute.Process() //see below
4. while c > 0 do
4.0 c -= 1
4.1 ArrayOfRouteOptions[c].Optimize(OptimizeLevel) // see below
5. ShortestRoute := GetShortestRoute() //a simple loop that checks all the considered options' lengths and finds the shortest
6. FollowRoute(ShortestRoute)
7. CleanMemory()
end;
proc Route.Process()
1. RouteGradient := GetRelativeGoalOrientation() //Work out the 'angle' from the robot facing straight up to the goal as the crow flies. If there are no obstructions, this is the fastest route and trunc it to either of:
-1.0: Goal is directly to the left
-0.5: Goal is diagonally up to the left
+/- 0: Goal is straight up
+0.5: Goal is diagonally up to the right
+1.0: Goal is directly to the right
2. If RouteGradient > 1 or <-1 then
2.1 (Rotate180)
2.2 RouteGradient := GetRelativeGoalOrientation()
2.3 if RouteGradient > 1 or < -1 then
2.3.1 You have arrived
3. TryDirection := RouteGradient - 0.5 //we add 0.5 on the first cycle of the while loop
3. CanAdvance := False //forgot to set so 2 number 3s.... sorry :D
4. while CanAdvance = False do
4.0 TryDirection += 0.5
4.1 if GlobalBinaryMap[ RobotPositionX + round(RouteGradientToChangeInX), RobotPositionX + round(RouteGradientToChangeInX)] = false then //convert our TryDirection to a vector type deal...
4.1.1 AddWayPoint(RobotPositionX + round(RouteGradientToChangeInX), RobotPositionX + round(RouteGradientToChangeInX)) //adds a waypoint for that empty 'cell' in the 2D array as the path to follow. ALSO flips the bit at that position in the local array to say its part of the route.
4.1.2 CanAdvance := True
5. If LastWayPointIsAtGoal = false then
5.1 Process() //please use a loop rather than this... I'm lazy :D
end;
proc Route.Optimize
This is getting long so heres the deal: ::)
Follow the routes waypoints and each waypoint check if any adjacent cells are not empty. If so, we have a shortcut so find which waypoint to jump to and wipe everything in between. Update the 2d array at the same time.
Rerun as you like as it 'smooths' the route though can be CPU intensive...
end;
Long confusing and personally I have similar - though longer - pseudocode on paper in a messier font of my own hand since I've never had to implement it... My enemies always go from point to point and I collision detect so... Enjoy - and if there are any question fire away
Sorry for the long post guys
Edit: seeing this makes me wish we had syntax highlighting in that... even just a little bit ...and maybe I should code it up, and make it look nice as an article for the PGD library too O.o
Bookmarks