This code uses fuzzy logic to determine which critter is in the best position to solve the problem the other critters will not compete and wont get in the way. The problem they face is getting the ball (yellow circle) into the goal (Red circle) you can move the ball any where in the legal area the critters will always clean up after you.
// ======================================= //
// AI : Team work //
// --------------------------------------- //
// The problem : //
// A team of bots must work together to //
// put the ball in the goal //
// --------------------------------------- //
// Progress towards the solution is graded //
// by first distance between ball and goal //
// second distance between AI bot and ball //
// Third whether bot can see ball and goal //
// ======================================= //
debug = 0
open window 640, 512
window origin "cc"
UP = 16
DN = 64
LF = 128
RT = 32
LZ = 4096
RZ = 8192
bots = 3 // bot 0 is the ball
dim PosX(bots) // position x
dim PosY(bots) // position y
dim RotZ(bots) // rotation
dim DirX(bots) // lateral direction
dim DirY(bots) // foward direction
dim Radi(bots) // Radius
dim Ctrl(bots) // control flags
dim SeeBall(bots)
dim SeeGoal(bots)
dim NearBall(bots)
dim Solved(bots)
BestProgress = 640
label Init
// The environment
x1 = -220.0
y1 = -220.0
x2 = 220.0
y2 = 220.0
x3 = -200.0+50.0
y3 = -200.0+50.0
x4 = 200.0-50.0
y4 = 200.0-50.0
// The goal
GoalX = 0.00
GoalY = 0.00
GoalR = 30.0
// the problem
PosX(0) = -100.0
PosY(0) = -100.0
RotZ(0) = pi*3/2
Radi(0) = 10.00
// The AI bots
for b = 1 to bots
Ctrl(b) = 0
Radi(b) = 30.0
DirX(b) = 0.0
DirY(b) = 0.0
RotZ(b) = ran(pi) - ran(pi)
0 PosX(b) = ran(200.0-Radi) - ran(200.0-Radi)
PosY(b) = ran(200.0-Radi) - ran(200.0-Radi)
for a = 0 to bots
if (a <> b) then
adj = PosX(a) - PosX(b)
opp = PosY(a) - PosY(b)
hyp = sqrt(adj*adj + opp*opp)
if (hyp < Radi(a) + Radi(b)) goto 0
endif
next a
next b
bot = 1
label Main
gosub Goal
gosub AI
gosub Control
gosub Draw
//wait 0.002
goto Main
label Goal
adj = PosX(0) - GoalX
opp = PosY(0) - GoalY
LastProgress = Progress
Progress = sqrt(adj*adj + opp*opp)
if (Progress < BestProgress) BestProgress = Progress
return
label AI
Ctrl(0) = peek("port1")
if (Progress <= 10.0) then
// Success
for b = 1 to bots
adj = PosX(b) - PosX(0)
opp = PosY(b) - PosY(0)
hyp = sqrt(adj*adj + opp*opp)
if (hyp < 3.0*Radi(b)) then
// Move away from ball
Ctrl(b) = DN
else
// Stay away from ball
Ctrl(b) = 0
endif
next b
return
else
Winner = 0
Solution = 1000
// Reset progress towards solution
for b = 1 to bots
Ctrl(b) = 0
//
// Viewing angle of ball
px = PosX(b)
py = PosY(b)
tx = PosX(0)
ty = PosY(0)
vx = cos(RotZ(b)) + PosX(b)
vy = sin(RotZ(b)) + PosY(b)
SeeBall(b) = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
//
// Viewing angle of goal
vx = cos(RotZ(b)) + PosX(b)
vy = sin(RotZ(b)) + PosY(b)
SeeGoal(b) = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
//
// Distance to ball
adj = PosX(b) - PosX(0)
opp = PosY(b) - PosY(0)
NearBall(b) = sqrt(adj*adj + opp*opp)
adj = PosX(b) - GoalX
opp = PosY(b) - GoalY
NearBall(b) = NearBall(b)/sqrt(adj*adj + opp*opp)
//
// Calculate which bot is closest to the solution
Solved(b) = (NearBall(b)*SeeGoal(b))/SeeBall(b)
if (Solved(b) < Solution) then
Solution = Solved(b)
Winner = b
endif
next b
endif
for b = 1 to bots
// Look at ball using Crossproduct
px = PosX(b)
py = PosY(b)
tx = PosX(0)
ty = PosY(0)
vx = cos(RotZ(b)+pi/4) + PosX(b)
vy = sin(RotZ(b)+pi/4) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP > 0.0) then
vx = cos(RotZ(b)-pi/4) + PosX(b)
vy = sin(RotZ(b)-pi/4) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP < 0.0) then
vx = cos(RotZ(b)) + PosX(b)
vy = sin(RotZ(b)) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP < 0.0) Ctrl(b) = Ctrl(b) + RZ
if (CP > 0.0) Ctrl(b) = Ctrl(b) + LZ
else
Ctrl(b) = Ctrl(b) + LZ
endif
else
Ctrl(b) = Ctrl(b) + RZ
endif
// Move towards ball using dot product
adj = PosX(b) - PosX(0)
opp = PosY(b) - PosY(0)
hyp = sqrt(adj*adj + opp*opp)
if (hyp > 8.0*Radi(b)) then
vx = cos(RotZ(b)) + PosX(b)
vy = sin(RotZ(b)) + PosY(b)
DP1 = (vx-PosX(b))*adj + (vy-PosY(b))*opp
vx = cos(RotZ(b)+pi) + PosX(b)
vy = sin(RotZ(b)+pi) + PosY(b)
DP2 = (vx-PosX(b))*adj + (vy-PosY(b))*opp
if (abs(DP1) > abs(DP2)) then
if (DP1 < 0.0) Ctrl(b) = Ctrl(b) + UP
if (DP1 > 0.0) Ctrl(b) = Ctrl(b) + DN
else
if (DP2 < 0.0) Ctrl(b) = Ctrl(b) + LF
if (DP2 > 0.0) Ctrl(b) = Ctrl(b) + RT
endif
else
// Get the Goal in view using Crossproduct
px = PosX(b)
py = PosY(b)
tx = GoalX
ty = GoalY
vx = cos(RotZ(b)+pi/4) + PosX(b)
vy = sin(RotZ(b)+pi/4) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP > 0.0) then
vx = cos(RotZ(b)-pi/4) + PosX(b)
vy = sin(RotZ(b)-pi/4) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP < 0.0) then
if (Winner = b) then
vx = cos(RotZ(b)) + PosX(b)
vy = sin(RotZ(b)) + PosY(b)
CP = (vx-tx)*(py-ty) - (px-tx)*(vy-ty)
if (CP < 0.0) Ctrl(b) = Ctrl(b) + LF
if (CP > 0.0) Ctrl(b) = Ctrl(b) + RT
// Try to move the ball towards the goal
if (LastProgress > Progress) Ctrl(b) = Ctrl(b) + UP
if (LastProgress = Progress) Ctrl(b) = Ctrl(b) + UP
if (LastProgress < Progress) Ctrl(b) = Ctrl(b) + DN
endif
else
if (Winner = b) Ctrl(b) = Ctrl(b) + RT
endif
else
if (Winner = b) Ctrl(b) = Ctrl(b) + LF
endif
endif
next b
return
label Control
for b = 0 to bots
DirX(b) = 0.0
DirY(b) = 0.0
if (and(Ctrl(b), UP) = UP) DirY(b) = 0.5
if (and(Ctrl(b), DN) = DN) DirY(b) =-0.5
if (and(Ctrl(b), LF) = LF) DirX(b) =-0.5
if (and(Ctrl(b), RT) = RT) DirX(b) = 0.5
if (and(Ctrl(b), LZ) = LZ) RotZ(b) = RotZ(b) - pi/180.0
if (and(Ctrl(b), RZ) = RZ) RotZ(b) = RotZ(b) + pi/180.0
PosX(b) = PosX(b) + DirY(b)*cos(RotZ(b))
PosY(b) = PosY(b) + DirY(b)*sin(RotZ(b))
PosX(b) = PosX(b) + DirX(b)*cos(RotZ(b) + pi/2)
PosY(b) = PosY(b) + DirX(b)*sin(RotZ(b) + pi/2)
next b
for b = 0 to bots
for a = 0 to bots
if (a <> b) then
adj = PosX(b) - PosX(a)
opp = PosY(b) - PosY(a)
hyp = sqrt(adj*adj + opp*opp)
if (hyp < Radi(a) + Radi(b)) then
cs = adj/hyp
sn = opp/hyp
Radi = (Radi(a) + Radi(b)) / 2
CenterX = (PosX(a) + PosX(b))/2
CenterY = (PosY(a) + PosY(b))/2
PosX(a) = CenterX - Radi*cs
PosY(a) = CenterY - Radi*sn
PosX(b) = CenterX + Radi*cs
PosY(b) = CenterY + Radi*sn
endif
endif
next a
next b
for b = 1 to bots
if (PosX(b) < x1 + Radi(b)) PosX(b) = x1 + Radi(b)
if (PosY(b) < y1 + Radi(b)) PosY(b) = y1 + Radi(b)
if (PosX(b) > x2 - Radi(b)) PosX(b) = x2 - Radi(b)
if (PosY(b) > y2 - Radi(b)) PosY(b) = y2 - Radi(b)
next b
b = 0
if (PosX(b) < x3 + Radi(b)) PosX(b) = x3 + Radi(b)
if (PosY(b) < y3 + Radi(b)) PosY(b) = y3 + Radi(b)
if (PosX(b) > x4 - Radi(b)) PosX(b) = x4 - Radi(b)
if (PosY(b) > y4 - Radi(b)) PosY(b) = y4 - Radi(b)
return
label Draw
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
// The environment
setrgb 1, 064, 128, 064
fill box x1, y1 to x2, y2
setrgb 1, 255, 255, 255
box x3, y3 to x4, y4
// The goal
setrgb 1, 255, 064, 064
fill circle GoalX, GoalY, GoalR
setrgb 1, 128, 016, 016
circle GoalX, GoalY, GoalR
// The ball
setrgb 1, 255, 255, 064
fill circle PosX(0), PosY(0), Radi(0)
setrgb 1, 064, 064, 004
circle PosX(0), PosY(0), Radi(0)
// The AI bots
for b = 1 to bots
cs = Radi(b)*cos(RotZ(b))
sn = Radi(b)*sin(RotZ(b))
setrgb 1, 064, 064, 128
fill circle PosX(b), PosY(b), Radi(b)
setrgb 1, 016, 016, 064
line PosX(b), PosY(b) to PosX(b)+cs, PosY(b)+sn
circle PosX(b), PosY(b), Radi(b)
setrgb 1, 064, 064, 128
fill circle PosX(b), PosY(b), 10
setrgb 1, 016, 016, 064
text PosX(b)-5, PosY(b)+3, str$(b)
next b
// Debug Reads Progress Data
if (debug <> 0) then
setrgb 1, 255, 255, 255
text -300, -240, "Distance to Goal : " + str$(Progress)
text -300, -220, "Best Distance is : " + str$(BestProgress)
text 100, -240, "Solution : " + str$(Solution)
text 100, -220, "Winner : " + str$(Winner)
endif
return
