% -*- Prolog -*- % % BOTWORLD project, 1998. % % Peter Rickwood, s2156226. % Peter Gammie, s2191617. % % $Id: bot.pro,v 1.8 2000/07/18 03:02:52 peteg Exp $ /************************************************************** ** GENERAL METHODS AND OVERVIEW ** ** Step 1 : World Creation ** Each bot in botworld builds up a representation of the ** world. This involved expanding everything that the bot may run into ** (beacons and boxes) by the bots radius so that we may then treat the ** bot as a single point in this new configuration space. ** ** Next, we added vertices at the corners of each of these (expanded) ** obstacles. Through some basic arithmetic calculations, the smallest ** possible fence (where each side is a multiple of the bar length) was ** worked out, and a vertex was placed near each bar position in the fence ** so that a bot may stand at that vertex and place a bar. ** ** Vertices were also places either side of each bar, so that a bot ** could reach that vertex and pick up the nearby bar. ** ** At this point, vertices in the world map out all the places where bots ** will ever need to go (next to bars, around the fence, and at the corners ** of each obstacle). The connected graph consisting of all these vertices ** will be the where bots may travel. . . . all that remains is to join ** the vertices . . . and so . . . ** ** The next step was to write some predicates that determine if a ** path from vertex A to vertex B passed through an obstacle. The way ** this is done is as follows: ** ** For each obstacle in our world, determine if the line intersects ** any of that obstacles sides. This is done by extending out original ** line to infinite length, extending each obstacles edge to infinite ** length, finding the intersection point of these two lines (via ** basic trigonometry) and determinging if this intersection point lies ** on the edge of the object in question. ** The algorithm is actually a little smarter than this, in that it rules ** out obstacles where it is obvious that no collision could occur. ** See the comments to pathBlocked for a fuller explanation :-) ** ** Once this is done, it is possible to join vertices in the graph and ** so long as the bot only traverses edges in this graph, it will never ** collide. ** ** The next problem is path planning - given the world ** representation already discussed, how does the bot decide what to do? ** ** The approach here is to decide on a goal vertex to be at, and find ** a path (through a breadth first search say) to that vertex. Deciding ** on the goal would depend on the current state of the world (ie if the ** bot is not holding a bar then a good goal node might be one next to ** a bar, otherwise a good goal node would be next to a fence position). ** ** So much for single bots. The approach for multibots was firstly to ** determine whether a collision occured. This is easily done because ** if, at the end of any move from vertex A to vertex B, the bot is not ** at vertex B, it must have collided. But what to do? ** ** Firstly and most optimistically, the bot retries the last move again. ** The reasoning being that the other bot may have moved by this stage. ** Secondly, the bot tries to find a vertex adjacent to vertex it was trying ** to reach and goes there instead. ** ** Thirdly, the bot moves back to it's starting location and reconsiders the ** path it is to use to get to the goal node. If there is no path to retrace, ** then an arbitrary length-2 path is used instead. ** ** Collision avoidance depends on the fact that the bot will only collide with ** other bots, and can hence assume that the path will eventually clear. ** ** ** NOTE: The bot must start at least bot_radius = 14 units from an obstacle. ** It'd be handy if botworld defined this predicate for us. */ %%%%%%%%%%%%%%%%%%%% %% Constants %%%%%%%%%%%%%%%%%%%% bot_radius(15). arm_length(9). bar_length(50). beacon_radius(5). tolerance(2). % How close we need to be to 'be' at a vertex % to consider ourselves actually at that vertex %%%%%%%%%%%%%%%%%%%% %% Dynamic predicates %%%%%%%%%%%%%%%%%%%% :- dynamic(adjacent/2). :- dynamic(bar/4). :- dynamic(bot/6). :- dynamic(bots/1). :- dynamic(box/5). :- dynamic(fence/4). :- dynamic(limit/1). :- dynamic(obstacle/5). :- dynamic(onBar/2). :- dynamic(vertex/4). :- dynamic(vertexID/1). :- dynamic(queue/1). %%%%%%%%%%%%%%%%%%%% %% Base predicates %%%%%%%%%%%%%%%%%%%% vertexID(1). % we number our vertices, starting with 1 %%%%%%%%%%%%%%%%%%%% %% Entry point: %%%%%%%%%%%%%%%%%%%% start(Bot) :- setupWorld(Bot), makeFence(Bot, 0, Bot), detach(Bot). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% World rep gen code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% %% setupWorld: %%%%%%%%%%%%%%%%%%%% setupWorld(Bot) :- say(Bot, "Creating world..."), look(Bot), % Add boxes in around beacons (so we don't bump into them) findall(BNum, beacon(BNum, _, _), BeaconList), addBeaconBoxes(BeaconList), % Grow the boxes into obstacles in Configuration Space findall(N, box(N, _, _, _, _), Boxlist), setupObstacles(Boxlist), % Design a fence around the beacons designFence, % Add vertices for the bars findall(B, bar(B, _, _, _), BarList), addBarVertices(BarList), % Prune out the invalid vertices introduced in the above processes % Invalid vertices are those that lie within obstacles findall(V, vertex(V, _, _, _), VertexList), findall(O, obstacle(O, X1, Y1, X2, Y2), ObList), write("Pruning vertices...\n"), pruneInvalidVertices(VertexList, ObList), % Add in the bot's starting position bot(Bot, CurX, CurY, _, _, _), asserta(vertex(0, CurX, CurY, corner)), % Grow the graph (add edges) write("Joining vertices...\n"), joinVertices([0 | VertexList], [0 | VertexList]), % Constants for the driver code. countBots, findLimit, silent(Bot), !. %%%%%%%%%%%%%%%%%%%% % nextVertexID: Create a new identifier for a vertex. %%%%%%%%%%%%%%%%%%%% nextVertexID(Num) :- vertexID(Num), retract(vertexID(Num)), Next is Num + 1, asserta(vertexID(Next)). %%%%%%%%%%%%%%%%%%%% % countBots: figure how many bots are in the world. %%%%%%%%%%%%%%%%%%%% countBots :- findall(Num, bot(Num, _, _, _, _, _), List), length(List, Count), asserta(bots(Count)). %%%%%%%%%%%%%%%%%%%% % findLimit: figure out the max number of bar -> fencepos moves to make % take the min number of bars and fencepos's %%%%%%%%%%%%%%%%%%%% findLimit :- countBars(Bars), countFencepos(Fencepos), bmin(Bars, Fencepos, Limit), asserta(limit(Limit)). countBars(Count) :- findall(Num, bar(Num, _, _, _), List), length(List, Count). countFencepos(Count) :- findall(Num, vertex(Num, _, _, fencepos(_, _)), List), length(List, Count). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Code to add vertices for all the objects in the world %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% % addBeaconBoxes: Insert boxes around beacons so we don't crash into them %%%%%%%%%%%%%%%%%%%% addBeaconBoxes([]). addBeaconBoxes([B | Rest]) :- beacon_radius(BeaconRadius), beacon(B, X, Y), Left is X - BeaconRadius, Right is X + BeaconRadius, Top is Y - BeaconRadius, Bottom is Y + BeaconRadius, BoxNum is 0 - B, asserta(box(BoxNum, Left, Top, Right, Bottom)), addBeaconBoxes(Rest). %%%%%%%%%%%%%%%%%%%% % setupObstacles: transform boxes into obstacles in Configuration Space %%%%%%%%%%%%%%%%%%%% setupObstacles([]). setupObstacles([B | Rest]) :- bot_radius(Radius), box(B, X1, Y1, X2, Y2), NewX1 is X1 - Radius, NewY1 is Y1 - Radius, NewX2 is X2 + Radius, NewY2 is Y2 + Radius, asserta(obstacle(B, NewX1, NewY1, NewX2, NewY2)), nextVertexID(V1), asserta(vertex(V1, NewX1, NewY1, normal)), nextVertexID(V2), asserta(vertex(V2, NewX1, NewY2, normal)), nextVertexID(V3), asserta(vertex(V3, NewX2, NewY1, normal)), nextVertexID(V4), asserta(vertex(V4, NewX2, NewY2, normal)), setupObstacles(Rest). %%%%%%%%%%%%%%%%%%%% % designFence: Calculate the positions and angles required to construct a % fence around the beacons. This is a long predicate due to % the large amount of common data, and sequential nature of % the computation. The fence around the beacons is also % padded out to be an integral number of bar lengths. %%%%%%%%%%%%%%%%%%%% designFence :- write("Designing fence...\n"), % Load up the constants bar_length(Bar_Length), bot_radius(Bot_Radius), beacon_radius(Beacon_Radius), % Get all the beacon coordinates findall(X, beacon(_, X, _), Xlist), findall(Y, beacon(_, _, Y), Ylist), % Find the extrema of our fence (ie the coordinates of the leftmost, % rightmost, topmost, and bottommost beacons) minlist(Xlist, XMin), Left is XMin - Beacon_Radius, maxlist(Xlist, XMax), Right is XMax + Beacon_Radius, minlist(Ylist, YMin), Top is YMin - Beacon_Radius, maxlist(Ylist, YMax), Bottom is YMax + Beacon_Radius, % Calc the number of bars required, and the dimensions of the resultant fence BaseWidth is Right - Left, BaseHeight is Bottom - Top, HorizontalBars is ((BaseWidth - 1) // Bar_Length) + 1, VerticalBars is ((BaseHeight - 1) // Bar_Length) + 1, Width is HorizontalBars * Bar_Length, Height is VerticalBars * Bar_Length, PadWidth is Width - BaseWidth, PadHeight is Height - BaseHeight, Fence_Left is Left - (PadWidth // 2) + (PadWidth mod 2), Fence_Right is Right + (PadWidth // 2), Fence_Top is Top - (PadHeight // 2) + (PadHeight mod 2), Fence_Bottom is Bottom + (PadHeight // 2), asserta(fence(Fence_Left, Fence_Top, Fence_Right, Fence_Bottom)), % Finally, add in the vertices for the fence addFenceVertices(Fence_Left, Fence_Top, VerticalBars, HorizontalBars). %%%%%%%%%%%%%%%%%%%%%% %% The following predicates add the vertices around our fence (one at each %% place where we need to drop a bar). In order %% to make things easier for the bot, some information is stored in each of %% these vertices telling the bot what angle to be at to place a bar in %% that fence position. %% %% This is done simply by recursing on the number of horizontal bars and %% vertical bars that we need to place. %%%%%%%%%%%%%%%%%%%%%%% % Vbars is the number of Vertical bars needed in the fence % Hbars is the number of Horizontal bars needed in the fence % (L, T) are the coordinates of the top left of the fence addFenceVertices(L, T, Vbars, Hbars) :- addFVsub(L, T, L, T, Vbars, Hbars, 1). % no more vertices left to add addFVsub(L, T, X, Y, 0, 0, _) :- abolish(fence/4), !. % only vertices for horizontal bars positions left to add addFVsub(L, T, X, Y, 0, Horiz, Position) :- !, arm_length(Arm_length), bot_radius(Bot_Radius), EffectiveArmLength is Arm_length + Bot_Radius, bar_length(Bar_length), Vertex_X is X + Bar_length/2, Vertex_Y1 is T - EffectiveArmLength, nextVertexID(Vnum), asserta(vertex(Vnum, Vertex_X, Vertex_Y1, fencepos(Position, 90))), NewPos1 is Position + 1, fence(L, T, _, B), Vertex_Y2 is B + EffectiveArmLength, nextVertexID(Vnum2), asserta(vertex(Vnum2, Vertex_X, Vertex_Y2, fencepos(NewPos1, 270))), NewPosition is NewPos1 + 1, NewX is X + Bar_length, NewHoriz is Horiz - 1, addFVsub(L, T, NewX, Y, 0, NewHoriz, NewPosition). % vertices for both vertical and horizontal fence positions % still to add addFVsub(L, T, X, Y, Vert, Horiz, Position) :- arm_length(Arm_length), bot_radius(Bot_Radius), bar_length(Bar_length), EffectiveArmLength is Arm_length + Bot_Radius, Vertex_Y is Y + Bar_length/2, Vertex_X1 is L - EffectiveArmLength, nextVertexID(Vnum1), asserta(vertex(Vnum1, Vertex_X1, Vertex_Y, fencepos(Position, 0))), NewPos1 is Position + 1, fence(L, T, R, _), Vertex_X2 is R + EffectiveArmLength, nextVertexID(Vnum2), asserta(vertex(Vnum2, Vertex_X2, Vertex_Y, fencepos(NewPos1, 180))), NewPosition is NewPos1 + 1, NewY is Y + Bar_length, NewVert is Vert - 1, addFVsub(L, T, X, NewY, NewVert, Horiz, NewPosition). %%%%%%%%%%%%%%%%%%%% % addBarVertices: Calc pos and orientation for the bot relative to each bar %%%%%%%%%%%%%%%%%%%% addBarVertices([]). addBarVertices([Bar | BarList]) :- bot_radius(Bot_Radius), arm_length(Arm_Length), DistanceFromBar is Bot_Radius + Arm_Length, bar(Bar, X, Y, Orientation), AngleA_deg is (Orientation + 90) mod 360, AngleB_deg is (Orientation + 270) mod 360, degreesToRadians(AngleA_deg, RadianA), degreesToRadians(AngleB_deg, RadianB), endPoint(X, Y, RadianA, DistanceFromBar, Vertex1_X, Vertex1_Y), endPoint(X, Y, RadianB, DistanceFromBar, Vertex2_X, Vertex2_Y), nextVertexID(V1_ID), asserta(vertex(V1_ID, Vertex1_X, Vertex1_Y, bar(Bar, AngleA_deg))), nextVertexID(V2_ID), asserta(vertex(V2_ID, Vertex2_X, Vertex2_Y, bar(Bar, AngleB_deg))), addBarVertices(BarList). %%%%%%%%%%%%%%%%%%%% % pruneInvalidVertices: Remove unreachable vertices (inside obstacles) %%%%%%%%%%%%%%%%%%%% pruneInvalidVertices([], ObList). pruneInvalidVertices([Vertex | Rest], ObList) :- vertex(Vertex, X, Y, Type), inobstacle(X, Y, ObList), write("Pruning: "), write(vertex(Vertex, X, Y, Type)), write("\n"), retract(vertex(Vertex, X, Y, Type)), pruneInvalidVertices(Rest, ObList). pruneInvalidVertices([Vertex | Rest], ObList) :- pruneInvalidVertices(Rest, ObList). %%%%% %% inobstacle - does the point X, Y lie within any of the %% objects in the list [Head | Tail]? %%%%% inobstacle(X, Y, [Head | Tail]) :- obstacle(Head, TL_x, TL_y, BR_x, BR_y), X > TL_x, X < BR_x, Y > TL_y, Y < BR_y, !. % dont find redundant solutions inobstacle(X, Y, [Head | Tail]) :- inobstacle(X, Y, Tail). % Determines whether a given line lies along the edge % of an obstacle onEdge(X, PY1, X, _, obstacle(_, X, Y1, _, Y2)) :- PY1 >= Y1, PY1 =< Y2, !. onEdge(X, PY1, X, _, obstacle(_, _, Y1, X, Y2)) :- PY1 >= Y1, PY1 =< Y2, !. onEdge(PX1, Y, _, Y, obstacle(_, X1, Y, X2, _)) :- PX1 >= X1, PX1 =< X2, !. onEdge(PX1, Y, _, Y, obstacle(_, X1, _, X2, Y)) :- PX1 >= X1, PX1 =< X2, !. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Code to add edges between the vertices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% % joinVertices: Put in all edges that don't pass through obstacles. %%%%%%%%%%%%%%%%%%%% joinVertices([], AllVertices) :- !. joinVertices([Head | Rest], AllVertices) :- joinVertex(Head, AllVertices), joinVertices(Rest, AllVertices). joinVertex(Vertex, []) :- !. % no reflexive edges joinVertex(Vertex, [Vertex | Tail]) :- !, joinVertex(Vertex, Tail). joinVertex(Vertex, [Head | Tail]) :- not(adjacent(Vertex, Head)), % already done symmetries vertex(Vertex, X, Y, _), vertex(Head, X2, Y2, _), not(pathBlocked(X, Y, X2, Y2)), asserta(adjacent(Vertex, Head)), asserta(adjacent(Head, Vertex)), % write("adjacent("), write(Vertex), write(", "), write(Head), write(").\n"), % write("adjacent("), write(Head), write(", "), write(Vertex), write(").\n"), !, joinVertex(Vertex, Tail). joinVertex(Vertex, [Head | Tail]) :- joinVertex(Vertex, Tail). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % A function to determine if a line from (x1,y1) to (x2,y2) % passes through any obstacle %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% % A path is Blocked if it crosses a horizontal side of any % obstacle, or if it crosses the vertical side of any obstacle, % % Extensive use of cuts are made in the clauses below. These cuts are, % in the main, used to prevent prolog from finding redundant solutions. % For example, if we know that a path from A to B is blocked by the % side of a particular obstacle, there is no need to check if it is % blocked by any other obstacle. %%%%%%% %%%%%%%%%%%%%%%%%%%% %% pathBlocked: Determine if there is a clear path between two vertices in %% Configuration Space. %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Before going on, a brief explanation of how we determine if a path from %%% A (X1,Y1) to B (X2,Y2) is blocked. %%% %%% For each obstacle, we determine if it is possible that the line A -> B %%% intersects it. This basically involves the ruling out of obstacles that do not %%% share either X or Y values with our endpoints. The way this is done is to extend the %%% side of an obstacle to infinite length and see if the line from A -> B intersects that %%% line. For example, a line from 1,1 to 2,1 cannot %%% intersect with an obstacle whose left side has equation x = 4 and right side equation %%% x = 10. %%% %%% If it is possible that A -> B intersects with a particular obstacle, we determine the %%% intersection point of A -> B and each (extended) obstacle edge and see if that point lies %%% on the (unextended) obstacle edge. This requires a bit of trigonometry for lines that %%% are not horizontal or vertical. %%%%%%%%%%%%%%%%%%%%%%%%%%% % Eliminate reflexive edges pathBlocked(X1, Y1, X1, Y1). % Eliminate the degenerate case of paths internal to obstacles. pathBlocked(PX1, PY1, PX2, PY2) :- obstacle(_, X1, Y1, X2, Y2), PX1 >= X1, PX1 =< X2, PX2 >= X1, PX2 =< X2, PY1 >= Y1, PY1 =< Y2, PY2 >= Y1, PY2 =< Y2, not(onEdge(PX1, PY1, PX2, PY2, obstacle(_, X1, Y1, X2, Y2))), !. % Check horizontal and vertical cases pathBlocked(X1, Y1, X2, Y2) :- horizontalBlocked(X1, Y1, X2, Y2), !. pathBlocked(X1, Y1, X2, Y2) :- verticalBlocked(X1, Y1, X2, Y2), !. %%%%%%%%%%%%%%%%%%%% % horizontalBlocked: Check if the path crosses the left or right side of an obstacle % The horizontal progress of any line is halted if, for any obstacle, % that line crosses the vertical edges of that obstacle %%%%%%%%%%%%%%%%%%%% horizontalBlocked(X1, Y1, X2, Y2) :- % path is blocked if pathAngle(X1, Y1, X2, Y2, Angle), !, obstacle(_, TL_x, TL_y, BR_x, BR_y), % there exists an obstacle % such that it crosses one of that obstacles sides crossesVertical(X1, Y1, X2, Y2, Angle, obstacle(_,TL_x,TL_y,BR_x,BR_y)). %%%%%%%%%%%%%%%%%%%% % crossesVertical: True if the path crosses a vertical edge of an obstacle %%%%%%%%%%%%%%%%%%%% % if A to B is vertical, can't intersect a vertical line crossesVertical(X, _, X, _, _, _) :- !, fail. % Ensure path goes from left to right for later convenience. crossesVertical(X1,Y1,X2,Y2,Angle,Obst) :- X1 > X2, crossesVertical(X2,Y2,X1,Y1,Angle,Obst). %%%%%%%%%%%% % If line from A to B is horizontal, we can skip all the trigonometry, as % determining intersection points are easy: % % It crosses an edge if the given obstacle's edges share Y values % and point A is to the left of a given edge and point B is to the right. % (so it must intersect somewhere inbetween). %%%%%%%%%%%% crossesVertical(X1, Y, X2, Y, _, obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- !, Y > TL_y, Y < BR_y, X1 < TL_x, X2 > TL_x. crossesVertical(X1, Y, X2, Y, _, obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- !, Y > TL_y, Y < BR_y, X1 < BR_x, X2 > BR_x. %%%%%%%%%%%%%%%%%%%%% % If the object's edge is not inbetween start and finish of our path % then we cannot intersect it. (ie it does not share any Y values, so we cannot % cross a vertical edge of its) % % if any of these inequalities succeed, dont bother checking any other clauses % (hence the cuts). %%%%%%%%%%%%%%%%%%%% crossesVertical(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- BR_x =< X1, !, fail. % path is to the right of specified obstacle crossesVertical(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- TL_x >= X2, !, fail. % path is to the left of specified obstacle crossesVertical(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- X1 >= TL_x, X2 =< BR_x, !, fail. % path lies inbetween left and % right edges, so cannot intersect % them. %%%%%%%%%%%%%%%%%%%%%%% % The main clause. Work out where the path crosses the (extended) vertical edge of % the given obstacle, and if that point lies on the (unextended) edge of that obstacle, % then the path is blocked. % % NB - if this clause is executed, then we can be assured that % either X1 < TL_x and X2 > TL_x % or X1 < BR_x and X2 > BR_x % % (In English - A lies to the left of the left edge of the given obstacle and % B lies to the right of the left edge of the given obstacle. % OR A lies to the left of the right edge of the given abstacle and % B lies to the right of the right edge.) %%%%%%%%%%%%%%%%%%%%%% crossesVertical(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- LeftEdgeDistance is TL_x - X1, % check the left edge LeftEdgeDistance > 0, Angle2 is 3.1415926 / 2 - Angle, Ycoord is ((LeftEdgeDistance * sin(Angle)) / sin(Angle2)) + Y1, Ycoord > TL_y, Ycoord < BR_y, !. % see if intersection point lies on edge crossesVertical(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- RightEdgeDistance is BR_x - X1, % check the right edge RightEdgeDistance > 0, Angle2 is 3.1415926 / 2 - Angle, Ycoord is ((RightEdgeDistance * sin(Angle)) / sin(Angle2)) + Y1, Ycoord > TL_y, Ycoord < BR_y, !. % see if intersection point lies on edge %%%%%%%%%%%%%%%%%%%%%% %% The following clauses are mirrors of the above clauses, only they now check %% for intersection with horizontal lines, not vertical. %%%%%%%%%%%%%%%%%%%%%% verticalBlocked(X1,Y1,X2,Y2) :- pathAngle(X1,Y1,X2,Y2, Angle), !, obstacle(_,TL_x,TL_y,BR_x,BR_y), crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)). %% if the line is horizontal, it cannot intersect with a horizontal line crossesHorizontal(_,Y,_,Y,_,_) :- !, fail. %% make sure path is declared top to bottom crossesHorizontal(X1,Y1,X2,Y2,Angle,Obst) :- Y1 > Y2, crossesHorizontal(X2,Y2,X1,Y1,Angle,Obst). crossesHorizontal(X, Y1, X, Y2, Angle, obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- !, X > TL_x, X < BR_x, Y1 < TL_y, Y2 > TL_y. crossesHorizontal(X, Y1, X, Y2, Angle, obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- !, X > TL_x, X < BR_x, Y1 < BR_y, Y2 > BR_y. % Rule out cases where the path lies completely below or above % the obstacle, or within the obstacles Y values. crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- TL_y >= Y2, !, fail. crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- BR_y =< Y1, !, fail. crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- Y1 >= TL_y, Y2 =< BR_y, !, fail. %%% NB - if the main clause is executed, then we can be assured that %%% either Y1 < TL_y and Y2 > TL_y %%% or Y1 < BR_y and Y2 > BR_y %% debug check this. X2 may be > X1 . . . but I think that that is OK. %% debug if we didnt have the >= and <= (and instead had < and > %% then lines can pass through the corners of obstacles %% (ie the very corner) and be considered OK) crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- TopEdgeDistance is TL_y - Y1, TopEdgeDistance > 0, Angle2 is 3.1415926 / 2 - Angle, Xcoord is ((TopEdgeDistance*sin(Angle2)) / sin(Angle)) + X1, Xcoord >= TL_x, Xcoord =< BR_x, !. crossesHorizontal(X1,Y1,X2,Y2,Angle,obstacle(_,TL_x,TL_y,BR_x,BR_y)) :- BottomEdgeDistance is BR_y - Y1, BottomEdgeDistance > 0, Angle2 is 3.1415926 / 2 - Angle, Xcoord is ((BottomEdgeDistance*sin(Angle2)) / sin(Angle)) + X1, Xcoord >= TL_x, Xcoord =< BR_x, !. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % RETURNS THE ANGLE IN RADIANS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % works out the angle between the X axis and the line. % Remember that Claude's coordinate system is not conventional % Top left is Origin. % make sure path is declared left to right pathAngle(AX, AY, BX, BY, Angle) :- BX < AX, pathAngle(BX, BY, AX, AY, Angle). pathAngle(AX, AY, BX, BY, Angle) :- Hypotenuse is sqrt((AX - BX) * (AX - BX) + (AY - BY) * (AY - BY)), Height is BY - AY, Angle is asin(Height / Hypotenuse). %%%%%%%%%%%%%%%%%%%%%%%%%%%% % endPoint: given a starting point, an angle, and a distance, find the endpoint %%%%%%%%%%%%%%%%%%%%%%%%%%%% % RADIANS endPoint(StartX, StartY, Angle, Distance, EndX, EndY) :- Angle2 is 3.1415926 / 2 - Angle, EndX is StartX - Distance * sin(Angle2), EndY is StartY - Distance * sin(Angle). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Goal and path selection, and BOTWORLD driving code. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% % makeFence(BotNumber, CurVertexNumber, CurGoalNumber): move bars to the fence. % CurGoalNumber is the bar/fence pos number %%%%%%%%%%%%%%%%%%%% makeFence(Bot, Cur, N) :- limit(Limit), N > Limit, parkBot(Bot, Cur), write("Finished making fence...\n"), !. makeFence(Bot, Cur, N) :- write("Making fence...\n"), moveBarToFence(Bot, N, Cur, Loc), bots(B), NextN is N + B, makeFence(Bot, Loc, NextN). %%%%%%%%%%%%%%%%%%%% % moveBarToFence: Do a complete -> bar -> fence move %%%%%%%%%%%%%%%%%%%% moveBarToFence(Bot, N, Cur, FenceN) :- vertex(BarN, _, _, bar(N, _)), write("Finding path from "), write(Cur), write(" -> "), write(BarN), write("\n"), findPath(BarN, Cur, BPath), write("Found: "), write(BPath), write("\n"), movePath(Bot, [], BPath), !, vertex(FenceN, _, _, fencepos(N, _)), findPath(FenceN, BarN, FPath), movePath(Bot, [], FPath), !. %%%%%%%%%%%%%%%%%%%% % findPath: do a depth-first iterative deepening search (courtesy of the text) % to find a path from the current node to the bar or fencepos. %%%%%%%%%%%%%%%%%%%% findPath(Start, Start, [Start]). findPath(Start, Goal, [Goal | Path]) :- findPath(Start, PredGoal, Path), % write("findPath: halfway "), write([Goal|Path]), nl, adjacent(PredGoal, Goal), not(member(Goal, Path)). % write("Path: "), write([Goal|Path]), write("\n"). %%%%%%%%%%%%%%%%%%%% % movePath: take a path and issue BotWorld commands to traverse it %%%%%%%%%%%%%%%%%%%% movePath(Bot, Past, []). movePath(Bot, Past, [Next | Future]) :- vertex(Next, _, _, VType), doMove(Bot, VType, Past, [Next | Future]), movePath(Bot, [Next | Past], Future). % If a bar is the goal: goto(), turn(), grab() doMove(Bot, bar(Bar, BarAngle), Past, [Next]) :- !, getThere(Bot, Past, Next), look(Bot), bot(Bot, _, _, BotAngle, _, _), Angle is BarAngle - BotAngle, turn(Bot, Angle), grab(Bot, Bar). % If a fence position is the goal: goto(), turn(), drop() doMove(Bot, fencepos(_, FenceAngle), Past, [Next]) :- !, getThere(Bot, Past, Next), look(Bot), bot(Bot, _, _, BotAngle, _, _), Angle is FenceAngle - BotAngle, turn(Bot, Angle), drop(Bot). % Otherwise just goto() doMove(Bot, _, Past, [Next | Future]) :- getThere(Bot, Past, Next). %%%%%%%%%%%%%%%%%%%% % getThere: Ensure we get to a given vertex. %%%%%%%%%%%%%%%%%%%% getThere(Bot, Past, Next) :- vertex(Next, NextX, NextY, _), goto(Bot, NextX, NextY), checkMove(Bot, Past, Next). % Check if we made it checkMove(Bot, Past, Next) :- atVertex(Bot, Next). % Nope, try to recover % Try it again straight away, naive and optimistic checkMove(Bot, Past, Next) :- vertex(Next, NextX, NextY, _), goto(Bot, NextX, NextY), atVertex(Bot, Next), !. % Look for a vertex adjacent to Pred and Next checkMove(Bot, [Pred | Past], Next) :- adjacent(Pred, Int), adjacent(Int, Next), vertex(Int, IntX, IntY, _), goto(Bot, IntX, IntY), vertex(Next, NextX, NextY, _), goto(Bot, NextX, NextY), atVertex(Bot, Next), !. % Catch the case of no history checkMove(Bot, [], Next) :- % Select an arbitrary length-2 path from the immediate predecessor to somewhere adjacent(Pred, Int1), not(unify(Int1, Next)), adjacent(Int1, Int2), not(unify(Int2, Pred)), not(unify(Int2, Next)), % Select a path to it, and then to the Next node findPath(Next, Int2, Path), append([Pred, Int1, Int2], Path, BPath), movePath(Bot, [], BPath), !. % Give up, unwind the path we followed so far, then allow backtracking to fix it checkMove(Bot, Past, Next) :- movePath(Bot, [], Past), !, fail. %%%%%%%%%%%%%%%%%%%% % atVertex: figure out if we're at a particular vertex. %%%%%%%%%%%%%%%%%%%% atVertex(Bot, VNum) :- look(Bot), bot(Bot, BotX, BotY, _, _, _), vertex(VNum, X, Y, _), tolerance(T), abs(BotX - X) < T, abs(BotY - Y) < T. atVertex(Bot, VNum) :- bot(Bot, BotX, BotY, _, _, _), vertex(VNum, X, Y, _), write("bot not at vertex:\n BotX = "), write(BotX), write("\n BotY = "), write(BotY), write("\n VX = "), write(X), write("\n VY = "), write(Y), write("\n"), fail. %%%%%%%%%%%%%%%%%%%% %% parkBot: Do something (!) until all bots are finished %%%%%%%%%%%%%%%%%%%% parkBot(Bot, Cur) :- say(Bot, "Done"), look(Bot), not(bot(_, _, _, _, _, "")), !. % Go for a wander parkBot(Bot, Cur) :- findPath(0, Cur, Path), movePath(Bot, [], Path), !. parkBot(Bot, Cur). %%%%%%%%%%%%%%%%%%%% % General utility predicates %%%%%%%%%%%%%%%%%%%% % Find the max / min of two numbers bmin(A, none, A) :- !. bmin(A, B, A) :- A =< B, !. bmin(A, B, B) :- B < A. bmax(A, none, A) :- !. bmax(A, B, A) :- A >= B, !. bmax(A, B, B) :- B > A. % Find the max / min of a list maxlist([X], X). maxlist([X, Y | Rest], Max) :- maxlist([Y | Rest], MaxRest), bmax(X, MaxRest, Max). minlist([X], X). minlist([X, Y | Rest], Min) :- minlist([Y | Rest], MinRest), bmin(X, MinRest, Min). % Convert to radians degreesToRadians(Degree, Radians) :- Radians is ((Degree * 3.1415926 * 2) / 360). %%%%%%%%%%%%%%%%%%%% % Standard library predicates % append/3: as usual. append([], X, X). append([X|Xs], Y, [X|Z]) :- append(Xs, Y, Z). % findall/3: dodgy. findall(Param, Goal, _) :- call(Goal), assertz(queue(Param)), fail. findall(_, _, Xlist) :- assertz(queue(bottom)), collect(Xlist), !. collect(L) :- retract(queue(X)), !, new_list(X, L). new_list(bottom, []). new_list(X, [X|L1]) :- collect(L1). % length/2: as usual. length([], 0). length([_|Tail], N) :- length(Tail, N0), N is N0 + 1. % member/2: an old friend. member(X, [X|_]). member(X, [_|Ys]) :- member(X, Ys). % not/1: It's easier in prolog than native. not(X) :- call(X), !, fail. not(X). %%%%%%%%%%%%%%%%%%%% % Rebuild the applet-db build :- serialize("bot.ser").