Nurullah Akkaya: an explorer's log

Line Segment/Circle - Collision Detection

Here is another piece of code I didn't want to throw away, it uses vector projection to determine if a circle collides with a line segment. The idea was to figure out if one NPC can pass the ball to another in a way that no opposing team members can intercept it. We represent each opposing team player as a circle, the size of the circle will grow or shrink depending on the velocity of the player then we check each circle for collision with the line segment if non collides we assume it is a safe pass, following images shows a safe pass where non of the three opposing players can intercept the pass,

 (defn closest-point-on-line [a b c]
   (let [ac (subtract c a)
         ab (subtract b a)
         proj-mag (dot-product ac (normalize ab))]
     (cond (< proj-mag 0) a
           (> proj-mag (magnitude ab)) b
           :default (add (project ac ab) a))))

We begin by creating two new vectors, one from the start of the line to end of the line (AB) and one from start of the line to the center of the circle (AC), then we calculate the magnitude (length) of the projection of AC onto AB, if it is smaller than 0 then the closest point on this line to the circle is the point A (start of the line segment), if it is bigger than the magnitude of the AB vector then the closest point is B, else we return the projection of AC onto AB plus A (which converts it back into world coordinates) that gives us the closest point on the line to the circle.

 (defn path-clean [a b c r]
   (let [closest (closest-point-on-line a b c)
         distance (magnitude (subtract c closest))]
     (if (<= distance r)
       false true)))

Now that we know the location of the closest point on the line to the circle, collision detection is as simple as calculating the length between closest point and the circle, if it is smaller than the radius of the circle we have a collision else we have a clean path.

collision-detection.clj

Physics with Clojure

A brief demonstration of simulating mechanical things in Clojure using ODE4J,

 (defn world [g]
   (let [[x y z] g] 
     (doto (OdeHelper/createWorld)
       (.setGravity x y z))))

We begin by creating a world object which will hold all rigid bodies and joints and set the gravitational force that will act on our objects.

 (defn body [world [x y z]]
   (let [body (OdeHelper/createBody world)
         mass (doto (OdeHelper/createMass)
                (.setSphere 2500 0.05))] 
     (doto body
       (.setMass mass)
       (.setPosition x y z))))

For each object we want to simulate we create a body and set the parameters associated with it, i.e its shape and mass, for this simple example we create a body that behaves like a sphere with a density of 2500 and a radius of 0.05,

 (defn environment [world]
   (let [b1 (body world [1 2 0])
         b2 (body world [2 2 0])
         j1 (doto (OdeHelper/createHingeJoint world)
              (.attach b1 nil)
              (.setAnchor 0 2 0)
              (.setAxis 0 0 1)
              (.setParamVel 0)
              (.setParamFMax 30))
         j2 (doto (OdeHelper/createBallJoint world)
              (.attach b1 b2)
              (.setAnchor 1 2 0))] 
     [b1 b2 j2 j1]))

Next we create the simulation environment we will be using, we create two bodies and two joints forming two pendulums connected to each other, bottom joint is a ball joint which allows the body to move freely, upper joint is a hinge joint which is a motor driven joint that rotates around Z axis with an initial angular speed of 0 and 30 units of maximum force.

 (defn bang-bang [joint]
   (if (> (.getAngle joint) 2.5)
     (.setParamVel joint -0.1)
     (.setParamVel joint 2)))

In order to keep the pendulum at desired angle, we use a type of controller called a bang bang controller (on-off controller),

 (defn bang-bang [joint]
   (if (> (.getAngle joint) 2.5)
     (.setParamVel joint -0.1)
     (.setParamVel joint 2)))

