end0tknr's kipple - 新web写経開発

http://d.hatena.ne.jp/end0tknr/ から移転します

最短経路探索アルゴリズムの A* (A-STAR)を perlで試す

「A*」を聞いたことはありますが、実装したことはない為、写経。

今回の写経で、2次元の最短経路探索は理解できた気がするので、次は3次元? 立体? 経路探索に発展させたい。

参考にさせて頂いたurl

A*アルゴリズムは1968年に発表された為、インターネット上に多くの情報がありますが、 私には次のurlが分かりやすかった。

A*の動作を視覚的に理解しやすいGIGAZINE

gigazine.net

しっかり?と定義が記載されているwikipedia

https://ja.wikipedia.org/wiki/A*

pythonのsrcもコメントも分かりやすい pashango_p

d.hatena.ne.jp

今回は、上記 pashango_p の pythonperl写経

以下は、perlで記載していますが、申し訳ない程、pashango_p さんのsrcのまんま

#!/usr/local/bin/perl
use strict;
use warnings;
use utf8;
use List::Util;
use Data::Dumper;

my $MAP_DATA =
    ['OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
     'OS  O     O     O         O          O',
     'O   O  O  O  O  O         O    OOOO GO',
     'O      O     O  O   OOOO  O    O  OOOO',
     'OO OOOOOOOOOOOOOOO  O     O    O     O',
     'O                O  O     O          O',
     'O        OOO     O  O     OOOOOOOOO  O',
     'O  OO    O    OOOO  O     O      OO  O',
     'O   O    O          O     O  O   O   O',
     'O   OOO  O          O        O   O   O',
     'O        O          O        O       O',
     'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO'];
my $MAP_WIDTH  = List::Util::max(map { length($_) } @$MAP_DATA);
my $MAP_HEIGHT = scalar(@$MAP_DATA);
my $START_POS = [];
my $GOAL_POS  = [];
my $MAX_SEEK_LOOP = 5000;

main();

