[Go][library]A* path search: astar.go

astar.go

// A* lib port from pythonic A*: http://gist.github.com/147645
package astar

import "container/heap"
import "container/vector"

// position data for A*: e.g. start and goal.
type Location interface {
        Equals(other Location) bool;
        // Returned value should be unique for each location value.
        ToKey() string;
}

// Tuple for score and path
type scored struct {
        score float64;
        path *vector.Vector;
}
// queue for scored to use "container/heap"
type heapq struct {
        vector.Vector;
}
func (q *heapq) Init(len int) *heapq {
        q.Vector.Init(len);
        return q;
}
func (q *heapq) At(i int) *scored {
        return q.Vector.At(i).(*scored);
}
func (q *heapq) Less(i int, j int) bool {
        return q.At(i).score < q.At(j).score;
}
func (q *heapq) Push(o interface {}) {
        q.Vector.Push(o.(vector.Element));
}
func (q *heapq) Pop() interface {} {
        return q.Vector.Pop();
}

func locs(elems []vector.Element) []Location {
        result := make([]Location, len(elems));
        for i, v := range(elems) {
                result[i] = v.(Location);
        }
        return result;
}

// A* search from start to goal.
func AStar(
        start Location,
        goal Location,
        nexts func (loc Location) <-chan Location,
        distance func (path []Location) float64,
        heulistic func (loc Location) float64)
        []Location {

        queue := new(heapq).Init(0);
        heap.Init(queue);
        init := vector.New(1);
        init.Set(0, start);
        initScore := distance(locs(init.Data())) + heulistic(start);
        checked := map[string]float64{start.ToKey(): initScore};
        heap.Push(queue, &scored{initScore, init});

        for queue.Len() > 0 {
                head := heap.Pop(queue).(*scored);
                last := head.path.Last().(Location);
                if last.Equals(goal) {
                        return locs(head.path.Data());
                }
                for pos := range nexts(last) {
                        path := head.path.Slice(0, head.path.Len());
                        path.Push(pos);
                        score := distance(locs(path.Data())) + heulistic(pos);
                        poskey := pos.ToKey();
                        if old, ok := checked[poskey]; ok && old <= score {
                                continue;
                        }
                        checked[poskey] = score;
                        heap.Push(queue, &scored{score, path});
                }
        }
        return nil;
}