All thats left to do is to paint the bodies to visualize the motion, on every tick we repaint the panel, calculate the velocity needed and let the engine step 0.05 seconds,

 (defn panel [world environment]
   (let [pos #(let [pos (.getPosition %)] [(.get0 pos) (.get1 pos)])
         coords #(vector (+ 100 (* 50 (first %))) (* 50 (second %)))
         circle #(let [rad 20 
                       offset (int (/ rad 2))
                       x (- %2 offset) 
                       y (- %3 offset)]
                   (.fill %1 (Ellipse2D$Double. x y rad rad)))]
     (proxy [JPanel ActionListener] [] 
       (paintComponent
        [g]
        (let [[b1 b2 _ j2] environment
              [x1 y1] (coords (pos b1))
              [x2 y2] (coords (pos b2))
              [lx1 ly1] (coords [0 2])]
          (.setColor g java.awt.Color/WHITE)
          (.fillRect g 0 0 (.getWidth this) (.getHeight this))
          (.setColor g java.awt.Color/BLACK)
          (.drawString g (apply str "Angle: " 
                                (take 5 (str (.getAngle j2)))) 20 20)
          (.drawLine g lx1 ly1 x1 y1)
          (circle g x1 y1)
          (.drawLine g x1 y1 x2 y2)
          (circle g x2 y2)))
       (actionPerformed [e] 
                        (.repaint this)
                        (bang-bang (last environment))
                        (.step world 0.05)))))

Files,

bang-bang.clj

Modifying Behaviors Using Decorators

My previous post on behavior trees made use of composite and action types, this posts serves as an example on how to improve behaviors using decorators. They take their name from the software design pattern. In the context of behavior trees a decorator node is a node with a single child, it modifies the behavior of the branch in some way, i.e. you don't want your NPC to keep kicking the door forever when it is blocked or replay an animation without completing the cycle.

The problem with my previous Robocode tree was that every time we execute it, it searched through the whole tree and as a consequence it kept switching targets, using decorators we can create loops in the tree that way we don't switch targets unless the robot we are going after dies.

Kill All sequence demonstrates two decorators used for looping, since at the beginning of the match we don't have any enemies to fight we scan around until we find a robot to kill, until-success decorator will keep executing its child until it returns success meaning we found at least one robot to fight. Next in the sequence is Attack branch, using until-fail decorator it will pick a target to attack, keep attacking until the enemy is dead then move on to a new target until select-target fails which means all the robots in the arena are dead, having loops in the tree allows us to execute the tree once unlike the previous example which kept executing the tree forever in a while loop.

In order to combat robots that track our movement, two non deterministic composite nodes exists which shuffle their children prior to execution such as the one used in the Strafe branch which gives us some degree of non determinism.

Controlling Robocode Engine from Clojure

Since Robocode is designed for Java it can get really annoying when you want to program your robots in Clojure, basically you need to keep a REPL for compiling and testing the robot and a shell for actually running Robocode. I was looking for a way to streamline the process, turns out you can control Robocode engine programmatically which allows us to run battles from REPL, this scheme isn't perfect but close.

 -Xmx512M -XX:MaxPermSize=512M

Robocode needs a lot memory to operate so depending on how you start your REPL you need to add the above options or you will keep getting OutOfMemoryException every 10 minutes.

[org.nakkaya.robocode/robocode "1.7.2.0"]

I have uploaded all the required Jars to Clojars, adding the above dependency to you project is all that is needed to get all the Jars you need, then you can use the following snippet to start Robocode engine from Clojure.

 (ns robocode-engine
   (:use clojure.contrib.shell-out)
   (:import (java.io File)
            (robocode.control 
             RobocodeEngine BattlefieldSpecification BattleSpecification)
            (robocode.control.events BattleAdaptor)))
 (System/setProperty "NOSECURITY" "true")

 (defn battle-console []
   (proxy [BattleAdaptor] []
     (onBattleMessage [e] (println "Msg> " (.getMessage e)))
     (onBattleError [e] (println "Err> " (.getError e)))
     (onBattleCompleted 
      [e] 
      (println "\n\n-- Battle Complete --\n")
      (doseq [result (.getSortedResults e)]
        (println (.getTeamLeaderName result) (.getScore result))))))

 (defn engine [dir vis?]
   (let [engine (doto (RobocodeEngine. (File. dir))
                  (.setVisible vis?)
                  (.addBattleListener (battle-console)))]
     (sh "jar" "xf" "lib/clojure-1.1.0.jar" "clojure")
     (sh "mv" "clojure" "classes/")
     engine))

 (defn battle [engine rounds size robots]
   (let [[x y] size
         field (BattlefieldSpecification. x y)]
     (BattleSpecification.
      rounds field
      (into-array
       (reduce (fn[h v]
                 (conj h (first (.getLocalRepository engine v))))
               [] robots)))))

 (defn run-battle
   ([engine rounds size robots & wait]
      (run-battle engine (battle engine rounds size robots))
      (if (not (nil? wait))
        (.waitTillBattleOver engine)))
   ([engine battle]
      (.runBattle engine battle false)))

 (defn close [engine]
   (.close engine)
   (sh "rm" "-rf" "classes/")
   (sh "mkdir" "classes"))

