问在给定的图中找到“足够短”的路径

EN

namespace Astar
{
  struct CostEvaluation
  {
    int distance_cost;
    int transfer_cost;
    // others

    int costToTraverseNodes( const Node& first, const Node& second ) const
    {
      int distance = // apply distance_cost to distance between nodes
      int transfer = // apply transfer_cost if there is a transfer between nodes

      return distance + transfer;
    }
  }
}

module AStar =
    module Internals =
        let makeRoute (explo : Map<int,(int * float)>) at tgt =
            let rec loop at acc =
                let dst,c = explo.[at] 
                match at,dst with
                | (_,b) when b = tgt -> (at,c) :: acc
                | (_,b) -> loop b ((at,c) :: acc)
            [(tgt,0.0)] @ loop at [] 

        let makeRouteBackward (exploBW : Map<int, (int * float)>) at tgt =
            let rec loop at acc = 
                let src,c = exploBW.[at]
                match at,src with
                | (_,b) when b = tgt -> acc @ [(at,c)]
                | (_,b) -> loop b (acc @ [at,c])
            let r = loop at [] @ [(tgt,0.0)]
            let rev = List.rev r
            List.zip r rev |> List.map (fun ((id1,c1),(id2,c2)) -> id1,c2) 

    let classic neighbors h cost start goal =
        let prioSelect (lopen : (int * float) list) =
            let sorted = List.sortBy (fun (id,p) -> p) lopen //|> List.rev
            (fst (List.head sorted), List.tail sorted)
        let rec search (lopen : (int * float) list) (routes : Map<int,int * float>) =
            let rec searchNeighbors cur nl o (r : Map<int,(int * float)>) =
                match nl with
                | [] -> o,r
                | next::others -> 
                    let newCost = (snd (r.[cur])) + cost cur next
                    if (not (Map.containsKey next r)) || (newCost < snd r.[next])
                    then
                        let r1 = r |> Map.remove next |> Map.add next (cur,newCost)
                        let prio = newCost + h next goal
                        //printfn "current = %d -- next = %d -- newCost = %f -- prio = %f -- h = %f" cur next newCost prio (h next goal)
                        let o1 = (next,prio) :: o
                        searchNeighbors cur others o1 r1
                    else
                        searchNeighbors cur others o r
            match lopen with 
            | [] -> []
            | _::_ ->
                let current,rest = prioSelect lopen
                if current = goal then Internals.makeRoute routes current start
                else
                    let lopen1,routes1 = searchNeighbors current (neighbors current) rest routes
                    search lopen1 routes1
        search [start,0.] (Map.ofList [start,(start,0.0)])

    let twoWay sources targets hforward hbackward costforward costbackward (start : int) (goal : int) (n : int) rr =
        let prioSelect (lopen : (int * float) list) =
            let sorted = List.sortBy (fun (id,p) -> p) lopen //|> List.rev
            (fst (List.head sorted), List.tail sorted)
        let searchforward lopen exploredF exploredB nfound acc =
            let rec searchNeighbors cur nl o (r : Map<int,(int * float)>) =
                match nl with
                | [] -> o,r
                | next::others -> 
                    //printfn "fwd: current = %d -- next = %d -- nl = %A  -- r = %A" cur next nl r
                    let newCost = (snd (r.[cur])) + costforward cur next
                    if (not (Map.containsKey next r)) || (newCost < snd r.[next])
                    then
                        let r1 = r |> Map.remove next |> Map.add next (cur,newCost)
                        let prio = newCost + hforward next goal
                        let o1 = (next,prio) :: o
                        if Map.containsKey next exploredB then
                            rr (next, Internals.makeRoute r1 next start, Internals.makeRouteBackward exploredB next goal)
                        searchNeighbors cur others o1 r1
                    else
                        searchNeighbors cur others o r
            match lopen with 
            | [] -> (lopen,exploredF,0,acc)
            | _::_ ->
                let current,rest = prioSelect lopen
                if current = goal then
                   (rest,exploredF,nfound+1,acc @ [Internals.makeRoute exploredF current start] )
                else
                    let lopen1,explored1 = searchNeighbors current (targets current) rest exploredF
                    (lopen1, explored1, nfound, acc)
        let searchbackward lopen exploredB exploredF nfound acc = 
            let rec searchNeighbors cur nl o (r : Map<int,(int * float)>) =
                match nl with
                | [] -> o,r
                | next::others -> 
                    //printfn "bwd: current = %d -- next = %d -- nl = %A  -- r = %A" cur next nl r
                    let newCost = (snd (r.[cur])) + costbackward cur next
                    if (not (Map.containsKey next r)) || (newCost < snd r.[next])
                    then
                        let r1 = r |> Map.remove next |> Map.add next (cur,newCost)
                        let prio = newCost + hbackward next start
                        let o1 = (next,prio) :: o
                        searchNeighbors cur others o1 r1
                    else
                        searchNeighbors cur others o r
            match lopen with 
            | [] -> (lopen,exploredB,0,acc)
            | _::_ ->
                let current,rest = prioSelect lopen
                if current = start then
                   //(rest,explored,nfound+1,acc @ [Internals.makeRoute explored current goal []])
                   (rest,exploredB,nfound+1,acc @ [Internals.makeRouteBackward exploredB current goal] )
                else
                    let lopen1,explored1 = searchNeighbors current (sources current) rest exploredB
                    (lopen1, explored1, nfound, acc)
        let rec driver openF openB exploredF exploredB nfoundF nfoundB accF accB =
            let openF1, exploredF1,nfoundF1,accF1 = searchforward openF exploredF exploredB nfoundF accF
            let openB1, exploredB1,nfoundB1,accB1 = searchbackward openB exploredB exploredF nfoundB accB

            match (nfoundF1+nfoundB1), List.isEmpty openF1, List.isEmpty openB1 with
            | (s,false,false) when s < n -> 
                driver openF1 openB1 exploredF1 exploredB1 nfoundF1 nfoundB1 accF1 accB1
            | _ -> 
                accF1 @ accB1
        driver [start,0.0] [goal,0.0] (Map.ofList [start,(start,0.0)]) (Map.ofList [goal,(goal,0.0)]) 0 0 [] []


