146 lines
3.3 KiB
Plaintext
146 lines
3.3 KiB
Plaintext
|
|
# Write to matrix
|
|
robot_marker = "X"
|
|
|
|
# copy a full matrix row
|
|
function mat_copyrow(out,ro,in,ri){
|
|
out[ro] = {}
|
|
var icr = 1
|
|
while(icr <= size(in[ri])) {
|
|
out[ro][icr]=in[ri][icr]
|
|
icr = icr + 1
|
|
}
|
|
}
|
|
|
|
function getvec(t,row){
|
|
return math.vec2.new(t[row][1],t[row][2])
|
|
}
|
|
|
|
function init_test_map(len){
|
|
map = {}
|
|
var indexR = 1
|
|
while(indexR <= len) {
|
|
map[indexR] = {}
|
|
var indexC = 1
|
|
while(indexC <= len) {
|
|
map[indexR][indexC] = 1.0
|
|
indexC = indexC + 1
|
|
}
|
|
indexR = indexR + 1
|
|
}
|
|
# DEBUG\TEST: puts an obstacle right in the middle
|
|
map[5][5] = 0.0
|
|
map[6][5] = 0.0
|
|
map[4][5] = 0.0
|
|
|
|
log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.")
|
|
}
|
|
|
|
function init_map(len){
|
|
map = {}
|
|
var indexR = 1
|
|
while(indexR <= len) {
|
|
map[indexR] = {}
|
|
var indexC = 1
|
|
while(indexC <= len) {
|
|
map[indexR][indexC] = 1.0
|
|
indexC = indexC + 1
|
|
}
|
|
indexR = indexR + 1
|
|
}
|
|
log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
|
|
}
|
|
|
|
function add_obstacle(pos, off, inc_trust) {
|
|
var xi = math.round(pos.x)
|
|
var yi = math.round(pos.y)
|
|
|
|
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
|
|
#log("Add obstacle in cell: ", xi, " ", yi)
|
|
var old=map[xi][yi]
|
|
if(old-inc_trust > 0.0)
|
|
map[xi][yi] = old-inc_trust
|
|
else
|
|
map[xi][yi] = 0.0
|
|
}
|
|
}
|
|
|
|
function remove_obstacle(pos, off, dec_trust) {
|
|
var xi = math.round(pos.x)
|
|
var yi = math.round(pos.y)
|
|
|
|
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
|
|
#log("Remove obstacle in cell: ", xi, " ", yi)
|
|
var old=map[xi][yi]
|
|
if(old + dec_trust < 1.0) #x,y
|
|
map[xi][yi] = old+dec_trust
|
|
else
|
|
map[xi][yi] = 1.0
|
|
}
|
|
}
|
|
|
|
|
|
function get_occupied_cells(cur_cell){
|
|
var i = 1
|
|
var occupied_cells = {}
|
|
occupied_cells[0] = cur_cell
|
|
occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
|
|
occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
|
|
occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
|
|
occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
|
|
return occupied_cells
|
|
}
|
|
|
|
function is_in(dictionary, x, y){
|
|
var i = 0
|
|
while(i < size(dictionary)){
|
|
if(dictionary[i].x == x and dictionary[i].y == y){
|
|
return 1
|
|
}
|
|
i = i + 1
|
|
}
|
|
return 0
|
|
}
|
|
|
|
|
|
function table_print(t) {
|
|
foreach(t, function(key, value) {
|
|
log(key, " -> ", value)
|
|
})
|
|
}
|
|
|
|
function table_copy(t) {
|
|
var t2 = {}
|
|
foreach(t, function(key, value) {
|
|
t2[key] = value
|
|
})
|
|
return t2
|
|
}
|
|
|
|
function print_pos(t) {
|
|
var ir=1
|
|
while(ir <= size(t)) {
|
|
log(ir, ": ", t[ir][1], " ", t[ir][2])
|
|
ir = ir + 1
|
|
}
|
|
}
|
|
|
|
function print_map(t) {
|
|
var ir=size(t)
|
|
log("Printing a ", size(t), " by ", size(t[1]), " map")
|
|
while(ir > 0) {
|
|
logst=string.concat("\t", string.tostring(ir), "\t:")
|
|
ic = size(t[ir])
|
|
while(ic > 0) {
|
|
if(ir==cur_cell.x and ic==cur_cell.y)
|
|
logst = string.concat(logst, " XXXXXXXX")
|
|
else
|
|
logst = string.concat(logst, " ", string.tostring(t[ir][ic]))
|
|
ic = ic - 1
|
|
}
|
|
log(logst)
|
|
ir = ir - 1
|
|
}
|
|
export_map(map)
|
|
}
|