Now you can initialize the engine,

  (def engine (engine "/Applications/robocode/" true))

then you can compile and run your robot from REPL (assuming you robot is in the namespace gez),

  (do 
    (compile 'gez)
    (run-battle engine 3 [800 600] 
                ["sample.SittingDuck" 
                 "sample.Fire" 
                 "sample.Crazy"
                 "sample.RamFire" 
                 "gez*"]))

alter-ego - A Reactive AI Library

alter-ego is a reactive AI library based on the concept of behavior trees. Behavior trees combines a number of AI techniques such as Hierarchical State Machines, Scheduling, Planning, and Action Execution. Their strength comes from the fact that it is very easy to see logic, they are fast to execute and easy to maintain.

Behavior trees allow programmers to piece together reusable blocks of code which can be as simple as looking up a variable in game state to running an animation, then the behavior tree is used to control the flow and execution of these blocks of code.

As its name implies a behavior tree is a tree structure, made up of three types of nodes, action, decorator, composite. Composite and decorator nodes are used to control the flow within the three and action nodes are where we execute code they return success or failure and their return value is then used to decide where to navigate next in the tree.

In alter-ego actions are functions, if you are using the built in editor for composing trees an action is function that takes a single variable called blackboard this is what the behavior tree uses to communicate with the outside world, if you are programmatically building your tree, you can pass arbitrary number of variables. Actions are used to change state such as calculating a new path or shooting at the player.

Selector and sequence nodes are workhorse internal nodes. Selector node will try to execute its first child, if it returns success it will also return success if it fails it will try executing its next child until one of its children returns success or it runs out of children at which point it will return failure. This property allows us to choose which behavior to run next,

On the other hand a sequence represents a series of behaviors that we need to accomplish. A sequence will try to execute all its children from left to right, if all of its children succeeds sequence will also succeed, if one of its children fails sequence will stop and return failure.

With the above tree, root selector first checks attack sequence, attack in return executes action Player In Range? if it succeeds we pick a weapon, face the player and fire. If Player In Range? fails we stop execution and it causes attack sequence to fail and selector tries to execute Taunt sequence.

Thats enough theory to put together something that works,

This represents a very simple AI logic for a robocode bot, every time we execute the tree, we scan for others bots in the arena, pick one to attack, then we execute Move selector, if we are not too close we move forward, if we are too close forward sequence will fail and we fall back to back action. We want our robot to be always ready to fire so the first thing we do in Fire sequence is to turn the turret towards the target then we check if we are within gun range, if we are we fire, if we are not we bail out which takes us back to scan, whether we fire or not we always keep the gun locked on the target.

On the Clojure side, we only need to implement actions (green leaf nodes), rest is handled in the bundled behavior tree editor this is what makes behavior trees powerful, you can reorganize, cut, copy, paste nodes without changing a single line of code,

 (defn fire [blackboard]
   (from-blackboard blackboard [robot]
                    (.fire robot 3) true))

Actions are Clojure functions, they take one variable blackboard which is a reference holding a map. Blackboard is what nodes use to communicate with each other such as when an action picks a target it is written to the blackboard then another action reads that value and acts on it. When working with Java methods you need to keep in mind that return value of the action will be coerced to boolean that is what determines success or failure of the action when methods return null even when they succeed such as fire above you need to return true explicitly. from-blackboard is a convenience macro that will lookup the values of its bindings in the blackboard,

(from-blackboard blackboard [robot])

will expand to,

(let [robot (:robot @blackboard)])

After we define all our actions, all we need to do is load the behavior tree and execute it,

 (defn -run [robot]
   (setup robot)
   (let [tree (load-tree "/Users/nakkaya/Desktop/robocode/tank.bt" 
                         (.blackboard robot))]
     (forever (exec tree))))

Note that when loading the tree you can provide your own blackboard or one will be automatically provided for your nodes even if you don't plan on using it.

Files,

Finite State Machine Implementation in Clojure

A finite state machine is set of states, one being the start state, each state has a list of transitions, transitions in turn has conditions and actions whenever a condition for a transition is met FSM performs the action and enters the new state. FSM are widely used for game and robotic AI. Most games are just a bunch of FSMs running little chunks of code reacting to state changes. An NPC for example when instantiated can be in patrol state and as soon as the player approaches, it might cause it to transition into attack state which might cause it to run towards you.

 (defn state-machine [transition-table initial-state]
   (ref initial-state :meta transition-table))

A state machine is a ref holding the current state, transition table containing the list of states and transition rules are attached as meta data.

 (def traffic-light
      {:green [{:conditions [] :transition :yellow}]
       :yellow  [{:conditions [] :transition :red}]
       :red [{:conditions [] :transition :green}]})

Transition table is represented as a map containing states as keys and vector of maps containing condition, action and transition information.

 (defn- switch-state? [conds]
   (if (empty? conds)
     true
     (not (some false? (reduce #(conj %1 (if (fn? %2) (%2) %2)) [] conds)))))

 (defn- first-valid-transition [ts]
   (find-first #(= (second %) true)
               (map #(let [{conds :conditions 
                            transition :transition
                            on-success :on-success} %]
                       [transition (switch-state? conds) on-success]) ts)))

 (defn update-state [state]
   (let [transition-list ((meta state) @state)
         [transition _ on-success] (first-valid-transition transition-list)]
     (if-not (nil? transition)
       (do 
         (if-not (nil? on-success)
           (on-success))
         (dosync (ref-set state transition))))))

Every time we try to update state machines state, first we get the list of transition rules for the current state, then we start checking conditions for transition in the order they appear in the vector first transition that returns true for all its conditions is picked, if it has a on-success function it will be executed and reference will be set to the new state.

 (let [sm (state-machine traffic-light :green)] 
   (dotimes [_ 4]
     (println @sm)
     (update-state sm)))

 state-machine.core=> :green
 :yellow
 :red
 :green

Above example shows how traffic light state machine iterates through its states. A more complicated and famous example is a find-lisp state machine that would search for the word lisp in a character sequence,

 (defn find-lisp [char-seq]
   (let [start-trans {:conditions []
                      :on-success #(pop-char char-seq)
                      :transition :start}
         found-l-trans {:conditions [#(= (first @char-seq) \l)] 
                        :on-success #(pop-char char-seq)
                        :transition :found-l}]

     {:start [found-l-trans
              start-trans]

      :found-l [found-l-trans
                {:conditions [#(= (first @char-seq) \i)] 
                 :on-success #(pop-char char-seq)
                 :transition :found-i}
                start-trans]

      :found-i [found-l-trans
                {:conditions [#(= (first @char-seq) \s)] 
                 :on-success #(pop-char char-seq)
                 :transition :found-s}
                start-trans]

      :found-s [found-l-trans
                {:conditions [#(= (first @char-seq) \p)] 
                 :on-success #(do (println "Found Lisp")
                                  (pop-char char-seq))
                 :transition :start}
                start-trans]}))

When we run it, it will print Found Lisp every time we find the sequence of characters l,i,s,p in this particular order,

 (let [char-seq (ref "ablislasllllispsslis")
       sm (state-machine (find-lisp char-seq) :start)] 
   (dotimes [_ (count @char-seq)]
     (update-state sm)))

 state-machine.core=> Found Lisp

Even though it is not designed for this but Vijual works for quick and dirty visualization of state machines,

   (use 'vijual)
   (do (println )
       (draw-graph (prepare-nodes (state-machine traffic-light :start))))

 state-machine.core=> 
 +--------+   +-------+
 |        |   |       |
 | yellow |---|       |
 |        |   | green |
 +--------+   |       |
   |          |       |
   |          +-------+
   |            |
   |   +--------+
   |   |
 +-----+
 | red |
 +-----+

state-machine.clj

Clojure I/O Cookbook

There are a number of ways for us to read a file. If the file is small enough and can be held in memory, simplest approach is to use slurp which will return a string containing the content of the file,

(slurp "some.txt")

For files that you can't or don't want to hold in memory, we can use BufferedReader, line-seq combo and process files on a line by line basis,

 (with-open [rdr (java.io.BufferedReader. 
                  (java.io.FileReader. "project.clj"))]
   (let [seq (line-seq rdr)]
     (count seq)))

These days it is more common (at least for me) to retrieve a URL then it is to read file,

 (defn fetch-url[address]
   (with-open [stream (.openStream (java.net.URL. address))]
     (let  [buf (java.io.BufferedReader. 
                 (java.io.InputStreamReader. stream))]
       (apply str (line-seq buf)))))

 (fetch-url "http://google.com")

Above will work on text files but corrupt binary files because BufferedReader assumes it is dealing with textual data, for downloading a binary file (video, music etc.) and saving it to a file on disk,

 (defn fetch-data [url]
   (let  [con    (-> url java.net.URL. .openConnection)
          fields (reduce (fn [h v] 
                           (assoc h (.getKey v) (into [] (.getValue v))))
                         {} (.getHeaderFields con))
          size   (first (fields "Content-Length"))
          in     (java.io.BufferedInputStream. (.getInputStream con))
          out    (java.io.BufferedOutputStream. 
                  (java.io.FileOutputStream. "out.file"))
          buffer (make-array Byte/TYPE 1024)]
     (loop [g (.read in buffer)
            r 0]
       (if-not (= g -1)
         (do
           (println r "/" size)
           (.write out buffer 0 g)
           (recur (.read in buffer) (+ r g)))))))

 (fetch-data "http://google.com")

Or if you prefer interacting with the socket directly,

 (defn socket [host port]
   (let [socket (java.net.Socket. host port)
         in (java.io.BufferedReader. 
             (java.io.InputStreamReader. (.getInputStream socket)))
         out (java.io.PrintWriter. (.getOutputStream socket))]
     {:in in :out out}))

 (def conn (socket "irc.freenode.net" 6667))
 (println (.readLine (:in conn)))

Now for writing stuff back to disk,

 (require 'clojure.contrib.duck-streams)
 (clojure.contrib.duck-streams/spit "output.txt" "test")

If you don't want to depend on the contrib library you can bind *out* to a FileWriter and print the content,

 (binding [*out* (java.io.FileWriter. "some.dat")]
   (prn {:a :b :c :d}))

Motor Control via Ardumoto Using Arduino and Clodiuno

Here is another post that is partly for documentation and partly for safekeeping. Following snippet which I ripped from another robot we have been working on, demonstrates how to control the Ardumoto motor driver shield using Clodiuno.

Control for motor attached to OUT 1-2 is connected to digital line 12 (direction) and digital line 10 (PWM). Control for motor attached to OUT 3-4 is connected to digital line 13 (direction) and digital line 11 (PWM).

 (ns ardumoto.core
   (:use [clojure.contrib.swing-utils :only [add-action-listener]])
   (:use :reload-all clodiuno.core)
   (:import (javax.swing JFrame JPanel JButton)
            (net.miginfocom.swing MigLayout)))

 (def pwm-pin-motor-a 10)
 (def pwm-pin-motor-b 11)
 (def dir-pin-motor-a 12)
 (def dir-pin-motor-b 13)

 (def speed 200)

 (defn board []
   (arduino "/dev/tty.usbserial-A6008nhh"))

 (defn init-pins [board]
   (pin-mode board pwm-pin-motor-a PWM)
   (pin-mode board pwm-pin-motor-b PWM)
   (pin-mode board dir-pin-motor-a OUTPUT)
   (pin-mode board dir-pin-motor-b OUTPUT))

 (defn motor [board m p]
   (let [motor-pin (if (= m :a) pwm-pin-motor-a pwm-pin-motor-b)
         dir-pin   (if (= m :a) dir-pin-motor-a dir-pin-motor-b)
         pwm   (if (< p 0) (* -1 p) p)
         dir   (if (< p 0) LOW HIGH)]
     (analog-write board motor-pin pwm)
     (digital-write board dir-pin dir)))

 (defn listener [press-f release-f]
   (proxy [java.awt.event.MouseListener] [] 
     (mousePressed [e] (press-f))
     (mouseReleased [e] (release-f))
     (mouseClicked [e])
     (mouseEntered [e])
     (mouseExited [e])))

 (defn panel []
   (let [arduino (ref nil)
         panel (JPanel. (MigLayout.))
         left (JButton. "Left")
         right (JButton. "Right")
         forward (JButton. "Forward")
         backward (JButton. "Backward")
         connect  (JButton. "Connect")
         init  (JButton. "Init")
         set-speed #(do (motor @arduino :a %1) (motor @arduino :b %2))
         stop #(do (motor @arduino :a 0) (motor @arduino :b 0))]

     (add-action-listener connect (fn[_] (dosync (ref-set arduino (board)))))
     (add-action-listener init (fn[_] (init-pins @arduino)))

     (.addMouseListener left (listener #(set-speed (* -1 speed) speed) stop))
     (.addMouseListener right (listener #(set-speed speed (* -1 speed)) stop))
     (.addMouseListener forward (listener #(set-speed speed speed) stop))
     (.addMouseListener 
      backward (listener #(set-speed (* -1 speed) (* -1 speed)) stop))

     (doto panel
       (.add forward "cell 3 1")
       (.add backward "cell 3 3")
       (.add left "cell 2 2")
       (.add right "cell 4 2")
       (.add connect "cell 2 4")
       (.add init "cell 4 4"))))

 (defn frame []
   (let [panel (panel)]
     (doto (JFrame.)
       (.add panel)
       (.pack)
       (.setVisible true))))

Path Finding Using A-Star in Clojure

For a recent project, I had to implement A* (A-Star) in Clojure, since it's a very popular path finding algorithm used in gaming I thought it might be interesting to other clojurians too.

AStar uses best-first search to find the the least-cost path from a given initial node to one goal node (out of one or more possible goals). Functions,

g(x) - cost of getting to that node from starting node.
h(x) - cost of getting to the goal node from current node.
f(x) - g(x)+h(x)

are used to determine the order in which search visits nodes. Beginning with the start node, we keep track of two lists, open and closed, open list contains the list of nodes to traverse sorted by their f(x) cost, closed list contains the list of nodes that we have processed. At each step algorithm removes the first node on the open list, calculate f(x), g(x) and h(x) values for its neighbors and add the ones that are not on the closed list to the open list. This is done until goal node has been found or no nodes are left on the open list.

In a nutshell we will,

Add the starting node to the open list.
Loop
- Remove the node with the lowest f(x) from the open list.
- Add it to closed list.
- Calculate 8 adjacent squares.
- Filter neighbors that are not on the closed list and walkable.
- For each square
  - If it is not on the open list, calculate F, G and H costs, make the current square parent of this square and add it open list.
  - If it is on the open list, check to see if this path to that square is better using the G cost, a lower G indicates a better path if so change its parent to this square and recalculate F G and H costs.
Until
- Target node is added to the closed list indicating a path has been found.
- No more nodes left in the open list indicating there is no path between nodes.

Before diving into code, let me start with a disclaimer, my first priority was clarity so this is not the most efficient implementation, for my use case it can find the path around 20-30 milliseconds which is good enough for me but there are tons of stuff you can do to improve performance if you need to.

   (def maze1 [[0 0 0 0 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 0 0 0 0]])

Surface is represented using a 2D vector of 0s and 1s, 0 denoting walkable nodes and 1 denoting non walkable nodes.

 (defn node 
   ([x y] {:x x :y y})
   ([x y f g h] {:x x :y y :f f :g g :h h}))

Each node is represented by a map, which holds the information about its position and F, G, H costs associated with it.

 (defn manhattan-distance [[x1 y1] [x2 y2]]
   (+ (Math/abs (- x2 x1)) (Math/abs (- y2 y1))))

 (defn update-cost [{:keys [x y]} start end]
   (let [g (manhattan-distance start [x y])
         h (manhattan-distance [x y] end)
         f (+ g h)] 
     (node x y f g h)))

Quality of the path found will depend on the distance function used to calculate F, G, and H costs, for this implementation I choose to use Manhattan distance since it is cheaper to calculate then Euclidean distance but keep in mind that different distance metrics will produce different paths so depending on your condition expensive metrics can produce more natural looking paths.

 (defn search 
   ([map start end]
      (let [[x y] start
            open [(update-cost (node x y) start end)]
            closed []]
        (search (grid map open closed start end))))
   ([grid]
      (if (has-next? grid)
        (if-not (done? grid)
          (let [node (head grid)
                grid (remove-head grid)]
            (recur (add-nodes grid node (edges grid node))))
          (path (head grid) (remove-head grid))))))

Search function is where it all happens and it pretty much summarizes all of the above steps. First we create two vectors that will hold our open and closed lists then add the starting node to open list, and call search, we will keep calling search until no elements are left on the open list or first node on the open list is our goal node. Unless we are done we remove the first item on the open list, put it to closed list and process nodes around it.

 (defn edges [grid n]
   (let [{map :map} grid
         {x :x y :y} n
         edges (for [tx (range (- x 1) (+ x 2)) 
                     ty (range (- y 1) (+ y 2))] (node tx ty))
         max-x (dec (count (first map)))
         max-y (dec (count map))]
     (filter #(let [{nx :x ny :y} %]
                (cond (or (< nx 0) (< ny 0)) false
                      (or (> nx max-x) (> ny max-y)) false
                      (= [x y] [nx ny]) false
                      (= (nth (nth map ny) nx) 1) false
                      (closed? grid %) false
                      :default true)) edges)))

For each node we are examining, we need to build a list of nodes around it. We filter them by checking if the node contains a 1 in its place on the map which means we can't go over it or it is already in the closed list which means we have already looked at it.

 (defn add-node [g p n]
   (let [{open :open start :start end :end} g
         {parent-x :x parent-y :y} p]
     (if-not (open? g n)
       (let [n (assoc (update-cost n start end) :p {:x parent-x :y parent-y})]
         (assoc g :open (conj open n)))
       (let [pn (get-node g n)
             open (filter #(not (eq? n %)) open)
             n (assoc (update-cost n start end) :p {:x parent-x :y parent-y})]
         (if (< (:g n) (:g pn))
           (assoc g :open (conj open n)) g)))))

 (defn add-nodes [grid parent nodes]
   (let [grid (reduce (fn[h v] (add-node h parent v)) grid nodes)]
     (assoc grid :open (sort-by :f (:open grid)))))

After we get the list of adjacent nodes, they need to be added to the open list for further exploration, for nodes that are not on the open list, we calculate their costs and append them to the open vector, for nodes that are already on the open list, we check which one, the one on the open list or the one we just calculated has a lower G cost if the new one has a lower G cost we replace the one on the list with the new one. After we add all the nodes, we resort the open list using F cost.

 (defn parent [g n]
   (let [{closed :closed} g
         {{x :x y :y} :p} n]
     (find-first #(eq? % (node x y)) closed)))

 (defn path [end grid]
   (let [nodes (loop [path [end]
                      p (parent grid end)]
                 (if (nil? p)
                   path
                   (recur (conj path p) (parent grid p))))]
     (reverse (map #(vector (% :x) (% :y)) nodes))))

When we hit our target node, we need to work backwards starting from target node, go from each node to its parent until we reach the starting node. That is our path.

   (def maze1 [[0 0 0 0 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 0 0 0 0]])

   (draw-map maze1 [1 2] [5 2])

 astar.core=> "Elapsed time: 10.938 msecs"
 (      X      )
 (    X # X    )
 (  X   #   X  )
 (      #      )
 (             )

   (def maze2 [[0 0 0 0 0 0 0]
               [0 0 1 1 1 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]
               [0 0 0 1 0 0 0]])

   (draw-map maze2 [1 3] [5 2])

 astar.core=> "Elapsed time: 10.162 msecs"
 (    X X X    )
 (  X # # # X  )
 (    X #   X  )
 (  X   #      )
 (      #      )

   (def maze3 [[0 1 0 0 0 1 0]
               [0 1 0 1 0 1 0]
               [0 1 0 1 0 1 0]
               [0 1 0 1 0 1 0]
               [0 0 0 1 0 0 0]])

   (draw-map maze3 [0 0] [6 0])

 astar.core=> "Elapsed time: 8.98 msecs"
 (X #   X   # X)
 (X # X # X # X)
 (X # X # X # X)
 (X # X # X # X)
 (  X   #   X  )

   (def maze4 [[0 0 0 0 0 0 0 0]
               [1 1 1 1 1 1 1 0]
               [0 0 0 1 0 0 0 0]
               [0 0 0 1 0 0 0 0]
               [0 0 0 1 0 0 0 0]
               [0 0 0 1 1 1 0 1]
               [0 0 0 0 0 1 0 1]
               [0 0 0 0 0 1 0 1]
               [0 0 0 0 0 0 0 1]
               [1 1 1 1 0 1 1 1]
               [0 0 0 1 0 0 0 0]
               [0 0 0 1 0 0 0 0]
               [0 0 0 0 0 0 0 0]])

   (draw-map maze4 [0 0] [0 12])

 astar.core=> "Elapsed time: 20.136 msecs"
 (X X X X X X X  )
 (# # # # # # # X)
 (      #     X  )
 (      #   X    )
 (      #   X    )
 (      # # # X #)
 (          # X #)
 (          # X #)
 (          X   #)
 (# # # # X # # #)
 (      # X      )
 (      # X      )
 (X X X X        )

Download code.

References

jMonkeyEngine Hello World in Clojure

I was experimenting with jMonkeyEngine in order to visualize a simulation, later I scrapped the idea thinking it was going to take way to much time just to make it look cool and switched to a 2D engine. I had already ported their Hello World example to Clojure so I am posting it here to give anyone who want to play with jMonkeyEngine a head start, I have uploaded all necessary jars to Clojars, use lein to grab everything you need,

 (defproject monkey-test "1.0.0-SNAPSHOT"
   :description "FIXME: write"
   :dependencies [[org.clojure/clojure "1.1.0"]
                  [org.clojure/clojure-contrib "1.1.0"]
                  [org.clojars.nakkaya.jmonkeyengine/jme "2.0.1"]]
   :native-dependencies [[penumbra/lwjgl "2.4.2"]]
   :dev-dependencies    [[native-deps "1.0.0"]])

All tutorials on jMonkeyEngine wiki, makes use of SimpleGame class, we add stuff to our scene by attaching objects to rootNode of SimpleGame which is defined as protected hence we can't use this in Clojure, we don't have access to protected fields when using proxy so below example uses StandartGame instead,

 (ns monkey-test.core
   (:import (com.jmex.game StandardGame)
            (com.jmex.editors.swing.settings GameSettingsPanel)
            (com.jmex.game.state DebugGameState)
            (com.jmex.game.state GameStateManager)
            (com.jme.scene.shape Box)
            (com.jme.math Vector3f)
            (com.jme.bounding BoundingSphere)))

 (defn standart-game []
   (let [game (StandardGame. "A Simple Test")]
     (GameSettingsPanel/prompt (.getSettings game))
     (.start game)))

 (defn add-box [state]
   (let [box (Box. "Mybox" (Vector3f. 0 0 0) (Vector3f. 1 1 1))] 
     (.setModelBound box (BoundingSphere. ))
     (.updateModelBound box)
     (.updateRenderState box)
     (.attachChild (.getRootNode state) box)
     (.updateRenderState (.getRootNode state))))

 (defn -main []
   (let [game (standart-game)
         state (DebugGameState.)]
     (add-box state)
     (.attachChild (GameStateManager/getInstance) state)
     (.setActive state true)))