aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 20:08:57 +0100
committerAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 20:08:57 +0100
commitd708798dbcccd0a7ffe5e9b4255bb458874077c4 (patch)
tree169c0ed325c2bc41f8d581b32d7da49bfa54038c
parent1931b7e0e2389a9d04b07e36676f9060ebf4b9c3 (diff)
parent57a3b0a8ca868e428d00e9549d15f73872e4f832 (diff)
downloadRobotique-Projet-d708798dbcccd0a7ffe5e9b4255bb458874077c4.tar.gz
Robotique-Projet-d708798dbcccd0a7ffe5e9b4255bb458874077c4.zip
Merge branch 'master' of https://github.com/Alexis211/Robotique
-rw-r--r--geom.hpp11
-rw-r--r--problem.cpp25
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;