finished path-planning-mov

Author: ClementRolinat

Diff for: collision-detect-fix

@@ -11,7 +11,7 @@ LIBS	 = $(shell pkg-config --libs  ibex)
 ifeq ($(DEBUG), yes)
 CXXFLAGS := $(CXXFLAGS) -O0 -g -pg -Wall
 else
-CXXFLAGS := $(CXXFLAGS) 
+CXXFLAGS := $(CXXFLAGS)
 endif
 
 collision-detect-fix: collision-detect-fix.o vibes.o tools.o node.o

Diff for: collision-detect-fix.o

@@ -5,10 +5,20 @@
 using namespace std;
 using namespace ibex;
 
+IntervalVector findClosest(vector<IntervalVector> listBoxes, IntervalVector boatSpeed){
+    double dist = 1000000000;
+    IntervalVector outputBox(2); 
+    for ( int i = 0; i < listBoxes.size(); i++){
+        if (distance(boatSpeed, listBoxes[i]) < dist){
+            dist = distance(boatSpeed, listBoxes[i]);
+            outputBox = listBoxes[i];
+        }
+    }
+    return outputBox;
+}
 
-
-void pavingMov(IntervalVector speed, vector<SepInter*> listSep){
-    if (width(speed) < 0.2){
+void pavingMov(IntervalVector speed, vector<SepInter*> listSep, vector<IntervalVector>& listBoxes){
+    if (speed.max_diam() < 0.2){
         return;
     }
     IntervalVector XinEnd(2);
@@ -19,11 +29,11 @@ void pavingMov(IntervalVector speed, vector<SepInter*> listSep){
     vector<IntervalVector> listXin;
 
 
-    vibes::drawBoxes({{speed[0].lb(), speed[0].ub(), speed[1].lb(), speed[1].ub()}}, "[cyan]");
+    //vibes::drawBoxes({{speed[0].lb(), speed[0].ub(), speed[1].lb(), speed[1].ub()}}, "[cyan]");
+    //sleep(1);
 
     for (int i = 0; i < listSep.size(); i++){
         listSep[i]->SepInter::separate(Xin, Xout);
-        //cout << "test2" << endl;
         listXout.push_back(Xout);
         Xin = speed;
         Xout = speed;
@@ -35,8 +45,23 @@ void pavingMov(IntervalVector speed, vector<SepInter*> listSep){
         XoutEnd = XoutEnd | listXout[i];
     }
 
+    IntervalVector newBox(2);
+
+    IntervalVector* ListComplementary;
+
+    int size = XoutEnd.complementary(ListComplementary);
+
+    for ( int i = 0; i < size; i++){
+        newBox = ListComplementary[i]&speed;
+        if (!newBox.is_flat()){
+            listBoxes.push_back(newBox);
+            vibes::drawBoxes({{newBox[0].lb(), newBox[0].ub(), newBox[1].lb(), newBox[1].ub()}}, "[cyan]");
+        }
+    }
 
+    
     vibes::drawBoxes({{XoutEnd[0].lb(), XoutEnd[0].ub(), XoutEnd[1].lb(), XoutEnd[1].ub()}}, "[red]");
+    //sleep(1);
 
     Xin = XoutEnd;
     Xout = XoutEnd;
@@ -55,9 +80,10 @@ void pavingMov(IntervalVector speed, vector<SepInter*> listSep){
     }
 
     vibes::drawBoxes({{XinEnd[0].lb(), XinEnd[0].ub(), XinEnd[1].lb(), XinEnd[1].ub()}}, "[yellow]");
+    //sleep(1);
 
-    pavingMov(left(XinEnd), listSep);
-    pavingMov(right(XinEnd), listSep);
+    pavingMov(left(XinEnd), listSep, listBoxes);
+    pavingMov(right(XinEnd), listSep, listBoxes);
 }
 
 
@@ -68,6 +94,9 @@ int main(int argc, char** argv) {
     double pos[2][2] ={{-1,1},{-1,1}};
     IntervalVector boatInitPos(2, pos);
 
+    double _boatSpeed[2][2] = {{4,4.5}, {2,2.5}};
+    IntervalVector boatSpeed(2,_boatSpeed);
+
 
     vector<IntervalVector> obstacles;
     double pos1[4][2] = {{2, 2.4}, {30, 35}, {-10, -8}, {2.9, 3.5}}; // {speed, posInitx, posInity, heading}
@@ -93,10 +122,7 @@ int main(int argc, char** argv) {
     SepFwdBwd* pSep1;
     SepFwdBwd* pSep2;
     SepFwdBwd* pSep3;
-    SepInter* pSep;
-
-
-    
+    SepInter* pSep;    
 
     for ( int i = 0; i < obstacles.size(); i++){
         pf1 = new Function(vx, vy, (vx - obstacles[i][0]*cos(obstacles[i][3]))*T  +boatInitPos[0] - obstacles[i][1]);
@@ -115,10 +141,17 @@ int main(int argc, char** argv) {
     double _speed[2][2] = {{-10, 10}, {-10, 10}};
     IntervalVector speed(2, _speed);
 
+    vector<IntervalVector> listBoxes;
+
+    pavingMov(speed, listSep, listBoxes);
+
+    delete pf1, pf2, pf3, pSep1, pSep2, pSep3, pSep, listSep;
 
+    vibes::drawBoxes({{boatSpeed[0].lb(), boatSpeed[0].ub(), boatSpeed[1].lb(), boatSpeed[1].ub()}}, "[red]");
 
-    pavingMov(speed, listSep);
+    IntervalVector newSpeed = findClosest(listBoxes, boatSpeed);
 
+    vibes::drawBoxes({{newSpeed[0].lb(), newSpeed[0].ub(), newSpeed[1].lb(), newSpeed[1].ub()}}, "[green]");
 
     return 0;