I'm attempting to create an implementation of the A* pathfinding algorithm in TorqueScript with pre-calculated node neighbors. I have neighbor detection working, but the actual pathfinding simply isn't working (returning "").
Any help would be greatly appreciated.
function findPath( %nodes, %start, %goal )
{
%closedset = listObject();
%openset = listObject(); %openset.fillWords( %nodes );
%came_from = dictObject();
%g_score[ %start ] = 0;
%h_score[ %start ] = path_dist( %start, %goal );
%f_score[ %start ] = %g_score[ %start ] + %h_score[ %start ];
while ( %cnt = %openset.size() )
{
%x = -1;
for ( %i = 0 ; %i < %cnt ; %i++ )
{
%obj = %openset.get( %i );
%val = ( strLen( %val = %f_score[ %obj ] ) ? %val : 0 );
if ( !strLen( %min ) || %val < %min )
{
%min = %val;
%x = %obj;
}
}
if ( %x == %goal )
{
%rec = reconstruct_path( %came_from, %came_from.get( %goal ) );
%came_from.delete();
%openset.delete();
%closedset.delete();
return %rec;
}
%openset.remove( %x );
%closedset.add( %x );
%nbrs = neighbor_nodes( %x );
%cnt = getWordCount( %nbrs );
if ( strLen( %nbrs ) && %cnt > 0 ) for ( %i = 0 ; %i < %cnt ; %i++ )
{
%y = getWord( %nbrs, %i );
if ( %closedset.has( %y ) )
continue;
%tentative_g_score = %g_score[ %x ] + path_dist( %x, %y );
if ( !%openset.has( %y ) )
{
%openset.add( %y );
%tentative_is_better = true;
}
else if ( %tentative_g_score < %g_score[ %y ] )
%tentative_is_better = true;
else
%tentative_is_better = false;
if ( %tentative_is_better )
{
%came_from.set( %y, %x );
%g_score[ %y ] = %tentative_g_score;
%h_score[ %y ] = path_dist( %y, %goal );
%f_score[ %y ] = %g_score[ %y ] + %h_score[ %y ];
}
}
}
%closedset.delete();
%openset.delete();
%came_from.delete();
return "failure";
}
// function A*(start,goal)
// closedset := the empty set // The set of nodes already evaluated.
// openset := {start} // The set of tentative nodes to be evaluated, initially containing the start node
// came_from := the empty map // The map of navigated nodes.
//
// g_score[start] := 0 // Cost from start along best known path.
// h_score[start] := heuristic_cost_estimate(start, goal)
// f_score[start] := g_score[start] + h_score[start] // Estimated total cost from start to goal through y.
//
// while openset is not empty
// x := the node in openset having the lowest f_score[] value
// if x = goal
// return reconstruct_path(came_from, came_from[goal])
//
// remove x from openset
// add x to closedset
// foreach y in neighbor_nodes(x)
// if y in closedset
// continue
// tentative_g_score := g_score[x] + dist_between(x,y)
//
// if y not in openset
// add y to openset
// tentative_is_better := true
// else if tentative_g_score < g_score[y]
// tentative_is_better := true
// else
// tentative_is_better := false
//
// if tentative_is_better = true
// came_from[y] := x
// g_score[y] := tentative_g_score
// h_score[y] := heuristic_cost_estimate(y, goal)
// f_score[y] := g_score[y] + h_score[y]
//
// return failure
function reconstruct_path( %came_from, %current_node )
{
if ( %came_from.has( %current_node ) )
{
%p = reconstruct_path( %came_from, %came_from.get( %current_node ) );
return %p SPC %current_node;
}
else
return %current_node;
}
// function reconstruct_path(came_from, current_node)
// if came_from[current_node] is set
// p = reconstruct_path(came_from, came_from[current_node])
// return (p + current_node)
// else
// return current_node
function path_dist( %a, %b )
{
return mAbs( vectorDist( %a.position, %b.position ) );
}
function neighbor_nodes( %a )
{
return %a.connections;
}
datablock fxDTSBrickData( brickNodeData : brick2x2fData )
{
category = "Special";
subCategory = "Interactive";
uiName = "Node Brick";
isNodeBrick = true;
};
function nodeGroupSG::findPath( %this, %start, %end )
{
return findPath( %this.getNodes(), %start, %end );
}
function nodeGroupSG::getNodes( %this )
{
%cnt = %this.getCount();
%str = "";
for ( %i = 0 ; %i < %cnt ; %i++ )
%str = %str @ ( strLen( %str ) ? " " : "" ) @ %this.getObject( %i );
return %str;
}
function nodeCanSee( %obj, %col )
{
%start = %obj.position;
%end = %col.position;
%mask = $TypeMasks::All;
%avoid = %obj.brick;
%ray = containerRayCast( %start, %end, %mask, %avoid );
return ( %col.brick == objFromRayCast( %ray ) );
}
function scriptObject::updateConns( %this )
{
%this.connections = "";
%cnt = %this.getGroup().getCount();
for ( %i = 0 ; %i < %cnt ; %i++ )
{
%col = %this.getGroup().getObject( %i );
if ( %col == %this )
continue;
if ( nodeCanSee( %this, %col ) )
{
%this.connections = %this.connections @ ( strLen( %this.connections ) ? " " : "" ) @ %col;
if ( !isEventPending( %col.brick.restoreRender ) )
{
%col.brick.setRendering( 1 );
%col.brick.restoreRender = %col.brick.schedule( 1000, setRendering, 0 );
%col.schedule( 100, updateConns );
}
}
}
}
package pathfinding_package
{
function fxDTSBrick::onPlant( %this )
{
parent::onPlant( %this );
if ( %this.isPlanted && %this.getDataBlock().isNodeBrick
&& !isObject( %this.nodeObj )
&& isObject( %bg = %this.getGroup() ) )
{
%this.nodeObj = new scriptObject()
{
brick = %this;
position = %this.getPosition();
numConnections = 0;
};
if ( !isObject( %ng = %bg.nodeGroup ) )
{
%ng = %bg.nodeGroup = new scriptGroup()
{
class = "nodeGroupSG";
brickGroup = %bg;
client = %this.client;
name = %bg.name;
bl_id = %bg.bl_id;
};
}
%ng.add( %this.nodeObj );
%this.nodeObj.schedule( 500, updateConns );
%this.setRendering( false );
%this.setColliding( false );
%this.setRayCasting( true );
}
}
function fxDTSBrick::onLoadPlant( %this )
{
parent::onLoadPlant( %this );
if ( %this.isPlanted && %this.getDataBlock().isNodeBrick
&& !isObject( %this.nodeObj )
&& isObject( %bg = %this.getGroup() ) )
{
%this.nodeObj = new scriptObject()
{
brick = %this;
position = %this.getPosition();
numConnections = 0;
};
if ( !isObject( %ng = %bg.nodeGroup ) )
{
%ng = %bg.nodeGroup = new scriptGroup()
{
class = "nodeGroupSG";
brickGroup = %bg;
client = %this.client;
name = %bg.name;
bl_id = %bg.bl_id;
};
}
%ng.add( %this.nodeObj );
%this.nodeObj.schedule( 500, updateConns );
%this.setRendering( false );
%this.setColliding( false );
%this.setRayCasting( true );
}
}
function fxDTSBrick::onRemove( %this )
{
if ( isObject( %obj = %this.nodeObj ) )
%obj.delete();
parent::onRemove( %this );
}
};
activatePackage( pathfinding_package );