// Location : x,y coordinate or lat/long - whatever.
// Edges: (id,cost) list
type Node = { Id : int; Location : int * int; Edges : (int * float) list; EdgesBackward : (int * float) list}

type Graph = Map<int,Node>

let addNode node graph =
    Map.add (node.Id) node graph

let newNode idgen x y  = 
    { Id = idgen(); Location = (x,y); Edges = []; EdgesBackward = [] }

let addEdge id cost node =
    { node with Node.Edges = node.Edges @ [(id,cost)]; }

let addEdgeBackward id cost node =
    { node with Node.EdgesBackward = node.EdgesBackward @ [(id,cost)]; }

let idgen startvalue =
    let next = ref startvalue
    fun () -> 
        let id = !next
        next := !next + 1
        id

let appendNode node nodeList = nodeList @ [node]

let sq x = x*x
let distance p1 p2 =
    let x1,y1 = p1
    let x2,y2 = p2
    sqrt( float (sq (x2-x1) + sq (y2-y1)) )

let solve (g : Graph) s e =
    let ns id =
        g.[id].Edges |> List.map (fun (id,c) -> id)
    let h at goal =
        float (distance (g.[at].Location) (g.[goal].Location))
    let c a b =
        g.[a].Edges |> List.pick (fun (id,cost) -> if id = b then Some(cost) else None)
    [AStar.classic ns h c s e]  // give it the same return type as solveTwoWay to make stuff below easier and shorter

let solveTwoWay (g : Graph) s e n =
    let edges id =
        let nl = g.[id].Edges |> List.map (fun (id,c) -> id)
        //printfn "2way edges id = %d list = %A" id nl
        nl
    let edgesBackward id =
        let nl = g.[id].EdgesBackward |> List.map (fun (id,c) -> id)
        //printfn "2way backwards edges id = %d list = %A" id nl
        nl
    let hforward at goal =
        float (distance (g.[at].Location) (g.[goal].Location))
    let hbackward at start =
        float (distance (g.[at].Location) (g.[start].Location))
    let costF a b =
        g.[a].Edges |> List.pick (fun (id,cost) -> if id = b then Some(cost) else None)
    let costB a b =
        g.[a].EdgesBackward |> List.pick (fun (id,cost) -> if id = b then Some(cost) else None)
    let  debugView arg =
        let id,r1,r2 = arg
        printfn "meeting at %d: r1 = %A r2 = %A" id r1 r2

    AStar.twoWay edgesBackward edges hforward hbackward costF costB s e n debugView
let solveProblem problem =
    let g, start, goal = problem
    g,start,goal,solve g start goal

let solveProblemTwoWay problem n =
    let g, start, goal = problem
    g,start,goal,solveTwoWay g start goal n

let save name solution =
    let graph, start, goal, routes = solution
    use  writer = System.IO.File.CreateText("""E:\temp\""" + name + """.txt""")
    fprintf writer "------------------------------------\n start = %d ----> goal = %d: %d routes found.\n" start goal (List.length routes)
    fprintf writer "Graph:\n"
    graph |> Map.iter
        (fun id node ->
            fprintf writer "Node: %A\n" node
        )
    routes |> List.iteri 
        (fun index route ->
            fprintf writer "Route %d: %A\n" index route
        )
// An example problem I used to play with:
// The graph is such, that  the nodes are connected to the right and     
// downwards and diagonally downwards only. 
// The cost is either 1.0 or sqrt(2), for the horizontal or vertical and
// the diagonal connection, respectively.
let problem2 () =
    let newNodeAN = newNode (idgen 0)
    let cond c x n =
        if c then n |> x else n
    let accessCost p =
        match p with
        | (4,4) | (4,5) | (5,4) | (5,5) -> 10.0
        | _ -> 1.0
    let right (n : Node) : Node =
        let t = 1 + fst n.Location, snd n.Location
        let c = accessCost t
        n 
        |> cond (fst n.Location < 9) (fun n -> addEdge (n.Id + 1) c n)
        |> cond (fst n.Location > 0) (fun n -> addEdgeBackward (n.Id - 1) c n)
    let down n =
        let t = fst n.Location, 1 + snd n.Location
        let c = accessCost t
        n
        |> cond (snd n.Location < 9) (fun n -> addEdge (n.Id + 10) c n)
        |> cond (snd n.Location > 0) (fun n -> addEdgeBackward (n.Id - 10) c n)
    let diagdown n =
        let t = 1 + fst n.Location, 1 + snd n.Location
        let c = (sqrt(2.0)) * accessCost t
        n
        |> cond (fst n.Location < 9 && snd n.Location < 9) (fun n -> addEdge (n.Id + 11) c n)
        |> cond (fst n.Location > 0 && snd n.Location > 0) (fun n -> addEdgeBackward (n.Id - 11) c n)

    [
        for y = 0 to 9 do
            for x = 0 to 9 do
                yield newNodeAN x y
    ]
    |> List.map 
        (fun n ->
            n 
            |> right
            |> down
            |> diagdown 
        )
    |> List.map (fun n -> (n.Id,n))
    |> Map.ofList
    , 0, 99

// Last not least, the code can be executed like this:
// And since both implementations yield the same data structures,
// they can be used interchangeably and compared to each other.
solveProblemTwoWay (problem2() 5) |> save "problem2_solution"