AI Tidy Up Teamwork

[rain_storm]

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.

Code: [Select]

// ======================================= //
// 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


[Shockwave]

It works well mate, I tried to triangulate the position of the ball and they always worked it out. Thats pretty cool stuff there.