From 37c3cf151b12a41e4190d0a7ec454909191ed656 Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Thu, 28 Sep 2017 18:49:13 -0400
Subject: [PATCH] new rtt* algo

---
 buzz_scripts/graphformGPS.bzz    |   2 +-
 buzz_scripts/include/rttstar.bzz | 233 +++++++++++++++++++++++++++++++
 buzz_scripts/testalone.bzz       |   3 +
 src/buzzuav_closures.cpp         |   4 +-
 4 files changed, 239 insertions(+), 3 deletions(-)
 create mode 100644 buzz_scripts/include/rttstar.bzz

diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz
index 324c52e..f3b0bd5 100644
--- a/buzz_scripts/graphformGPS.bzz
+++ b/buzz_scripts/graphformGPS.bzz
@@ -729,7 +729,7 @@ function init() {
   #v_tag = stigmergy.create(m_lockstig)
   #uav_initstig()
 	# go to diff. height since no collision avoidance implemented yet
-	TARGET_ALTITUDE = 3.0 + id * 1.25
+	#TARGET_ALTITUDE = 3.0 + id * 1.25
 	statef=turnedoff
 	UAVSTATE = "TURNEDOFF"
 }
diff --git a/buzz_scripts/include/rttstar.bzz b/buzz_scripts/include/rttstar.bzz
new file mode 100644
index 0000000..cfd4924
--- /dev/null
+++ b/buzz_scripts/include/rttstar.bzz
@@ -0,0 +1,233 @@
+#####
+# RRT* Path Planing
+# START, GOAL, TOL vec2
+# map table-based matrix
+#####
+
+map = {.nb_col=100, .nb_row=100, .mat={}}
+
+function RRTSTAR(START,GOAL,TOL) {
+    # RRT_STAR
+    HEIGHT = map.nb_col
+    WIDTH = map.nb_row
+    RADIUS = 2
+    
+    rng.setseed(11)
+
+    goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
+    arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=START.x,.1=START.y}}
+    numberOfPoints = 1
+    Q = {.nb_col=5,.nb_row=1,.mat={.0=START.x,.1=START.y,.2=0,.3=1,.4=0}}
+    log("Created ",Q)
+
+    # Open space or obstacle
+    # DISPLAY_patchwork(map);
+    
+    goalReached = 0;
+    while(goalReached == 0) {
+        # Point generation
+        x = -HEIGHT*rng.uniform(1.0)-1;
+        y = WIDTH*rng.uniform(1.0)+1;
+        log("First trial point (", rng.uniform(1.0), "): ", x, " ", y)
+
+        pointList = findPointsInRadius(x,y,Q,RADIUS);
+        
+        log(pointList.nb_col,pointList.nb_row,pointList.mat)
+
+        # Find connection that provides the least cost to come
+        nbCount = 0;
+        minCounted = inf;
+        minCounter = 0;
+        
+        if(pointList.nb_col!=0) {
+            i=0
+            while(i<pointList.nb_row){
+                pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
+                mat_copyrow(pointNumber,0,pointList,i)
+
+                # Follow the line to see if it intersects anything
+                intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
+
+                # If there is no intersection we need consider its connection
+                nbCount = nbCount + 1;
+                if(intersects != 1) {
+                    log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
+                    distance = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,pointNumber.mat[3],5)
+
+                    if(distance < minCounted) {
+                        minCounted = distance;
+                        minCounter = nbCount;
+                    }
+                }
+            }
+            if(minCounter > 0) {
+                arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
+                arrayOfPoints.mat[arrayOfPoints.nb_row]=x
+                arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
+                numberOfPoints = numberOfPoints + 1;
+
+                wmat(Q,numberOfPoints,0, x)
+                wmat(Q,numberOfPoints,1, y)
+                wmat(Q,numberOfPoints,2, rmat(pointList,minCounter,4));
+                wmat(Q,numberOfPoints,3, numberOfPoints)
+                wmat(Q,numberOfPoints,4, minCounted)
+
+                # Now check to see if any of the other points can be redirected
+                nbCount = 0;
+                i = 0
+                while(i<pointList.nb_col) {
+                    pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
+                    mat_copyrow(pointNumber,0,pointList,i)
+
+                    # Follow the line to see if it intersects anything
+                    intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
+
+                    # 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 = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,numberOfPoints,5)
+                        if(tmpdistance < rmap(Q,pointNumber.mat[3],5)) {
+                            wmat(Q,pointNumber.mat[3],3, numberOfPoints)
+                            wmat(Q,pointNumber.mat[3],5, rmap(Q,numberOfPoints,5)+math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y))
+                        }
+                    }
+                }
+
+                # Check to see if this new point is within the goal
+                if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
+                    goalReached = 1;
+            }
+        } else {
+            # Associate with the closest point
+            pointNum = findClosestPoint(x,y,arrayOfPoints);
+
+            # Follow the line to see if it intersects anything
+            intersects = doesItIntersect(x,y,math.vec2.new(rmat(arrayOfPoints,1,pointNum),rmat(arrayOfPoints,2,pointNum)),map);
+
+            # If there is no intersection we need to add to the tree
+            if(intersects != 1) {
+                arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
+                arrayOfPoints.mat[arrayOfPoints.nb_row]=x
+                arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
+                numberOfPoints = numberOfPoints + 1;
+
+                wmat(Q,numberOfPoints,0, x)
+                wmat(Q,numberOfPoints,1, y)
+                wmat(Q,numberOfPoints,2, pointNum);
+                wmat(Q,numberOfPoints,3, numberOfPoints)
+                wmat(Q,numberOfPoints,4, rmat(Q,pointNum,5)+math.vec2.length(rmat(Q,pointNum,1)-x,rmat(Q,pointNum,2)-y))
+
+                # Check to see if this new point is within the goal
+                if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
+                    goalReached = 1;
+            }
+        }
+        if(numberOfPoints % 100 == 0) {
+            log(numberOfPoints, " points processed. Still looking for goal.");
+        }
+    }
+    log("Goal found!");
+}
+
+function findClosestPoint(x,y,arrayOfPoints) {
+    # Go through each points and find the distances between them and the
+    # target point
+    distance = inf;
+    i=1
+    while(i<arrayOfPoints.nb_row) {
+        range = (x-rmat(arrayOfPoints,1,i))^2+(y-rmat(arrayOfPoints,2,i))^2;
+        
+        if(range < distance) {
+            distance = range;
+            pointNumber = i;
+        }
+        i = i + 1
+    }
+    return pointNumber
+}
+
+function findPointsInRadius(x,y,arrayOfPoints,RADIUS) {
+    counted = 0;
+    pointList = {.nb_col=arrayOfPoints.nb_col, .nb_row=counted, .mat={}}
+    i=1
+    while(i < arrayOfPoints.nb_row){
+        distance = math.vec2.length(rmat(arrayOfPoints,i,1)-x,rmat(arrayOfPoints,i,2)-y);
+        
+        if(distance < RADIUS){
+            counted = counted+1;
+            pointList.nb_row=counted
+            mat_copyrow(pointList,counted,arrayOfPoints,i)
+        }
+        i = i + 1
+    }
+    return pointList
+}
+
+function doesItIntersect(x,y,vector) {
+    x = -x;
+    vector.x = -vector.x;
+    distance = math.vec2.length(vector);
+    vec = math.vec2.scale(math.vec2.sub(math.vec2.new(x,y),vector),1/distance);
+    log("doesItIntersect: ",vec)
+    #range = linspace(0,distance,distance*100);
+    i = 1;
+    #plot(y,-x,'b*');
+    while(i<100) {
+        if(block == 0) {
+            range = distance/100.0*i
+            position = math.vec2.sub(math.vec2.new(x,y),math.vec2.scale(vec,range));
+            
+            # Find what block we're in right now
+            xi = math.floor(position.x)
+            yi = math.floor(position.y)
+            
+            if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
+                if(rmap(xi,yi) == 0)
+                    return 1;
+            } else
+                return 1;
+        }
+        i = i + 1
+    }
+    return 0
+}
+
+# Write to matrix
+function wmat(mat, row, col, val) {
+    var index = (row-1)*mat.nb_col + col
+    mat.mat[index] = val
+}
+
+# Read from matrix
+function rmat(mat, row, col) {
+    var index = (row-1)*mat.nb_col + col
+    if (mat.mat[index] == nil) {
+        return -1
+    } else {
+        return mat.mat[index]
+    }
+}
+
+# copy a full matrix row
+function mat_copyrow(out,ro,in,ri){
+    var indexI = (ri-1)*in.nb_col
+    var indexO = (ro-1)*out.nb_col
+    i=0
+    while(i<in.nb_col){
+        out.mat[indexO+i]=in.mat[indexI+i]
+        i = i + 1
+    }
+}
+
+function make_test_map(){
+    # creates a 5x5 map
+    map = {.nb_col=5, .nb_row=5, .mat={}}
+    index = 0
+    while(index<25){
+        map.mat[index]=0
+        index = index + 1
+    }
+    # puts an obstacle right in the middle
+    map.mat[5*2+3]=1
+}
\ No newline at end of file
diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz
index 7c987d6..215497e 100644
--- a/buzz_scripts/testalone.bzz
+++ b/buzz_scripts/testalone.bzz
@@ -3,6 +3,7 @@ include "update.bzz"
 include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
 include "vstigenv.bzz"
+include "rttstar.bzz"
 
 function action() {
   statef=action
@@ -15,6 +16,8 @@ function init() {
 	statef=turnedoff
 	UAVSTATE = "TURNEDOFF"
   	uav_initstig()
+	make_test_map()
+	RRTSTAR(math.vec2.new(0,0),math.vec2.new(5,5),math.vec2.new(1,1))
 }
 
 # Executed at each time step.
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 6140310..5083dd1 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -43,9 +43,9 @@ namespace buzzuav_closures{
 
 	int buzzros_print(buzzvm_t vm) {
 	int i;
-	char buffer [50] = "";
+	char buffer [100] = "";
         sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
-	   for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
+	   for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
 	      buzzvm_lload(vm, i);
 	      buzzobj_t o = buzzvm_stack_at(vm, 1);
 	      buzzvm_pop(vm);