diff options
author | Alex Auvolat <alex.auvolat@ens.fr> | 2015-02-01 20:08:57 +0100 |
---|---|---|
committer | Alex Auvolat <alex.auvolat@ens.fr> | 2015-02-01 20:08:57 +0100 |
commit | d708798dbcccd0a7ffe5e9b4255bb458874077c4 (patch) | |
tree | 169c0ed325c2bc41f8d581b32d7da49bfa54038c /problem.cpp | |
parent | 1931b7e0e2389a9d04b07e36676f9060ebf4b9c3 (diff) | |
parent | 57a3b0a8ca868e428d00e9549d15f73872e4f832 (diff) | |
download | Robotique-Projet-d708798dbcccd0a7ffe5e9b4255bb458874077c4.tar.gz Robotique-Projet-d708798dbcccd0a7ffe5e9b4255bb458874077c4.zip |
Merge branch 'master' of https://github.com/Alexis211/Robotique
Diffstat (limited to 'problem.cpp')
-rw-r--r-- | problem.cpp | 25 |
1 files changed, 23 insertions, 2 deletions
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; |