diff options
| -rw-r--r-- | geom.hpp | 11 | ||||
| -rw-r--r-- | problem.cpp | 25 |
2 files changed, 30 insertions, 6 deletions
@@ -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 67287fa..ac4f608 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; |