sub main {

    #スタート位置とゴール位置を設定
    my $y = 0;
    for my $map_line ( @$MAP_DATA ){
        my $str_index = index($map_line,"S");
        if( $str_index >=0 ){
            $START_POS = [$str_index,$y];
        }
        $str_index = index($map_line,"G");
        if( $str_index >=0 ){
            $GOAL_POS = [$str_index,$y];
        }
        $y++;
    }

    #OpenリストとCloseリストを設定
    my $open_list     = NodeList->new();
    my $close_list    = NodeList->new();

    my $start_node    = Node->new( @$START_POS );
    $start_node->{fs} = $start_node->{hs};
    
    $open_list->append($start_node);

    my $i = 0;
    my $end_node;
    while(1){
        if( $i++ > $MAX_SEEK_LOOP){  #もしもの為のfail safe
            print "You reached MAX_SEEK_LOOP = $MAX_SEEK_LOOP \n";
            return;
        }

        #Openリストが空になったら解なし
        if( scalar(@{$open_list->{node_list}}) == 0 ){
            print "There is no route until reaching a goal.\n";
            return;
        }

        #Openリストからf*が最少のノードnを取得
        my ($n)= sort {$a->{fs}<=>$b->{fs}} @{$open_list->{node_list}};
        $open_list->remove($n);
        $close_list->append($n);

        if( $n->is_goal() ){ #最小ノードがゴールだったら終了
            $end_node = $n;
            last;
        }

        #f*() = g*() + h*() -> g*() = f*() - h*()
        my $n_gs = $n->{fs} - $n->{hs};

        #ノードnの移動可能方向のノードを調べる
        for my $v ([1,0],[-1,0],[0,1],[0,-1]){
            my $x = $n->{pos}->[0] + $v->[0];
            my $y = $n->{pos}->[1] + $v->[1];

            #マップが範囲外または壁(O)の場合はcontinue
            if($y< 0 or $MAP_HEIGHT < $y or
               $x< 0 or $MAP_WIDTH  < $x or
               substr($MAP_DATA->[$y],$x,1) eq 'O'){
                next;
            }

            #移動先のノードがOpen,Closeのどちらのリストに
            #格納されているか、または新規ノードなのかを調べる
            my $m = $open_list->find($x,$y);
            my $dist = ($n->{pos}->[0] - $x)**2 + ($n->{pos}->[1] - $y)**2;

            if ($m){
                #移動先のノードがOpenリストに格納されていた場合、
                #より小さいf*ならばノードmのf*を更新し、親を書き換え
                if( $m->{fs} > ($n_gs + $m->{hs} + $dist)){
                    $m->{fs} = $n_gs + $m->{hs} + $dist;
                    $m->{parent_node} = $n;
                }
            } else {
                $m = $close_list->find($x,$y);
                if($m){
                    #移動先のノードがCloseリストに格納されていた場合、
                    #より小さいf*ならばノードmのf*を更新し、親を書き換え
                    #かつ、Openリストに移動する
                    if($m->{fs} > $n_gs + $m->{hs} + $dist){
                        $m->{fs} = $n_gs + $m->{hs} + $dist;
                        $m->{parent_node} = $n;
                        $open_list->append($m);
                        $close_list->remove($m);
                    }
                } else {
                    #新規ノードならばOpenリストにノードに追加
                    $m = Node->new($x,$y);
                    $m->{fs} = $n_gs + $m->{hs} + $dist;
                    $m->{parent_node} = $n;
                    $open_list->append($m);
                }
            }
        }
    }

    #endノードから親を辿っていくと、最短ルートを示す
    my $m = [];
    for my $line ( @$MAP_DATA ){
        my @cols = split(//, $line);
        push(@$m,\@cols);
    }

    my $n = $end_node->{parent_node};
    
    while(1){
        last if not $n->{parent_node};
        $m->[$n->{pos}->[1]]->[$n->{pos}->[0]] = '+';
        $n = $n->{parent_node};
    }

    for my $cols ( @$m ){
        print join('', @$cols),"\n";
    }
}


package Node;  ########

sub new {
    my ($class, $x, $y) = @_;

    my $self = {};
    $self =  bless $self, $class;
    $self->{pos} = [$x, $y];
    $self->{hs}  = ($x-$GOAL_POS->[0])**2 + ($y-$GOAL_POS->[1])**2;
    $self->{fs}  = 0; ## なんで?

    $self->{owner_list} =  [];
    $self->{parent_node} = '';

    return $self;
}

sub is_goal {
    my ($self) = @_;
    if($self->{pos}->[0] == $GOAL_POS->[0] and
       $self->{pos}->[1] == $GOAL_POS->[1]){
        return $self; # means true
    }
    return; # means false
}


package NodeList; ########
use Data::Dumper;

sub new {
    my ($class) = @_;
    my $self = {};
    $self =  bless $self, $class;
    $self->{node_list} = [];
    return $self;
}

sub append {
    my ($self, $node_obj) = @_;

    if( defined($node_obj) and ref($node_obj) eq "Node" ){
        push( @{$self->{node_list}}, $node_obj);
        return;
    }
}

sub find {
    my ($self, $x, $y) = @_;

    for my $node_obj ( @{$self->{node_list}} ){
        if($node_obj->{pos}->[0] == $x and
           $node_obj->{pos}->[1] == $y ){
            return $node_obj;
        }
    }
    return;
}

sub remove {
    my ($self, $target_node_obj) = @_;

    my $i = 0;
    for my $node_obj ( @{$self->{node_list}} ){
        if($node_obj->{pos}->[0] == $target_node_obj->{pos}->[0] and
           $node_obj->{pos}->[1] == $target_node_obj->{pos}->[1] ){
            splice(@{$self->{node_list}},$i,1);
            return $self;
        }
        $i++;
    }
    return;
}

1;
__END__

↑こう書くと、↓こう動きます

$ ./a_star.pl
OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO
OS+ O     O     O         O   +++++++O
O + O  O  O  O  O  +++++++O   +OOOO GO
O +    O     O  O  +OOOO +O   +O  OOOO
OO+OOOOOOOOOOOOOOO +O    +O   +O     O
O ++++++++++++   O +O    +O   ++++++ O
O        OOO +   O +O    +OOOOOOOOO+ O
O  OO    O   +OOOO +O    +O +++++OO+ O
O   O    O   +++++++O    +O +O  +O++ O
O   OOO  O          O    ++++O  +O+  O
O        O          O        O  +++  O
OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO