File Coverage

blib/lib/AI/Pathfinding/SMAstar.pm

Criterion	Covered	Total	%
statement	114	199	57.2
branch	27	82	32.9
condition	5	15	33.3
subroutine	13	21	61.9
pod	8	12	66.6
total	167	329	50.7

package AI::Pathfinding::SMAstar;

use 5.006000;
use strict;
use warnings;

require Exporter;

our @ISA = qw(Exporter);

# Items to export into callers namespace by default. Note: do not export
# names by default without a very good reason. Use EXPORT_OK instead.
# Do not simply export all your public functions/methods/constants.

# This allows declaration	use AI::Pathfinding::SMAstar ':all';
# If you do not need this, moving things directly into @EXPORT or @EXPORT_OK
# will save memory.
our %EXPORT_TAGS = ( 'all' => [ qw(

) ] );

our @EXPORT_OK = ( @{ $EXPORT_TAGS{'all'} } );

our @EXPORT = qw(

);

our $VERSION = '0.07';

use AI::Pathfinding::SMAstar::PriorityQueue;
use AI::Pathfinding::SMAstar::Path;
use Scalar::Util;
use Carp;

my $DEBUG = 0;


##################################################
# SMAstar constructor
##################################################
sub new {
    my $invocant = shift;
    my $class   = ref($invocant) || $invocant;
    my $self = {

	_priority_queue => AI::Pathfinding::SMAstar::PriorityQueue->new(),
	_state_eval_func => undef,
	_state_goal_p_func => undef,
	_state_num_successors_func => undef,
	_state_successors_iterator => undef,
	_show_prog_func => undef,
	_state_get_data_func => undef,


	@_, # attribute override
    };
    return bless $self, $class;
}


sub state_eval_func {
    my $self = shift;
    if (@_) { $self->{_state_eval_func} = shift }
    return $self->{_state_eval_func};
}

sub state_goal_p_func {
    my $self = shift;
    if (@_) { $self->{_state_goal_p_func} = shift }
    return $self->{_state_goal_p_func};
}

sub state_num_successors_func {
    my $self = shift;
    if (@_) { $self->{_state_num_successors_func} = shift }
    return $self->{_state_num_successors_func};
}

sub state_successors_iterator {
    my $self = shift;
    if (@_) { $self->{_state_successors_iterator} = shift }
    return $self->{_state_successors_iterator};
}

sub state_get_data_func {
    my $self = shift;
    if (@_) { $self->{_state_get_data_func} = shift }
    return $self->{_state_get_data_func};
}

sub show_prog_func {
    my $self = shift;
    if (@_) { $self->{_show_prog_func} = shift }
    return $self->{_show_prog_func};
}



###################################################################
#
# Add a state from which to begin the search.   There can
# be multiple start-states.
#
###################################################################
sub add_start_state
{
    my ($self, $state) = @_;


    my $state_eval_func = $self->{_state_eval_func};
    my $state_goal_p_func = $self->{_state_goal_p_func};
    my $state_num_successors_func = $self->{_state_num_successors_func},
    my $state_successors_iterator = $self->{_state_successors_iterator},
    my $state_get_data_func = $self->{_state_get_data_func};

    # make sure required functions have been defined
    if(!defined($state_eval_func)){
	croak "SMAstar:  evaluation function is not defined\n";
    }
    if(!defined($state_goal_p_func)){
	croak "SMAstar:  goal function is not defined\n";
    }
    if(!defined($state_num_successors_func)){
	croak "SMAstar:  num successors function is not defined\n";
    }
   if(!defined($state_successors_iterator)){
	croak "SMAstar:  successor iterator is not defined\n";
    }

    # create a path object from this state
    my $state_obj = AI::Pathfinding::SMAstar::Path->new(
	_state           => $state,
	_eval_func      => $state_eval_func,
	_goal_p_func    => $state_goal_p_func,
	_num_successors_func => $state_num_successors_func,
	_successors_iterator => $state_successors_iterator,
	_get_data_func  => $state_get_data_func,
	);


    my $fcost = AI::Pathfinding::SMAstar::Path::fcost($state_obj);
    # check if the fcost of this node looks OK (is numeric)
    unless(Scalar::Util::looks_like_number($fcost)){
	croak "Error:  f-cost of state is not numeric.  Cannot add state to queue.\n";
    }
    $state_obj->f_cost($fcost);

    # check if the num_successors function returns a number
    my $num_successors = $state_obj->get_num_successors();
    unless(Scalar::Util::looks_like_number($num_successors)){
	croak "Error:  Number of state successors is not numeric.  Cannot add state to queue.\n";
    }

    # test out the iterator function to make sure it returns
    #  an object of the correct type
    my $classname = ref($state);
    my $test_successor_iterator = $state_obj->{_successors_iterator}->($state);
    my $test_successor = $test_successor_iterator->($state);
    my $succ_classname = ref($test_successor);

    unless($succ_classname eq $classname){
	croak "Error:  Successor iterator method of object $classname does " .
	    "not return an object of type $classname.\n";
    }


    # add this node to the queue
    $self->{_priority_queue}->insert($state_obj);

}

###################################################################
#
# start the SMAstar search process
#
###################################################################
sub start_search
{
    my ($self,
	$log_function,
	$str_function,
	$max_states_in_queue,
	$max_cost,
	) = @_;

    if(!defined($str_function)){
	croak "SMAstar start_search:  str_function is not defined.\n";
    }

    sma_star_tree_search(\($self->{_priority_queue}),
                         \&AI::Pathfinding::SMAstar::Path::is_goal,
                         \&AI::Pathfinding::SMAstar::Path::get_descendants_iterator_smastar,
                         \&AI::Pathfinding::SMAstar::Path::fcost,
			 \&AI::Pathfinding::SMAstar::Path::backup_fvals,
			 $log_function,
			 $str_function,
			 \&AI::Pathfinding::SMAstar::Path::progress,
                         $self->{_show_prog_func},
			 $max_states_in_queue,
                         $max_cost,
	);
}



#################################################################
#
#  SMAstar search
#  Memory-bounded A* search
#
#
#################################################################
sub sma_star_tree_search
{

    my ($priority_queue,
	$goal_p,
	$successors_func,
	$eval_func,
	$backup_func,
	$log_function, # debug string func;  represent state object as a string.
	$str_function,
	$prog_function,
	$show_prog_func,
	$max_states_in_queue,
	$max_cost,
	) = @_;

    my $iteration = 0;
    my $num_states_in_queue = $$priority_queue->size();
    my $max_extra_states_in_queue = $max_states_in_queue;
    $max_states_in_queue = $num_states_in_queue + $max_extra_states_in_queue;
    my $max_depth = ($max_states_in_queue - $num_states_in_queue);

    my $best; # the best candidate for expansion



    if($$priority_queue->is_empty() || !$$priority_queue){
	return;
    }
    else{
	my $num_successors = 0;

	# loop over the elements in the priority queue
	while(!$$priority_queue->is_empty()){

	    # determine the current size of the queue
	    my $num_states_in_queue = $$priority_queue->{_size};
	    # get the best candidate for expansion from the queue
	    $best = $$priority_queue->deepest_lowest_cost_leaf_dont_remove();

	    #------------------------------------------------------
	    if(!$DEBUG){
		my $str = $log_function->($best);
		$show_prog_func->($iteration, $num_states_in_queue, $str);
	    }
	    else{
		my $str = $log_function->($best);
		print "best is: " . $str_function->($best) . ", cost: " . $best->{_f_cost}  . "\n";
	    }
	    #------------------------------------------------------


	    if($best->$goal_p()) {
		# goal achieved! iteration: $iteration, number of
		# states in queue: $num_states_in_queue.
		return $best;
	    }
	    elsif($best->{_f_cost} >= $max_cost){
		croak "\n\nSearch unsuccessful.  max_cost reached (cost:  $max_cost).\n";
	    }
	    else{
		my $successors_iterator = $best->$successors_func();
		my $succ = $successors_iterator->();

		if($succ){
		    # if succ is at max depth and is not a goal node, set succ->fcost to infinity
		    if($succ->depth() >= $max_depth && !$succ->$goal_p() ){
			$succ->{_f_cost} = $max_cost;
		    }
		    else{
			# calling eval for comparison, and maintaining pathmax property
			$succ->{_f_cost} = max($eval_func->($succ), $eval_func->($best));
			my $descendant_index = $succ->{_descendant_index};
			$best->{_descendant_fcosts}->[$descendant_index] = $succ->{_f_cost};
		    }
		}

		# determine if $best is completed, and if so backup values
		if($best->is_completed()){


		    # remove from queue first, back up fvals, then insert back on queue.
		    # this way, it gets placed in its rightful place on the queue.
		    my $fval_before_backup = $best->{_f_cost};

		    # STEPS:
		    # 1) remove best and all antecedents from queue, but only if they are
		    #    going to be altered by backing-up fvals.    This is because
		    #    removing and re-inserting in queue changes temporal ordering,
		    #    and we don't want to do that unless the node will be
		    #    placed in a new cost-bucket/tree.
		    # 2) then backup fvals
		    # 3) then re-insert best and all antecedents back on queue.


		    # Check if need for backup fvals
		    $best->check_need_fval_change();

		    my $cmp_func = sub {
			my ($str) = @_;
			return sub{
			    my ($obj) = @_;
			    my $obj_path_str = $str_function->($obj);
			    if($obj_path_str eq $str){
				return 1;
			    }
			    else{
				return 0;
			    }
			}
		    };

		    my $antecedent = $best->{_antecedent};
		    my %was_on_queue;
		    my $i = 0;

		    # Now remove the offending nodes from queue, if any
		    if($best->need_fval_change()){

			# remove best from the queue
			$best = $$priority_queue->deepest_lowest_cost_leaf();

			while($antecedent){
			    my $path_str = $str_function->($antecedent);

			    if($antecedent->is_on_queue() && $antecedent->need_fval_change()){
				$was_on_queue{$i} = 1;
				$$priority_queue->remove($antecedent, $cmp_func->($path_str));
			    }
			    $antecedent = $antecedent->{_antecedent};
			    $i++;
			}
		    }


		    #   Backup fvals
		    if($best->need_fval_change()){
			$best->$backup_func();
		    }


		    # Put everything back on the queue
		    if($best->need_fval_change()){
			$$priority_queue->insert($best);
			my $antecedent = $best->{_antecedent};
			my $i = 0;
			while($antecedent){
			    if($was_on_queue{$i} && $antecedent->need_fval_change()){
                                # the antecedent needed fval change too.
				$$priority_queue->insert($antecedent);
			    }
			    if($antecedent->need_fval_change()){
				# set need_fval_change back to 0, so it will not be automatically  seen as
				# needing changed in the future.  This is important, since we do not want
				# to remove an element from the queue *unless* we need to change the fcost.
				# This is because when we remove it from the queue and re-insert it, it
				# loses its seniority in the queue (it becomes the newest node at its cost
				# and depth) and will not be removed at the right time when searching for
				# deepest_lowest_cost_leafs or shallowest_highest_cost_leafs.
				$antecedent->{_need_fcost_change} = 0;
			    }

			    $antecedent = $antecedent->{_antecedent};
			    $i++;
			}
			# Again, set need_fval_change back to 0, so it will not be automatically
			# seen as needing changed in the future.
			$best->{_need_fcost_change} = 0;
		    }
		}


		#
		# If best's descendants are all in memory, mark best as completed.
                #
		if($best->all_in_memory()) {

		    if(!($best->is_completed())){
			$best->is_completed(1);
		    }

		    my $cmp_func = sub {
			my ($str) = @_;
			return sub{
			    my ($obj) = @_;
			    my $obj_str = $str_function->($obj);
			    if($obj_str eq $str){
				return 1;
			    }
			    else{
				return 0;
			    }
			}
		    };

		    my $best_str = $str_function->($best);

		    # If best is not a root node
		    if($best->{_depth} != 0){
			# descendant index is the unique index indicating which descendant
			# this node is of its antecedent.
			my $descendant_index = $best->{_descendant_index};
			my $antecedent = $best->{_antecedent};
			$$priority_queue->remove($best, $cmp_func->($best_str));
			if($antecedent){
			    $antecedent->{_descendants_produced}->[$descendant_index] = 0;
			}
		    }
		}

	        # there are no more successors of $best
		if(!$succ){
		    next;
		}

		my $antecedent;
		my @antecedents_that_need_to_be_inserted;

		# If the maximum number of states in the queue has been reached,
		# we need to remove the shallowest-highest-cost leaf to make room
		# for more nodes.   That means we have to make sure that the antecedent
		# produces this descendant again at some point in the future if needed.
		if($num_states_in_queue > $max_states_in_queue){
		    my $shcl_obj = $$priority_queue->shallowest_highest_cost_leaf($best, $succ, $str_function);

		    if(!$shcl_obj){
			croak "Error while pruning queue:   shallowest-highest-cost-leaf was null\n";
		    }
		    $antecedent = $shcl_obj->{_antecedent};
		    if($antecedent){
			my $antecedent_successors = \$antecedent->{_descendants_list};

			$antecedent->remember_forgotten_nodes_fcost($shcl_obj);
			$antecedent->{_forgotten_nodes_num} = $antecedent->{_forgotten_nodes_num} + 1;
			my $descendant_index = $shcl_obj->{_descendant_index};
		        # record the index of this descendant in the forgotten_nodes list
			$antecedent->{_forgotten_nodes_offsets}->{$descendant_index} = 1;
			# flag the antecedent as not having this descendant in the queue
			$antecedent->{_descendants_produced}->[$descendant_index] = 0;
			$antecedent->{_descendant_fcosts}->[$descendant_index] = -1;
			# flag the ancestor node as having deleted a descendant
			$antecedent->descendants_deleted(1);
			# update the number of descendants this node has in memory
			$antecedent->{_num_successors_in_mem} = $antecedent->{_num_successors_in_mem} - 1;
			# update the total number of nodes in the queue.
			$num_states_in_queue--;

		    }
		} # end if (num_states_on_queue > max_states)

		# if there is a successor to $best, insert it in the priority queue.
		if($succ){
		    $$priority_queue->insert($succ);
		    $best->{_num_successors_in_mem} = $best->{_num_successors_in_mem} + 1;
		}
		else{
		    croak "Error:  no successor to insert\n";
		}
	    }
	}
	continue {
	    $iteration++;
	}

	print "\n\nreturning unsuccessfully.   iteration: $iteration\n";
	return;
    }
}




sub max
{
    my ($n1, $n2) = @_;
    return ($n1 > $n2 ? $n1 : $n2);
}


sub fp_compare {
    my ($a, $b, $dp) = @_;
    my $a_seq = sprintf("%.${dp}g", $a);
    my $b_seq = sprintf("%.${dp}g", $b);



    if($a_seq eq $b_seq){
	return 0;
    }
    elsif($a_seq lt $b_seq){
	return -1;
    }
    else{
	return 1;
    }
}





1;
__END__