Merge branch 'master' of https://github.com/Alexis211/Robotique

2 files changed, 30 insertions, 6 deletions

diff --git a/geom.hpp b/geom.hpp
index 40d6813..27b3de5 100644
--- a/geom.hpp
+++ b/geom.hpp
@@ -188,7 +188,7 @@ struct circarc {
 };
 
 struct angular_sector {
-	// attention, les circarc doivent avoir le même centre
+	// attention, les circarc doivent avoir le même centre et les mêmes angles
 	circarc inner, outer;
 
 	angular_sector(circarc i, circarc o) : inner(i), outer(o) {
@@ -198,14 +198,17 @@ struct angular_sector {
 		
 	}
     bool is_in_sector(vec p) const{
-        
+        if(inner.is_in_pie(p) && (p-inner.c.c).norm() <=outer.c.r && inner.c.r <=(p-inner.c.c).norm())return true;
 	return false;
     }
 
     double dist(vec p) const{
 	if (is_in_sector(p)) return 0;
-	//TODO
-	return 42 ;
+	return std::min(std::min(std::min(inner.dist(p),outer.dist(p)),
+				 segment(vec::from_polar(inner.c.r,inner.theta1)+inner.c.c,
+					 vec::from_polar(outer.c.r,inner.theta2)+inner.c.c).dist(p)),
+			segment(vec::from_polar(inner.c.r,inner.theta2)+inner.c.c,
+				vec::from_polar(outer.c.r,inner.theta2)+inner.c.c).dist(p)) ;
     }
 };
 /* vim: set ts=4 sw=4 tw=0 noet :*/
diff --git a/problem.cpp b/problem.cpp
index f9f839f..dec9776 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -35,8 +35,29 @@ bool hilare_a_mvt::intersects(const obstacle &o) const {
 	    max((pos_init - center).norm()+(p->r_c_car),
 		(pos_init_trolley - center).norm()+(p->r_c_trolley));
 	//TODO
-	 double theta1 = 0;
-	double theta2 = 0;
+	double theta1;
+	double theta2;
+	if (domega>=0) {
+	    if(from.phi > 0){
+		theta1 = (from.pos()-center).angle();
+		theta2 = (to.pos_trolley()-center).angle();
+	    }
+	    else {
+		theta1 = (from.pos_trolley()-center).angle();
+		theta2 = (to.pos()-center).angle();
+		}
+	}
+	else {
+	    if(from.phi > 0){ //TODO ??
+		theta2 = (from.pos()-center).angle();
+		theta1 = (to.pos_trolley()-center).angle();
+	    }
+	    else {
+		theta2 = (from.pos_trolley()-center).angle();
+		theta1 = (to.pos()-center).angle();
+		}
+	}
+	theta2 = canon_angle(theta1,theta2);
     angular_sector sector = angular_sector(circarc(circle(center,r_min), theta1, theta2), circarc(circle(center,r_max), theta1, theta2));
     if (sector.dist(o.c.c)<=o.c.r)return true;
     if (from.intersects(o)) return true;