navMeshAstarPath地图寻路算法 - 守望者--AIR技术交流

package {
import flash.display.Sprite;
import flash.events.Event;
import flash.events.MouseEvent;
import flash.utils.getTimer;
/**
* ...
* http://wonderfl.net/c/lDKs
* http://www.cnblogs.com/neoragex2002/archive/2007/09/09/887556.html
* @author sliz http://game-develop.net/
*/
public class Main extends Sprite {
private var d:Delaunay;
private var astar:AStar;
private var g:Grid;
public function Main():void {
if (stage)
init();
else
addEventListener(Event.ADDED_TO_STAGE, init);
}
private function init(e:Event = null):void {
removeEventListener(Event.ADDED_TO_STAGE, init);
d = new Delaunay(400, 400);
var c:Number = 300;
while (c-- > 0){
d.add(400 * Math.random(), 400 * Math.random());
}
for each (var t:Triangle in d.triangles){
if (Math.random() < 0.2){
t.walkAble = false;
}
}
g = new Grid();
g.delaunay = d;
g.calculateLinks();
astar = new AStar(g);
g.startNode = d.triangles[int(d.triangles.length * Math.random())];
g.endNode = d.triangles[int(d.triangles.length * Math.random())];
stage.addEventListener(MouseEvent.CLICK, onclick);
astar.findPath();
draw();
}
private function onclick(e:MouseEvent):void {
g.startNode = g.endNode;
for each (var t:Triangle in d.triangles){
if (isPointInTriangle(t.node0.point.x, t.node0.point.y, t.node1.point.x, t.node1.point.y, t.node2.point.x, t.node2.point.y, mouseX, mouseY)){
g.endNode = t;
break;
}
}
if (g.startNode && g.endNode){
trace(astar.findPath(), astar.path);
draw();
}
}
private function draw():void {
graphics.lineStyle(0);
for each (var t:Triangle in d.triangles){
if (t.walkAble)
graphics.beginFill(0xffffff);
else
graphics.beginFill(0xff0000);
graphics.moveTo(t.node0.point.x, t.node0.point.y);
graphics.lineTo(t.node1.point.x, t.node1.point.y);
graphics.lineTo(t.node2.point.x, t.node2.point.y);
graphics.lineTo(t.node0.point.x, t.node0.point.y);
}
if (astar.path){
for (var i:int = 0; i < astar.path.length; i++){
t = astar.path[i];
if (i == 0)
graphics.beginFill(0xffff00);
else if (i == astar.path.length - 1)
graphics.beginFill(0x00ffff);
else
graphics.beginFill(0xff00ff);
graphics.moveTo(t.node0.point.x, t.node0.point.y);
graphics.lineTo(t.node1.point.x, t.node1.point.y);
graphics.lineTo(t.node2.point.x, t.node2.point.y);
graphics.lineTo(t.node0.point.x, t.node0.point.y);
}
}
}
public static function multiply(px1:Number, py1:Number, px2:Number, py2:Number, px3:Number, py3:Number):int {
return (px1 - px3) * (py2 - py3) - (px2 - px3) * (py1 - py3);
}
public static function isPointInTriangle(px1:Number, py1:Number, px2:Number, py2:Number, px3:Number, py3:Number, px:Number, py:Number):Boolean {
var v:int = multiply(px1, py1, px2, py2, px3, py3);
if (v * multiply(px1, py1, px2, py2, px, py) <= 0){
return false;
}
if (v * multiply(px2, py2, px3, py3, px, py) <= 0){
return false;
}
if (v * multiply(px3, py3, px1, py1, px, py) <= 0){
return false;
}
return true;
}
}
}
import flash.geom.Point;
class AStar {
private var open:BinaryHeap;
private var grid:Grid;
private var endNode:Triangle;
private var startNode:Triangle;
private var _path:Array;
private var nowversion:int = 1;
public var heuristic:Function;
public function AStar(grid:Grid){
this.grid = grid;
heuristic = distance;
}
private function justMin(x:Object, y:Object):Boolean {
return x.f < y.f;
}
public function findPath():Boolean {
endNode = grid.endNode;
nowversion++;
startNode = grid.startNode;
open = new BinaryHeap(justMin);
startNode.g = 0;
return search();
}
private function distance(t1:Triangle):Number {
var t2:Triangle = endNode;
var c:Number = Math.sqrt((t1.cx - t2.cx) * (t1.cx - t2.cx) + (t1.cy - t2.cy) * (t1.cy - t2.cy));
return c;
}
public function search():Boolean {
var node:Triangle = startNode;
node.version = nowversion;
while (node != endNode){
var len:int = node.links.length;
for (var i:int = 0; i < len; i++){
var test:Triangle = node.links[i].node;
var cost:Number = node.links[i].cost;
var g:Number = node.g + cost;
var h:Number = heuristic(test);
var f:Number = g + h;
if (test.version == nowversion){
if (test.f > f){
test.f = f;
test.g = g;
test.h = h;
test.parent = node;
}
} else {
test.f = f;
test.g = g;
test.h = h;
test.parent = node;
open.ins(test);
test.version = nowversion;
}
}
if (open.a.length == 1){
return false;
}
node = open.pop() as Triangle;
}
buildPath();
return true;
}
private function buildPath():void {
_path = [];
var node:Triangle = endNode;
path.push(node);
while (node != startNode){
node = node.parent;
path.unshift(node);
}
}
public function get path():Array {
return _path;
}
}
class BinaryHeap {
public var a:Array = [];
public var justMinFun:Function = function(x:Object, y:Object):Boolean {
return x < y;
};
public function BinaryHeap(justMinFun:Function = null){
a.push(-1);
if (justMinFun != null)
this.justMinFun = justMinFun;
}
public function ins(value:Object):void {
var p:int = a.length;
a[p] = value;
var pp:int = p >> 1;
while (p > 1 && justMinFun(a[p], a[pp])){
var temp:Object = a[p];
a[p] = a[pp];
a[pp] = temp;
p = pp;
pp = p >> 1;
}
}
public function pop():Object {
var min:Object = a[1];
a[1] = a[a.length - 1];
a.pop();
var p:int = 1;
var l:int = a.length;
var sp1:int = p << 1;
var sp2:int = sp1 + 1;
while (sp1 < l){
if (sp2 < l){
var minp:int = justMinFun(a[sp2], a[sp1]) ? sp2 : sp1;
} else {
minp = sp1;
}
if (justMinFun(a[minp], a[p])){
var temp:Object = a[p];
a[p] = a[minp];
a[minp] = temp;
p = minp;
sp1 = p << 1;
sp2 = sp1 + 1;
} else {
break;
}
}
return min;
}
}
class Delaunay {
//----------------------------------------
//VARIABLES
//点群
private var _points:Array;
//三角形の集まり
private var _triangles:Array;
private var stageWidth:Number;
private var stageHeight:Number;
/*
* コンストラクタ
*/
public function Delaunay(w:Number, h:Number){
stageWidth = w;
stageHeight = h;
//Security.allowDomain("*");
//Security.allowDomain("planet-ape.net");
//Security.allowDomain("wonderfl.net");
//Security.allowDomain("swf.wonderfl.net");
//初期化
_initialize();
}
/*
* 初期化
*/
private function _initialize():void {
_points = new Array();
_triangles = new Array();
//ステージの大きさの三角形2つを用意
_points.push(new Node(0, 0, 0));
_points.push(new Node(1, stageWidth, 0));
_points.push(new Node(2, stageWidth, stageHeight));
_points.push(new Node(3, 0, stageHeight));
_triangles.push(new Triangle(_points[0], _points[1], _points[2]));
_triangles.push(new Triangle(_points[0], _points[2], _points[3]));
}
/*
* アップデート
*/ /*private function _updateHandler(event : Event) : void {
_interaction();
// _draw();
}*/
/*
* インタラクション
*/
private function _interaction():void {
//一時保持の三角形群
var localTriangles:Array = new Array();
//辺
var edges:Array;
//多角形
var polygon:Array;
//ポイント群ループ
for (var k:int = 4; k < _points.length; k++){
var node:Node = _points[k];
localTriangles = new Array();
edges = new Array();
for (var i:String in _triangles){
//点が外接円
var tri:Triangle = _triangles[i];
if (inOuterCircle(node.point.x, node.point.y, tri)){
edges.push(new Edge(tri.node0, tri.node1));
edges.push(new Edge(tri.node1, tri.node2));
edges.push(new Edge(tri.node2, tri.node0));
} else {
localTriangles.push(tri);
}
}
//edgesからpolygonを作る（重複辺の削除
polygon = new Array();
for (i in edges){
var edge0:Edge = edges[i];
//重複チェック
var flg:Boolean = false;
for (var j:String in polygon){
var edge1:Edge = polygon[j];
if (judgeEdges(edge0, edge1)){
flg = true;
polygon.splice(j, 1);
break;
}
}
//データが存在しない場合は追加
if (!flg)
polygon.push(edges[i]);
}
//polygonから三角形を作って挿入
for (i in polygon){
var tri1:Triangle = new Triangle(polygon[i].node0, polygon[i].node1, node);
localTriangles.push(tri1);
}
}
if (localTriangles.length > 1)
_triangles = localTriangles;
}
/*
* 同じ辺かどうかの判定
*/
private function judgeEdges(edge:Edge, edge0:Edge):Boolean {
if (edge.node0.id == edge0.node0.id && edge.node1.id == edge0.node1.id){
return true;
}
if (edge.node1.id == edge0.node0.id && edge.node0.id == edge0.node1.id){
return true;
}
return false;
}
/*
* 外接円の内か外か
*/
public function inOuterCircle(x:Number, y:Number, tri:Triangle):Boolean {
var node0:Node = tri.node0;
var node1:Node = tri.node1;
var node2:Node = tri.node2;
var d:Number = (node0.point.x * node0.point.x + node0.point.y * node0.point.y - x * x - y * y) * ((node1.point.x - x) * (node2.point.y - y) - (node2.point.x - x) * (node1.point.y - y)) + (node1.point.x * node1.point.x + node1.point.y * node1.point.y - x * x - y * y) * ((node2.point.x - x) * (node0.point.y - y) - (node2.point.y - y) * (node0.point.x - x)) + (node2.point.x * node2.point.x + node2.point.y * node2.point.y - x * x - y * y) * ((node0.point.x - x) * (node1.point.y - y) - (node0.point.y - y) * (node1.point.x - x));
return ((node1.point.x - node0.point.x) * (node2.point.y - node0.point.y) - (node1.point.y - node0.point.y) * (node2.point.x - node0.point.x) > 0) ? d > 0 : d <= 0;
}
/*
* マウスクリック時
*/
public function add(x:Number, y:Number):void {
_points.push(new Node(_points.length, x, y));
_interaction();
}
public function get triangles():Array {
return _triangles;
}
public function get points():Array {
return _points;
}
}
class Edge {
private var _node0:Node;
private var _node1:Node;
public function Edge(node0:Node, node1:Node):void {
_node0 = node0;
_node1 = node1;
}
public function get node0():Node {
return _node0;
}
public function get node1():Node {
return _node1;
}
}
class Grid {
public var startNode:Triangle;
public var endNode:Triangle;
public var delaunay:Delaunay;
public function Grid(){
}
public function calculateLinks():void {
var ts:Array = delaunay.triangles;
for each (var t:Triangle in ts){
t.links = [];
}
for (var i:int = 0; i < ts.length; i++){
var t1:Triangle = ts[i];
if (!t1.walkAble)
continue;
for (var j:int = i + 1; j < ts.length; j++){
var t2:Triangle = ts[j];
if (!t2.walkAble)
continue;
var t1ns:Array = [t1.node0, t1.node1, t1.node2];
var t2ns:Array = [t2.node0, t2.node1, t2.node2];
var counter:int = 0;
for (var m:int = 0; m < t1ns.length; m++){
for (var n:int = 0; n < t2ns.length; n++){
if (t1ns[m] == t2ns[n]){
counter++;
}
}
}
if (counter > 1){
var c:Number = Math.sqrt((t1.cx - t2.cx) * (t1.cx - t2.cx) + (t1.cy - t2.cy) * (t1.cy - t2.cy));
t1.links.push(new Link(t2, c));
t2.links.push(new Link(t1, c));
}
}
}
}
}
class Link {
public var node:Triangle;
public var cost:Number;
public function Link(node:Triangle, cost:Number){
this.node = node;
this.cost = cost;
}
}
class Node {
private var _id:int;
private var _point:Point;
public function Node(id:int, x:Number, y:Number){
_id = id;
_point = new Point(x, y);
}
public function get id():int {
return _id;
}
public function get point():Point {
return _point;
}
}
class Triangle {
public var walkAble:Boolean = true;
public var version:int = 1;
public var links:Array;
public var f:Number;
public var g:Number;
public var h:Number;
public var parent:Triangle;
private var _node0:Node;
private var _node1:Node;
private var _node2:Node;
private var edge0:Edge;
private var edge1:Edge;
private var edge2:Edge;
public var cx:Number;
public var cy:Number;
public function Triangle(node0:Node, node1:Node, node2:Node):void {
_node0 = node0;
_node1 = node1;
_node2 = node2;
cx = (_node0.point.x + _node1.point.x + _node2.point.x) / 3;
cy = (_node0.point.y + _node1.point.y + _node2.point.y) / 3;
}
public function get node0():Node {
return _node0;
}
public function get node1():Node {
return _node1;
}
public function get node2():Node {
return _node2;
}
}

复制代码