% -*- 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").