Seiten

Mittwoch, 9. November 2011

F#. A* Star Pathfinding with Jump Point Search.



Das gesamte Projekt ist auf dem GitHub unter "Maze-Generator-and-Maze-Solver".

"Jump Point Search" Algorithmus ist ein sehr interessanter, einfacher und effizienter Algorithmus, der die A * Suche dadurch beschleunigt, dass letztendlich weniger Nodes besucht wird. Ich kann nur empfehlen den Blog-Eintrag und die ausführliche Beschreibung zu lesen, da die ganze Deatils zur Implementierung der Algorithmus dort ganz gut erklärt sind.
Es gibt bereits eine C++ Implementierung.

Die angepasste A* Suche Funktion.
//JumpPointsSearch.fs
...
   // return all jump points with parents and costs.  seq<jumpPoint   * (parent      * cost)> 
  //astarJump : int * int -> JumpPointEnvironment -> seq<(int * int) * ((int * int) * float)>
  let inline astarJump start env = 
      let inner (seen, q)  =
           match PriorityQueue.isEmpty q with
           | true -> failwith "No Solution."
           | false ->
               let ((currentCosts, next), dq) = PriorityQueue.deleteFindMin q
               let expanded, parent = next
               if currentCosts = 0.0 then None
               else 
                   match env.isGoal expanded with
                   | true -> Some ((expanded, (parent, currentCosts)),(seen, PriorityQueue.singleton 0.0 (expanded, expanded)))
                   | otherwise -> 
                       let succs = successors env.rooms expanded
                       let dir = directionToParent parent expanded
                       let jumpPoints= findJumpPoints env expanded dir succs  |> Set.ofSeq
 
                       let costs target = currentCosts + (env.stepCosts expanded target)  
                                            + (env.heuristic target) - (env.heuristic expanded) 

                       let q' = 
                           Set.difference jumpPoints seen |> Seq.map (fun a -> costs a, (a, expanded)) 
                           |> PriorityQueue.ofSeq |> PriorityQueue.merge dq
                       Some ((expanded, (parent, currentCosts)), ((Set.union seen jumpPoints), q'))
                   
      Seq.unfold inner ((Set.singleton start), (PriorityQueue.singleton (env.heuristic start) (start,start))) 
Typen und Hilfsfunktionen für das Jump Point Search Verfahren.
//
module JumpPointSearchType =
  open MazeType

  type StraightDirection = N  | S | E  | W  
  type DiagonalDirection = NE | NW | SE | SW
   
  type Direction =
    | Straight of StraightDirection * Cell
    | Diagonal of DiagonalDirection * Cell
    | NONE

  type JumpPointEnvironment = 
    { grid: Map<int * int, Direction list>;  //cells with avialiable Directions.
      w : int; h : int;     // Weight and Height
      isGoal : int * int -> bool;
      heuristic : int * int -> float;
      stepCosts : int * int -> int * int -> float}
        member x.successors point = 
          set[for direction in Map.find point x.grid  do
                  yield direction
              ]

  let empty = { grid = Map.empty; w = 20; h = 20; isGoal = (fun _ -> true);
                heuristic = (fun _ -> 0.0)
                stepCosts = (fun _ _-> 0.0)
               }

  let inline straightPosition direction =
    match direction with
    | N -> (0, -1)
    | S -> (0, 1)
    | E -> (1, 0)
    | W -> (-1, 0)
  
  let diagonalPosition direction =
    match direction with
    | NE -> (1,  -1)
    | SE -> (1,   1)
    | SW -> (-1,  1)
    | NW -> (-1, -1)
  
  let inline straight  direction = Straight (direction, straightPosition direction)
  let inline diagonal  direction = Diagonal (direction, diagonalPosition direction)

  type NotRule = Not of Direction

  let inline notRule direction = Not (straight direction)
  
  type PruningRule = 
      | StraightRule of (NotRule * Direction) 
      | DiagonalRule of Direction
  
  let inline straightRule notrule direction  = StraightRule (notrule, diagonal direction) 
  let inline diagonalRule direction  = DiagonalRule (straight direction)

  let inline flip f a b = f b a

  let inline inGrid w h cell =
            match cell with
            | x, y when (0 <= x && x <= w - 1 && 0 <= y && y <= h - 1) -> true
            | _  -> false
Jump Point Search Algorithm.
//  findJumpPoints : JumpPointEnvironment -> int * int -> Direction -> Set<Direction> -> (int * int) list
  let inline findJumpPoints env (x, y) direction neighbours  =
      let find naturalNeighbours forcedNeighboursRules =
          naturalNeighbours @ 
              (forcedNeighboursRules
               |> List.filter (not << flip Set.contains neighbours << fst)
               |> List.map snd)
          |> List.choose (jump env x y)
      //Neighbour Pruning Rules
      match direction with
      | Straight(N, _) -> 
          // add S neighbour to the pruned set of neighbours.
          // add SE neighbour only if E neighbour is obstacle.
          // add SW neighbour only if W neighbour is obstacle.
          find [straight S]
                    (List.zip   [straight E;     straight W] 
                                [diagonal SE;    diagonal SW])

      | Diagonal(NE, _) -> 
          find [diagonal SW; straight S; straight W]
                    (List.zip   [straight N;     straight E] 
                                [diagonal NW;    diagonal SE])

      | Straight(E, _) ->
          find [straight W]
                    (List.zip   [straight N;     straight S] 
                                [diagonal NW;    diagonal SW])
      | Straight(S, _) -> 
          find [straight N] 
                    (List.zip   [straight E;     straight W] 
                                [diagonal NE;    diagonal NW])
      | Diagonal(SE, _) -> 
          find [diagonal NW; straight N; straight W]   
                    (List.zip   [straight S;     straight E] 
                                [diagonal SW;    diagonal NE])
      | Straight(W, _) -> 
         find [straight E]           
                    (List.zip   [straight N;     straight S] 
                                [diagonal NE;    diagonal SE])
      | Diagonal(SW, _) -> 
          find [diagonal NE; straight N; straight E]   
                    (List.zip   [straight S;     straight W] 
                                [diagonal SE;    diagonal NW])
      | Diagonal(NW, _) -> 
          find [diagonal SE; straight S; straight E]
                   (List.zip   [straight N;     straight W] 
                               [diagonal NE;    diagonal SW])
      // return all neighbours as the pruned set of neighbours.
      | NONE -> directionsToPoints neighbours (x, y) |> Set.toList
Details
module JumpPointsSearch =
  open Microsoft.FSharp.Collections
  open Astar
  open JumpPointSearchType
  
  let sqrtTWO = 1.414213562
  
  let inline diagHeuristic (x1, y1) (x2, y2) =
    let diagonal = min (abs(x1 - x2)) (abs (y1 - y2)) |> float
    let straight = (abs (x1 - x2)) + (abs (y1 - y2)) |> float
    sqrtTWO * diagonal + (straight - 2.0 * diagonal)


  let inline stepCosts (x1,y1) (x2,y2) = 
        let xa, ya = abs(x1-x2), (abs(y1-y2))
        (sqrtTWO - 1.0) * (min xa ya |> float) + (max xa ya |> float) 

  // move with turning points rules in direction jumpDirection.
  // recursively apply the straight pruning rule or
  // the diagonal pruning rule.
  // jump : JumpPointEnvironment -> int -> int -> Direction -> (int * int) option
  let rec jump env x y jumpDirection =
          let generateSteps dx dy notObstacle =
                (x, y)
                |> Seq.unfold (fun cell -> 
                                    let nextCell = MazeUtils.addPoint cell (dx, dy)
                                    if inGrid env.w env.h nextCell && notObstacle cell then 
                                        Some(nextCell, nextCell) 
                                    else None) 
          let directionSteps direction = 
                match direction with
                | NONE -> Seq.empty
                | Straight(_,(dx, dy)) -> generateSteps dx dy (Set.contains direction << env.successors )
                | Diagonal(_,(dx, dy)) -> generateSteps dx dy (Set.contains direction << env.successors )          
          
          let move direction directionRules =
              //apply the pruning rules.
              let applayRules rules func (px, py) = 
                    Seq.map (fun rule -> 
                                    match rule with
                                    | StraightRule(Not a, b) -> (not <| func a) && func b 
                                    | DiagonalRule dir -> jump env px py dir |> Option.isSome) rules |> Seq.reduce (||)
              //all available steps in current direction.
              let steps = directionSteps direction 
              //try to find jump point p. 
              steps
              |> Seq.tryFind (fun p ->
                    env.isGoal p || applayRules directionRules (flip Set.contains (env.successors p)) p)
                        
          match jumpDirection with
          | NONE -> None
          
          | Straight(N, _) as dir -> 
            //(x, y) is a jump point if a NW neighbour exists which cannot be                                  
            // reached by a shorter path than one involving (x, y) or with other words W is obstacle or
            // if NE and not E
                                     move dir [straightRule (notRule W) NW; straightRule (notRule E) NE]

          | Straight(S, _) as dir -> move dir [straightRule (notRule W) SW; straightRule (notRule E) SE] 

          | Straight(E, _) as dir -> move dir [straightRule (notRule S) SE; straightRule (notRule N) NE]

          | Straight(W, _) as dir -> move dir [straightRule (notRule S) SW; straightRule (notRule N) NW]
          
          | Diagonal(NE, _) as dir -> 
            //(x, y) is a jump point if a SE neighbour exists which cannot be                                  
            // reached by a shorter path than one involving (x, y) or with other words S is obstacle or
            // if NW and not W or 
            // if we can reach other jump points by 
            // travelling vertically or horizontally.  
                                      move dir [straightRule (notRule S) SE; straightRule (notRule W) NW;
                                                diagonalRule N; diagonalRule E] 

          | Diagonal(SE, _) as dir -> move dir [straightRule (notRule W) SW; straightRule (notRule N) NE;
                                                diagonalRule S; diagonalRule E]
          | Diagonal(SW, _) as dir -> move dir [straightRule (notRule N) NW; straightRule (notRule E) SE;
                                                diagonalRule S; diagonalRule W]
          | Diagonal(NW, _) as dir -> move dir [straightRule (notRule E) NE; straightRule (notRule S) SW;
                                                diagonalRule N; diagonalRule W]
  
  // directionsToPoints : Set<Direction> -> int * int -> Set<int * int>
  let inline directionsToPoints directions (x, y)=
      let inner d = 
              match d with
              | Straight(_, (dx, dy)) ->    x + dx, y + dy
              | Diagonal(_, (dx, dy)) ->    x + dx, y + dy
              | NONE -> failwith "failed to determine direction."
      Set.map inner directions
Ausführen.
...
    open Maze.JumpPointSearchType
    open Maze.JumpPointsSearch

    type MazeEnvironment = 
        { maze : JumpPointEnvironment; obstacles : Set<(int * int)>; 
          wallSize : float; coinX : float; coinY : float; targetX : float; targetY : float}
        member this.IsEmpty = Map.isEmpty <| this.maze.grid

    let empty = { maze = empty; obstacles = Set.empty;
                  wallSize = 20.0; coinX = 0.0; coinY = 0.0;targetX = 0.0; targetY = 0.0 }
    // Create the grid from the obstacles set.
    // mapObstaclesToGrid : JumpPointEnvironment -> Set<int * int> -> JumpPointEnvironment
    let inline mapObstaclesToGrid mazeEnv obstacles =
        
        let notObstacleDiagonal straight pos =
            let isInGrid = List.forall (inGrid mazeEnv.w mazeEnv.h)  (pos :: straight)
            match isInGrid with
            | true -> not <| Set.contains pos obstacles && Set.intersect (Set.ofList straight) obstacles |> Set.count < 2
            | _  ->   false

        let notObstacle cell = 
            match inGrid  mazeEnv.w mazeEnv.h cell with
            | true -> not <| Set.contains cell obstacles 
            | false -> false

        let mkWall (x, y) =
            let add =  addPoint (x, y)
            (x,y), [straight W,   straightPosition W |> add |> notObstacle;
                    straight N,   straightPosition N |> add |> notObstacle;
                    straight E,   straightPosition E |> add |> notObstacle; 
                    straight S,   straightPosition S |> add |> notObstacle; 
                    diagonal NW,  diagonalPosition NW |> add |> notObstacleDiagonal [straightPosition N |> add; 
                                                                                     straightPosition W |> add ]; 
                    diagonal NE,  diagonalPosition NE |> add |> notObstacleDiagonal [straightPosition N |> add;
                                                                                     straightPosition E |> add ]; 
                    diagonal SW,  diagonalPosition SW |> add |> notObstacleDiagonal [straightPosition S |> add;
                                                                                     straightPosition W |> add ]; 
                    diagonal SE,  diagonalPosition SE |> add |> notObstacleDiagonal [straightPosition S |> add;
                                                                                     straightPosition E |> add ]]
            |> List.filter (id << snd)
            |> List.map fst
        {mazeEnv with 
            grid = Seq.map mkWall 
                        [ for x in [0..mazeEnv.w-1] do
                            for y in [0..mazeEnv.h-1] do
                            yield x, y] |> Seq.toList |> Map.ofList }
    
    // run : MazeEnvironment -> seq<(int * int) * ((int * int) * float)>    
    let run env = 
        let jumpPointEnv = mapObstaclesToGrid env.maze env.obstacles
        let start = env.coinX / env.wallSize |> int, env.coinY / env.wallSize |>int
        let finish = env.targetX / env.wallSize |> int, env.targetY / env.wallSize |> int
        astarJump start { jumpPointEnv with isGoal = ((=) finish); stepCosts = stepCosts;  heuristic = (diagHeuristic finish) }

    // jump points  seq<jumpPoint   * (parent      * cost)>  to path of points list.
    // resultPath : seq<(int * int) * ((int * int) * float)> -> (int * int) list
    let inline resultPath jumpPoints =
        jumpPoints
        |> Seq.groupBy (fst)
        |> Seq.map (fun (key, s)-> key, Seq.minBy (snd << snd) s |> snd |> fst) 
        |> Seq.toList |> List.rev
        |> List.fold (fun acc (curr, parent) -> 
                        match acc with
                        | [] -> [parent;curr;]
                        | x :: _ when x = curr-> parent :: acc
                        | _ -> acc) []
    
    let inline animatePath jumpPoints = jumpPoints |> Seq.map (fun (curr, (parent, _)) -> curr, parent, directionToParent curr parent)
Ehrlich gesagt habe ich die meiste Zeit mit WPF verbracht, um die halbwegs brauchbare Algorithmus-Animation zu erstellen.