Path finding using A* - Unexpected results
for of familiar a* pathfinding algorithm, i'm using euclidean distance method hueristic (h), g value of 1, , can move up/down/left/right , not diagonally. if @ visited array, can see robot should follow 0 values starting location (0,0) goal location (5,6). reason when gets (5,3) (h=3.00) , evaluating between moving (f=g+h=1+3.16=4.16) or right (f=g+h=1+52=53), moves right. decision making done in below loop, , should work out following:
// 0 index up. 1 right. 2 down. 3 left.
costarray[0]= 3.16+1=4.16
costarray[1]=52+1=53
costarray[2]=53.16+1=54.16
//finding node least cost
for(int i=0;i<4;i++)
{ if(costarray < maxval)
{index = i;
maxval = costarray;}}
is familiar a* can see why isn't working out way i'd expect to.
// 0 index up. 1 right. 2 down. 3 left.
costarray[0]= 3.16+1=4.16
costarray[1]=52+1=53
costarray[2]=53.16+1=54.16
//finding node least cost
for(int i=0;i<4;i++)
{ if(costarray < maxval)
{index = i;
maxval = costarray;}}
is familiar a* can see why isn't working out way i'd expect to.
op's code, posted properly:
note: following line test of cost, , merely prints empty line. intended?
code: [select]
#include <math.h>
// declaring grid , variations ( visited , hueristic , dynamic path )
//int grid[7][8]= {{1,1,1,1,1,1,1,1},
// {1,0,1,0,0,0,0,1},
// {1,0,1,0,0,0,0,1},
// {1,0,1,0,0,0,0,1},
// {1,0,1,0,0,1,0,1},
// {1,0,0,0,0,1,0,1},
// {1,1,1,1,1,1,1,1},};
int visited[7][8]={{1,1,1,1,1,1,1,1},
{1,0,1,0,0,0,0,1},
{1,0,1,0,1,0,0,1},
{1,0,1,0,1,0,0,1},
{1,0,1,0,1,1,0,1},
{1,0,0,0,1,1,0,1},
{1,1,1,1,1,1,1,1},};
double hueristic[7][8];
// declaring start goal , various other variables
int startx=1;
int starty=1;
int goalx=5;
int goaly=6;
int locx;
int locy;
int index;
double costarray[4] ;
double f;
int g = 1;
int maxval = 100;
void setup()
{
serial.begin(115200);
//calculating hueristic
if((goaly < 8 && goalx < 7 ) || (goalx > -1 && goaly > -1 ) )
{for(int = 0; <8; i++)
{for(int j = 0; j <7; j++)
{hueristic[j][i] = sqrt( ((j-goalx)*(j-goalx)) + ((i-goaly)*(i-goaly)) );
if (visited[j][i] == 1)
{
hueristic[j][i] = hueristic[j][i] + 50 ;
}}}}
//------------------------------------------------// uncomment region view hueristic grid
for(int = 0; <7; i++)
{for(int j = 0; j <8; j++)
{
serial.print(hueristic[i][j]);
serial.print(" ");
}
serial.println();
}
// for(;;);
//------------------------------------------------
locx = startx;
locy = starty;
}
void loop()
{ // 0 index up. 1 right. 2 down. 3 left. overhere assign our costs , prioritize our robots movement.
costarray[0] = g + hueristic[locx-1][locy];
costarray[1] = g + hueristic[locx][locy+1];
costarray[2] = g + hueristic[locx+1][locy];
costarray[3] = g + hueristic[locx][locy-1];
//----------------------------------------------------------------------------------------------------------------------
//finding node least cost
for(int i=0;i<4;i++)
{ if(costarray[i] < maxval)
serial.println();
serial.print(costarray[i]);
serial.println();
{index = i;
maxval = costarray[i];}}
//----------------------------
visited[locx][locy]=1;
//serial.print(index);
//serial.println();
//move position new location
if(index == 0)
{locx = locx-1;
}
else if(index == 1)
{locy = locy+1;
}
else if(index == 2)
{locx = locx+1;
}
else if(index == 3)
{locy = locy-1;
}
serial.print("(");
serial.print(locx);
serial.print(",");
serial.print(locy);
serial.print(")");
serial.println();
if(locx == goalx && locy == goaly)
{serial.println("target found ya piece of horse dung ");
for(;;);
}
}
note: following line test of cost, , merely prints empty line. intended?
code: [select]
if(costarray[i] < maxval)
serial.println();
Arduino Forum > Using Arduino > Programming Questions > Path finding using A* - Unexpected results
arduino
Comments
Post a Comment