aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorJean Fabre-Monplaisir <jean-isaac-fm@live.fr>2015-02-01 21:08:03 +0100
committerJean Fabre-Monplaisir <jean-isaac-fm@live.fr>2015-02-01 21:08:03 +0100
commit57a3b0a8ca868e428d00e9549d15f73872e4f832 (patch)
tree56e0ee53ff77f0d5cc63fae8ff5945d860d1b62f
parent2ef7173d248f994e58d4fb2d54cf6563a9041632 (diff)
downloadRobotique-Projet-57a3b0a8ca868e428d00e9549d15f73872e4f832.tar.gz
Robotique-Projet-57a3b0a8ca868e428d00e9549d15f73872e4f832.zip
add collision detection (maybe finished)
-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 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;