526 lines
19 KiB
Plaintext
526 lines
19 KiB
Plaintext
#####
|
|
# RRT* Path Planing
|
|
#
|
|
# map table-based matrix
|
|
#####
|
|
include "plan/mapmatrix.bzz"
|
|
|
|
RRT_TIMEOUT = 500
|
|
RRT_RUNSTEP = 3
|
|
PROX_SENSOR_THRESHOLD_M = 8.0
|
|
GSCALE = {.x=1, .y=1}
|
|
map = nil
|
|
cur_cell = {}
|
|
nei_cell = {}
|
|
mapgoal = {}
|
|
|
|
|
|
#
|
|
# Work in progress....
|
|
function navigate() {
|
|
BVMSTATE="NAVIGATE"
|
|
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
|
|
storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
|
|
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
|
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
|
}
|
|
|
|
print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
|
|
if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
|
|
log("Sorry this is too far.")
|
|
return
|
|
}
|
|
|
|
# create the map
|
|
if(map==nil) {
|
|
dyn_init_map(cur_goal)
|
|
if(V_TYPE == 0) {
|
|
homegps.lat = pose.position.latitude
|
|
homegps.long = pose.position.longitude
|
|
}
|
|
}
|
|
|
|
if(Path==nil) {
|
|
# add proximity sensor and neighbor obstacles to the map
|
|
populateGrid(m_pos)
|
|
# start the planner
|
|
path_it = 1
|
|
pathPlanner(cur_goal, m_pos)
|
|
} else if(path_it <= size(Path)) {
|
|
var cur_path_pt = convert_pt(getvec(Path, path_it))
|
|
print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
|
|
if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
|
|
populateGrid(m_pos)
|
|
if(check_path(Path, path_it, m_pos, 0)) {
|
|
# stop
|
|
goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0)
|
|
Path = nil
|
|
if(V_TYPE == 0)
|
|
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
|
# re-start the planner
|
|
path_it = 1
|
|
pathPlanner(cur_goal, m_pos)
|
|
} else {
|
|
cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
|
|
goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0)
|
|
}
|
|
} else
|
|
path_it = path_it + 1
|
|
} else {
|
|
Path = nil
|
|
BVMSTATE="IDLE"
|
|
}
|
|
}
|
|
|
|
# create a map with its length to fit the goal (rel. pos.) and the current position as the center
|
|
# TODO: test with khepera/argos
|
|
function dyn_init_map(m_navigation) {
|
|
# create a map big enough for the goal
|
|
init_map(math.round(2*math.vec2.length(m_navigation))+10)
|
|
# center the robot on the grid
|
|
cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
|
|
}
|
|
|
|
# start the RRT* to reach the goal (rel. pos.)
|
|
# TODO: test with khepera/argos
|
|
function pathPlanner(m_goal, m_pos, kh4) {
|
|
# place the robot on the grid
|
|
cur_cell = getcell(m_pos)
|
|
# create the goal in the map grid
|
|
mapgoal = getcell(math.vec2.add(m_goal, m_pos))
|
|
|
|
#print_map(map)
|
|
export_map(map)
|
|
|
|
# Initialize RRTStar var
|
|
log("--------Initialize RRTStar--------------")
|
|
HEIGHT = size(map)
|
|
WIDTH = size(map[1])
|
|
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
|
|
goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0}
|
|
#table_print(goalBoundary)
|
|
numberOfPoints = 1
|
|
arrayOfPoints = {}
|
|
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
|
|
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
|
|
Q = {}
|
|
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0}
|
|
goalReached = 0
|
|
timeout = 0
|
|
|
|
# search for a path
|
|
old_statef = statef
|
|
rrtstar()
|
|
}
|
|
|
|
# get the grid cell position (x,y) equivalent to the input position
|
|
# input should be relative to home position (planing start point)
|
|
function getcell(m_curpos) {
|
|
var cell = {}
|
|
# relative to center (start position)
|
|
cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
|
|
var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y)
|
|
cell = math.vec2.add(cell, offset)
|
|
cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
|
|
|
|
if(cell.x > size(map))
|
|
cell.x = size(map)
|
|
if(cell.y > size(map[1]))
|
|
cell.y = size(map[1])
|
|
if(cell.x < 1)
|
|
cell.x = 1
|
|
if(cell.y < 1)
|
|
cell.y = 1
|
|
|
|
return cell
|
|
}
|
|
|
|
function populateGrid(m_pos) {
|
|
# getproxobs(m_pos)
|
|
getneiobs (m_pos)
|
|
export_map(map)
|
|
}
|
|
|
|
# TODO: populate the map with neighors as blobs instead ?
|
|
function getneiobs (m_curpos) {
|
|
cur_cell = getcell(m_curpos)
|
|
# add all neighbors as obstacles in the grid
|
|
neighbors.foreach(function(rid, data) {
|
|
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
|
|
})
|
|
}
|
|
|
|
# old function tested with the khepera for reference
|
|
# DS 20/11
|
|
# function getneiobs (m_curpos) {
|
|
# var foundobstacle = 0
|
|
# cur_cell = getcell(m_curpos)
|
|
# var old_nei = table_copy(nei_cell)
|
|
|
|
# neighbors.foreach(function(rid, data) {
|
|
# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
|
|
# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
|
# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
|
|
# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
|
|
# if(old_nei[rid]!=nil) {
|
|
# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
|
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
|
|
# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
|
|
# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
|
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
|
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
|
|
# add_obstacle(Ncell, 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
|
# foundobstacle = 1
|
|
# }
|
|
# } else {
|
|
# add_obstacle(Ncell, 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
|
# foundobstacle = 1
|
|
# }
|
|
# })
|
|
# return foundobstacle
|
|
# }
|
|
|
|
# populate a line in front of the sensor using plateform independant proximity sensing
|
|
# TODO: adapt to M100 stereo
|
|
function getproxobs (m_curpos) {
|
|
var updated_obstacle = 0
|
|
cur_cell = getcell(m_curpos)
|
|
|
|
reduce(proximity,
|
|
function(key, value, acc) {
|
|
if (value.angle != -1) { # down sensor of M100
|
|
if(value.value < PROX_SENSOR_THRESHOLD_M) {
|
|
obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw)))
|
|
per = math.vec2.sub(obs,cur_cell)
|
|
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
|
|
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
|
obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
|
|
obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
|
# obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs)))
|
|
# obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
|
|
# obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
|
|
if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
|
|
add_obstacle(obs, 0, 0.25)
|
|
add_obstacle(obsr, 0, 0.25)
|
|
add_obstacle(obsl, 0, 0.25)
|
|
add_obstacle(obsr2, 0, 0.25)
|
|
add_obstacle(obsl2, 0, 0.25)
|
|
# add_obstacle(obs2, 0, 0.25)
|
|
# add_obstacle(obsr2, 0, 0.25)
|
|
# add_obstacle(obsl2, 0, 0.25)
|
|
updated_obstacle = 1
|
|
}
|
|
} else {
|
|
var line_length = PROX_SENSOR_THRESHOLD_M;
|
|
while(line_length > 0) {
|
|
obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw)))
|
|
per = math.vec2.sub(obs,cur_cell)
|
|
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
|
|
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
|
obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
|
|
obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
|
remove_obstacle(obs, 0, 0.05)
|
|
remove_obstacle(obsr, 0, 0.05)
|
|
remove_obstacle(obsl, 0, 0.05)
|
|
line_length = line_length - 1;
|
|
}
|
|
}
|
|
}
|
|
return acc
|
|
}, math.vec2.new(0, 0))
|
|
|
|
return updated_obstacle
|
|
}
|
|
|
|
# check if any obstacle blocks the way
|
|
# TODO: remove the kh4 bool for retro-compatilibty
|
|
function check_path(m_path, goal_l, m_curpos, kh4) {
|
|
var pathisblocked = 0
|
|
var nb = goal_l
|
|
cur_cell = getcell(m_curpos)
|
|
var Cvec = cur_cell
|
|
while(nb <= size(m_path) and nb <= goal_l+1) {
|
|
var Nvec = getvec(m_path, nb)
|
|
if(kh4 == 0) {
|
|
Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
|
|
Nvec = getcell(Nvec)
|
|
}
|
|
if(doesItIntersect(Cvec, Nvec)) {
|
|
log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
|
|
pathisblocked = 1
|
|
}
|
|
Cvec = Nvec
|
|
nb = nb + 1
|
|
}
|
|
|
|
return pathisblocked
|
|
}
|
|
|
|
##
|
|
# main search loop as a state
|
|
##
|
|
function rrtstar() {
|
|
# update state machine variables
|
|
BVMSTATE = "PATHPLAN"
|
|
|
|
|
|
step_timeout = 0
|
|
while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {
|
|
|
|
# Point generation only as grid cell centers
|
|
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
|
|
#log("Point generated: ", pt.x, " ", pt.y)
|
|
|
|
var pointList = findPointsInRadius(pt,Q,RADIUS);
|
|
|
|
# Find connection that provides the least cost to come
|
|
nbCount = 0;
|
|
minCounted = 999;
|
|
minCounter = 0;
|
|
|
|
if(size(pointList) != 0) {
|
|
#log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3])
|
|
ipt=1
|
|
while(ipt <= size(pointList)) {
|
|
pointNumber = {}
|
|
mat_copyrow(pointNumber,1,pointList,ipt)
|
|
|
|
# Follow the line to see if it intersects anything
|
|
intersects = doesItIntersect(pt,getvec(pointNumber,1));
|
|
#log("intersects1: ", intersects)
|
|
|
|
# If there is no intersection we need consider its connection
|
|
nbCount = nbCount + 1;
|
|
if(intersects != 1) {
|
|
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
|
|
var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5]
|
|
|
|
if(distance < minCounted) {
|
|
minCounted = distance;
|
|
minCounter = nbCount;
|
|
}
|
|
}
|
|
ipt = ipt + 1
|
|
}
|
|
if(minCounter > 0) {
|
|
numberOfPoints = numberOfPoints + 1;
|
|
arrayOfPoints[numberOfPoints] = {}
|
|
arrayOfPoints[numberOfPoints][1]=pt.x
|
|
arrayOfPoints[numberOfPoints][2]=pt.y
|
|
|
|
Q[numberOfPoints] = {}
|
|
Q[numberOfPoints][1] = pt.x
|
|
Q[numberOfPoints][2] = pt.y
|
|
Q[numberOfPoints][3] = pointList[minCounter][4]
|
|
Q[numberOfPoints][4] = numberOfPoints
|
|
Q[numberOfPoints][5] = minCounted
|
|
|
|
#log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
|
|
|
|
# Now check to see if any of the other points can be redirected
|
|
nbCount = 0;
|
|
ipt = 1
|
|
while(ipt <= size(pointList)) {
|
|
pointNumber = {}
|
|
mat_copyrow(pointNumber,1,pointList,ipt)
|
|
#log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])
|
|
|
|
# Follow the line to see if it intersects anything
|
|
intersects = doesItIntersect(pt,getvec(pointNumber,1));
|
|
#log("intersects2: ", intersects)
|
|
|
|
# If there is no intersection we need consider its connection
|
|
nbCount = nbCount + 1;
|
|
if(intersects != 1) {
|
|
# If the alternative path is shorter than change it
|
|
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
|
|
#log("Q: ", size(Q), "x", size(Q[1]))
|
|
if(tmpdistance < Q[pointNumber[1][4]][5]) {
|
|
Q[pointNumber[1][4]][3] = numberOfPoints
|
|
Q[pointNumber[1][4]][5] = tmpdistance
|
|
}
|
|
}
|
|
ipt = ipt + 1
|
|
}
|
|
|
|
# Check to see if this new point is within the goal
|
|
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
|
|
goalReached = 1;
|
|
}
|
|
} else {
|
|
# Associate with the closest point
|
|
pointNum = findClosestPoint(pt,arrayOfPoints);
|
|
|
|
# Follow the line to see if it intersects anything
|
|
intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum));
|
|
#log("intersects3 (", pointNum, "): ", intersects)
|
|
|
|
# If there is no intersection we need to add to the tree
|
|
if(intersects != 1) {
|
|
numberOfPoints = numberOfPoints + 1;
|
|
arrayOfPoints[numberOfPoints] = {}
|
|
arrayOfPoints[numberOfPoints][1]=pt.x
|
|
arrayOfPoints[numberOfPoints][2]=pt.y
|
|
|
|
Q[numberOfPoints] = {}
|
|
Q[numberOfPoints][1] = pt.x
|
|
Q[numberOfPoints][2] = pt.y
|
|
Q[numberOfPoints][3] = pointNum
|
|
Q[numberOfPoints][4] = numberOfPoints
|
|
Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))
|
|
|
|
#log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
|
|
|
|
# Check to see if this new point is within the goal
|
|
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
|
|
goalReached = 1;
|
|
}
|
|
}
|
|
if(numberOfPoints % 100 == 0) {
|
|
log(numberOfPoints, " points processed. Still looking for goal.");
|
|
}
|
|
timeout = timeout + 1
|
|
step_timeout = step_timeout + 1
|
|
}
|
|
|
|
if(goalReached){
|
|
log("Goal found(",numberOfPoints,")!")
|
|
Path = convert_path(getPath(Q,numberOfPoints))
|
|
# done. resume the state machine
|
|
BVMSTATE = "NAVIGATE"
|
|
} else if(timeout >= RRT_TIMEOUT) {
|
|
log("FAILED TO FIND A PATH!!!")
|
|
Path = nil
|
|
# done. resume the state machine
|
|
BVMSTATE = "NAVIGATE"
|
|
}
|
|
}
|
|
|
|
# Go through each points and find the distances between them and the
|
|
# target point
|
|
function findClosestPoint(point,aPt) {
|
|
var distance = 999
|
|
var pointNb = -1
|
|
var ifcp=1
|
|
while(ifcp <= size(aPt)) {
|
|
var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
|
|
|
|
if(range < distance) {
|
|
distance = range;
|
|
pointNb = ifcp;
|
|
}
|
|
ifcp = ifcp + 1
|
|
}
|
|
return pointNb
|
|
}
|
|
|
|
# Find the closest point in the tree
|
|
function findPointsInRadius(point,q,r) {
|
|
var ptList = {}
|
|
var counted = 0;
|
|
var iir = 1
|
|
while(iir <= size(q)) {
|
|
var tmpvv = getvec(q,iir)
|
|
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
|
|
var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
|
|
|
|
if(distance < r) {
|
|
counted = counted+1;
|
|
mat_copyrow(ptList,counted,q,iir)
|
|
}
|
|
|
|
iir = iir + 1
|
|
}
|
|
return ptList
|
|
}
|
|
|
|
# check if the line between 2 point intersect an obstacle
|
|
function doesItIntersect(point,vector) {
|
|
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
|
|
var dif = math.vec2.sub(point,vector)
|
|
var distance = math.vec2.length(dif)
|
|
if(distance == 0.0){
|
|
# Find what block we're in right now
|
|
var xi = math.round(point.x) #+1
|
|
var yi = math.round(point.y) #+1
|
|
if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
|
|
if(map[xi][yi] > 0.5)
|
|
return 1
|
|
else
|
|
return 0
|
|
} else
|
|
return 0
|
|
}
|
|
var vec = math.vec2.scale(dif,1.0/distance)
|
|
var pathstep = size(map) - 2
|
|
|
|
idii = 1.0
|
|
while(idii <= pathstep) {
|
|
var range = distance*idii/pathstep
|
|
var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
|
|
|
|
# Find what block we're in right now
|
|
var xi = math.round(pos_chk.x) #+1
|
|
var yi = math.round(pos_chk.y) #+1
|
|
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
|
|
|
|
# TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross
|
|
if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
|
|
#if(xi!=cur_cell.x and yi!=cur_cell.y) {
|
|
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
|
|
if(map[xi][yi] < 0.5) { # because obstacle use trust values
|
|
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
|
|
return 1;
|
|
}
|
|
} else {
|
|
#log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
|
|
return 1;
|
|
}
|
|
}
|
|
idii = idii + 1.0
|
|
}
|
|
#log("No intersect!")
|
|
return 0
|
|
}
|
|
|
|
# create a table with only the path's points in order
|
|
function getPath(Q,nb){
|
|
var pathL={}
|
|
var npt=0
|
|
# get the path from the point list
|
|
while(nb > 0) {
|
|
npt=npt+1
|
|
pathL[npt] = {}
|
|
pathL[npt][1]=Q[nb][1]
|
|
pathL[npt][2]=Q[nb][2]
|
|
if( nb > 1) {
|
|
if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening?
|
|
log("ERROR - Recursive path !!!")
|
|
return nil
|
|
}
|
|
}
|
|
nb = Q[nb][3];
|
|
}
|
|
|
|
# Re-Order the list.
|
|
var pathR={}
|
|
var totpt = npt + 1
|
|
while(npt > 0){
|
|
pathR[totpt-npt] = {}
|
|
var tmpgoal = getvec(pathL,npt)
|
|
pathR[totpt-npt][1]=tmpgoal.x
|
|
pathR[totpt-npt][2]=tmpgoal.y
|
|
npt = npt - 1
|
|
}
|
|
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
|
|
return pathR
|
|
}
|