diff options
author | Alex Auvolat <alex.auvolat@ens.fr> | 2015-02-01 18:07:27 +0100 |
---|---|---|
committer | Alex Auvolat <alex.auvolat@ens.fr> | 2015-02-01 18:07:27 +0100 |
commit | cacec4dbb8b5cc017b9c6d9975f8f792db13cc7a (patch) | |
tree | 00a3a31b475c678dbafca8b73f0ac26f14d15bfb /problem.cpp | |
parent | 29158da41e7943ea8019efdbff70c994fb1c73e9 (diff) | |
parent | 93cd8ba7c9ea2ee22bfcb828cd5190c4662a05b0 (diff) | |
download | Robotique-Projet-cacec4dbb8b5cc017b9c6d9975f8f792db13cc7a.tar.gz Robotique-Projet-cacec4dbb8b5cc017b9c6d9975f8f792db13cc7a.zip |
Merge branch 'master' of https://github.com/Alexis211/Robotique
Diffstat (limited to 'problem.cpp')
-rw-r--r-- | problem.cpp | 33 |
1 files changed, 30 insertions, 3 deletions
diff --git a/problem.cpp b/problem.cpp index 3169751..11f4d8e 100644 --- a/problem.cpp +++ b/problem.cpp @@ -11,12 +11,39 @@ using namespace std; double hilare_a_mvt::length() { // returns length traveled by the car - // TODO : two cases - return domega * (center - from.pos()).norm(); + if (is_arc) return domega * (center - from.pos()).norm(); + return ds ; +} + +bool hilare_a::intersects(const obstacle &o) const { + + if((pos()-o.c.c).norm() < o.c.r + param->r_c_car)return true ; + if((pos_trolley()-o.c.c).norm() < o.c.r + param->r_c_trolley)return true ; + if(segment(pos(),pos_trolley()).dist(o.c.c) < o.c.r)return true ; + return false ; } bool hilare_a_mvt::intersects(const obstacle &o) const { - // TODO + hilare_a_param *p = from.param; + vec pos_init = from.pos(); + vec pos_init_trolley = from.pos_trolley(); + if(is_arc){ + double r_min = + min((pos_init - center).norm()-(p->r_c_car), + (pos_init_trolley - center).norm()-(p->r_c_trolley)); + double r_max = + 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; + 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; + if (to.intersects(o)) return true; + return false; + + } return false; } |