ルート探索

青い矩形を障害物と見立て、左上から右下までの障害物に交差しないルートを求めています。
青い矩形はマウスドラッグで移動できます。(矩形同士が重なり合うケースは考慮されてませんので、 そういう操作をした場合は結果もおかしくなる可能性があります)

探索手順
1. 矩形の頂点を抽出
2. 互いに見えている(矩形の辺と交差していない）頂点同士を見つけてグラフを構築
3. ダイクストラ法で最短ルートを検索

ダイクストラによるルート探索アルゴリズムは少し手抜きあり。

参考エントリ：[ActionScript 3.0] 四分木からグラフを構築して、最短経路を探索する。

package
{
    import flash.display.*;
    import flash.events.*;
    import flash.geom.*;
    import flash.filters.DropShadowFilter;

    [SWF(width="500", height="500", backgroundColor="#ffffff")]
        public class Obstacle extends MovieClip
        {
            private var objects:Array = [];

            public function Obstacle()
            {
                stage.scaleMode = StageScaleMode.NO_SCALE;
                stage.align = StageAlign.TOP_LEFT;

                createObjects();

                findRoute();
            }

            private function findRoute():void{
                var g:Graph = createVisibilityGraph();
                calcShortestRoute(g);
                //drawRoute();
            }

            private function createVisibilityGraph():Graph{
                graphics.clear();

                var g:Graph = new Graph();

                // 全ての頂点をグラフに追加
                for each (var p:Polygon2D in objects){
                    var vs:Array = p.vertices;
                    for each (var pt:Point in vs){
                        var id:String = (p.x + pt.x).toString() + "-" + (p.y + pt.y).toString();
                        g.addVertex(new Vertex(id, p.x+pt.x, p.y+pt.y));
                    }
                }
                g.addVertex(new Vertex("0-0",0,0));
                g.addVertex(new Vertex("500-500",500,500));

                trace("g.vertices.length=" + g.vertices.length.toString());

                // 頂点同士のチェック
                for each (var v:Vertex in g.vertices){
                    for each (var v2:Vertex in g.vertices){
                        if(v.id != v2.id){
                            // 2頂点を結ぶ線分
                            var l:Line = new Line(new Point(v.x, v.y),
                                    new Point(v2.x, v2.y));

                            // 2頂点を結ぶ線分が、多角形の辺に交差するか検査
                            for each (var pl:Polygon2D in objects){
                                if(!isCross(l, pl)){
                                    /*
                                    graphics.lineStyle(1,0xff00,1);
                                    graphics.moveTo( v.x,  v.y);
                                    graphics.lineTo( v2.x,  v2.y);
                                     */

                                    var lh:Number = v2.x - v.x;
                                    var lv:Number = v2.y - v.y;
                                    // 辺を作成(距離が重み)
                                    new Edge(v, v2, Math.sqrt(lh*lh + lv*lv));
                                }
                            }
                        }
                    }
                }

                return g;
            }

            private function calcShortestRoute(g:Graph):void{

                var route:Array = Dijkstra.getRoute(g, "0-0", "500-500");
                if(route == null)
                    trace("no route");
                else{
                    graphics.lineStyle(10,0xff0000,0.9);
                    graphics.moveTo(0,0);
                    for each(var v:String in route){
                        trace(v);

                        var reg:RegExp = /([0-9\.]*)-([0-9\.]*)/;
                        var result:Array = reg.exec(v);
                        graphics.lineTo(result[1], result[2]);
                    }
                }
            }
            
            private function isCross(l:Line, p:Polygon2D):Boolean{
                for each (var p3:Polygon2D in objects){
                    // 多角形の集合の辺と交差するか
                    var lines:Array = p3.lines;
                    for each (var lp:Line in lines){
                        if(Line.isCross(l, lp)){
                            return true;
                        }
                    }

                }

                return false;
            }


            private function l(p1x:Number, p1y:Number, p2x:Number, p2y:Number):Number{
                return Math.sqrt((p1x - p2x)*(p1x - p2x) + (p1y - p2y)*(p1y - p2y));
            }

            private function createObjects():void
            {

                createPolygon(new Point (50,200), [
                        new Point(0,0) 
                        ,new Point(100,0) 
                        ,new Point(100,150)
                        ,new Point(0,150)
                        ]);
                createPolygon(new Point (330,100), [
                        new Point(0,0) 
                        ,new Point(100,0) 
                        ,new Point(100,150)
                        ,new Point(0,150)
                        ]);
                createPolygon(new Point (200,0), [
                        new Point(0,0) 
                        ,new Point(100,0) 
                        ,new Point(100,150)
                        ,new Point(0,150)
                        ]);

                createPolygon(new Point (200,200), [
                        new Point(0,0) 
                        ,new Point(0,100) 
                        ,new Point(100,100)
                        ,new Point(100,0)
                        ]);
               
                createPolygon(new Point (300,300), [
                        new Point(0,0) 
                        ,new Point(100,0) 
                        ,new Point(100,180)
                        ,new Point(0,180)
                        ]);

            }

            private function createPolygon(point:Point, a:Array):void{
                var p:Polygon2D = new Polygon2D(a);
                p.x = point.x;
                p.y = point.y;
                p.addEventListener( MouseEvent.MOUSE_DOWN, pickup );
                p.addEventListener( MouseEvent.MOUSE_UP, place );
                addChild(p);
                objects.push(p);
            }

            public function pickup( event:MouseEvent ):void {
                // ドラッグ処理を開始して、影フィルターをつける
                event.target.startDrag( );
                event.target.filters = [ new DropShadowFilter( ) ];
                // 最前面に表示されるようにする
                setChildIndex( DisplayObject( event.target ), numChildren - 1 );
            }
            public function place( event:MouseEvent ):void {
                // ドラッグ処理を終了して影フィルターをオフ
                event.target.stopDrag( );
                event.target.filters = null;

                findRoute();
            }
        }
}

internal class Graph{
    private var _vertices:Object = {};

    public function addVertex(v:Vertex):void{
        if(exists(v.id))
            trace(v.id.toString() + " exists!");
        else
            _vertices[v.id] = v;
    }

    public function vertex(s:String):Vertex{
        return _vertices[s];
    }

    public function exists(s:String):Boolean{
        return (_vertices[s] != null);
    }

    public function print():void{
        for (var id:String in _vertices){
            trace(id + " " + _vertices[id].edges.length);
        }
    }
    public function get vertices():Array{
        var result:Array = [];
        for (var key:String in _vertices)
            result.push(_vertices[key]);
        return result;
    }
}

import flash.display.*;
import flash.geom.*;

internal class Polygon2D extends Sprite
{
    private var _vertices:Array;
    public function Polygon2D(a:Array)
    {
        _vertices = a;

        this.graphics.clear();
        this.graphics.lineStyle(2, 0xff00, 1);
        this.graphics.beginFill(0xff);

        var st:Point = a[0];
        graphics.moveTo(st.x, st.y);
        for each (var p:Point in a){
            graphics.lineTo(p.x, p.y);
        }
        graphics.lineTo(st.x, st.y);

        this.graphics.endFill();
    }

    public function get vertices():Array{
        return _vertices;
    }

    public function get lines():Array{
        var result:Array = [];

        for each (var p:Point in _vertices){
            for each (var p2:Point in _vertices){
                if(p.x != p2.x || p.y != p2.y){
                    result.push(new Line(new Point(p.x + this.x, p.y + this.y), 
                        new Point(p2.x + this.x, p2.y + this.y)));
                }
            }
        }

        return result;
    }
}

internal class Dijkstra{
    public static function getRoute(g:Graph, start:String, goal:String):Array{
        var markedVertex:Object = new Object(); // マーク済みの頂点とそこまでのコスト
        var prev:Array = new Array(); // 頂点をキーに持ち、その頂点までの最短路の親の頂点を値に持つ
        markedVertex[start] = 0; // 開始地点を追加

        while(true){
            var shortestCost:int = -1;
            var shortestVertex:Vertex = null;
            //trace("check start");
            for (var s:String in markedVertex){
                var v:Vertex = g.vertex(s);
                if(v == null){
                    trace(s + " is not found in graph");
                    return null;
                }
                // その頂点に含まれる辺をすべて調べる
                //trace("check vertex : " + v.id + " " + v.edges.length + " edges");
                for each (var edge:Edge in v.edges){
                    //   trace("dst : " + edge.dst.id);
                    if(markedVertex[edge.dst.id] == null){
                        var cost:int = markedVertex[v.id] + edge.weight;
                        //   trace("cost to " + edge.dst.id + " = " + cost);
                        // 辺の他端に到達するには、過去に調べたルートより今のルートのほうが最短か？
                        if(shortestCost == -1 ||
                                cost < shortestCost){
                            // 最短路を更新
                            shortestCost = cost;
                            shortestVertex = edge.dst;
                            prev[shortestVertex.id] = edge.src.id;
                            //trace("shortestVertex=" + shortestVertex.id);
                            //trace("shortestCost=" + shortestCost);
                        }
                    }
                }
            }
            if(shortestVertex == null){
                //trace("** no route found **");
                //for (var s:String in markedVertex)
                //trace(s);
                return null;
            }
            markedVertex[shortestVertex.id] = shortestCost;
            if(shortestVertex.id == goal)
                break;
        }

        var result:Array = [];
        result.unshift(goal);
        var routev:String = goal;
        while(true){
            routev = prev[routev];
            result.unshift(routev);
            if(routev == start)
                break;
        }

        return result;
    }
}

internal class Vertex extends Object{
    public var edges:Array = new Array();
    public var id:String;
    public var x:int;
    public var y:int;
    public function Vertex(id:String, x:int, y:int){
        this.id = id;
        this.x = x;
        this.y = y;
    }
    public function addEdge(e:Edge):void{
        trace("addEdge["+id+ "] dst->" + e.dst.id);
        edges.push(e);
        trace(edges.length + "edges");

    }
}

internal class Edge {
    public var weight:int;
    public var src:Vertex;
    public var dst:Vertex;
    public function Edge(src:Vertex, dst:Vertex, weight:int){
        this.src = src;
        this.dst = dst;
        this.weight = weight;
        if(src != null)
            src.addEdge(this);
    }
}

internal class Line{
    public var p1:Point;
    public var p2:Point;
    public function Line(p1:Point, p2:Point){
        this.p1 = p1;
        this.p2 = p2;
    }

    // 直線の交点（線分の交点ではない）
    public static function crossPoint(lhs:Line, rhs:Line):Point {
        var a:Point = new Point(lhs.p2.x - lhs.p1.x, lhs.p2.y- lhs.p1.y);
        var b:Point = new Point(rhs.p2.x - rhs.p1.x, rhs.p2.y- rhs.p1.y);
        var c:Point = new Point(rhs.p1.x - lhs.p1.x, rhs.p1.y- lhs.p1.y);

        var result:Point = new Point();

        var cross_b_c:Number = b.x*c.y - b.y*c.x;
        var cross_b_a:Number = b.x*a.y - b.y*a.x;

        if(cross_b_a == 0)
            return null;

        result.x = lhs.p1.x + a.x * cross_b_c / cross_b_a;
        result.y = lhs.p1.y + a.y * cross_b_c / cross_b_a;

        return result;
    }

    public static function isCross(lhs:Line, rhs:Line):Boolean{
        var p:Point = crossPoint(lhs,rhs);

        return (p != null &&
                (p.x - rhs.p1.x) * (p.x - rhs.p2.x) + (p.y - rhs.p1.y) * (p.y - rhs.p2.y) < 0) &&
            ((p.x - lhs.p1.x) * (p.x - lhs.p2.x) + (p.y - lhs.p1.y) * (p.y - lhs.p2.y) < 0)
            ;
    }
}