aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 23:44:41 +0100
committerAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 23:44:41 +0100
commit97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3 (patch)
treef77879e5d72574f88057f1b6b22ec0a2bca4e21b
parent499d8b00d8017a32ae31deafc88416c452b48c8c (diff)
downloadRobotique-Projet-97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3.tar.gz
Robotique-Projet-97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3.zip
Some of it works...
-rw-r--r--problem.cpp64
-rw-r--r--problem.hpp25
-rw-r--r--ui.cpp20
3 files changed, 59 insertions, 50 deletions
diff --git a/problem.cpp b/problem.cpp
index c733200..ec5387d 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -92,6 +92,7 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
// calcul des centres des courbes canoniques
vec cca = pos_a.canon_curve_center();
+
double rca = (cca - pos_a.pos_trolley()).norm();
vec ccb = pos_b.canon_curve_center();
double rcb = (ccb - pos_b.pos_trolley()).norm();
@@ -104,47 +105,43 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
int ea = eps[i_eps][0];
int eb = eps[i_eps][1];
- double a = ((ea * rca - 1) * ccb.y - cca.y * (eb * rcb - 1)) / delta;
- double b = (cca.x * (eb * rcb - 1) - ccb.x * (ea * rca - 1)) / delta;
+ double xc = cca.x, yc = cca.y, xcp = ccb.x, ycp = ccb.y;
- line l(a, b, 1);
+ double a0 = (ea * rca - eb * rcb) / (xc - xcp);
+ double b0 = 0;
+ double c0 = (ea * rca - xc * a0);
- vec uv = vec(a, b).normalize();
+ double delta = xc * ycp - xcp * yc;
+ double a = (yc - ycp) / delta;
+ double b = (xcp - xc) / delta;
+ double c = 1;
- vec pa(0,0);
- if (l.on_line(cca + rca * uv)) {
- pa = cca + rca * uv;
- } else if (l.on_line(cca - rca * uv)) {
- pa = cca - rca * uv;
- } else {
- assert(false); // calculs de merde
- }
+ double di = a * a0 * a * a0 - (a0 * a0 - 1) * (a * a + b * b);
+ if (di < 0) continue;
- vec pb(0,0);
- if (l.on_line(ccb + rcb * uv)) {
- pb = ccb + rcb * uv;
- } else if (l.on_line(ccb - rcb * uv)) {
- pb = ccb - rcb * uv;
- } else {
- assert(false);
- }
+ double lambda = (-a * a0 + sqrt(di)) / (a * a + b * b);
+
+ line l(a0 + lambda * a, b0 + lambda * b, c0 + lambda * c);
- double domega1 = (pa - cca).angle() - (pos_a.pos_trolley() - cca).angle();
- double domega2 = (pos_b.pos_trolley() - ccb).angle() - (pb - ccb).angle();
- double xx = pos_a.theta + domega1 + domega2 - pos_b.theta;
+ vec v = l.proj(cca);
+ vec w = l.proj(ccb);
+ double domega1 = (v - cca).angle() - (pos_a.pos_trolley() - cca).angle();
+ double dtheta1 = pos_a.phi;
+ double dtheta2 = -pos_b.phi;
+ double domega2 = (pos_b.pos_trolley() - ccb).angle() - (w - ccb).angle();
+ double xx = pos_a.theta + domega1 + dtheta1 + dtheta2 + domega2 - pos_b.theta;
cout << "domega1: " << domega1
<< ", domega2: " << domega2
<< ", xx:" << xx << endl;
-
if (fabs(xx) < 0.01 || fabs(xx - 2*M_PI) < 0.01 && fabs(xx + 2*M_PI) < 0.01) {
vector<hilare_a_mvt> sol;
-
+
hilare_a_mvt r1;
r1.is_arc = true;
r1.from = pos_a;
r1.to = pos_a;
- r1.to.x = pa.x; r1.to.y = pa.y;
+ r1.to.x = v.x; r1.to.y = v.y;
r1.to.theta = r1.from.theta + domega1;
r1.center = cca;
r1.domega = domega1;
@@ -154,9 +151,9 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
hilare_a_mvt t;
t.from = r1.to;
t.to = t.from;
- t.to.x = pb.x; t.to.y = pb.y;
+ t.to.x = w.x; t.to.y = w.y; t.to.phi = 0;
t.is_arc = false;
- t.ds = (pb - pa).norm();
+ t.ds = (w - v).norm();
t.dtheta_before = t.from.phi;
sol.push_back(t);
@@ -217,7 +214,10 @@ void solver::run() {
_d = d;
}
- while (!_please_stop) {
+ int i = 0;
+ while (!_please_stop && (i++) < 300) {
+ sf::sleep(sf::milliseconds(100));
+
solution s = d.try_find_solution();
if (s.movement.size() > 0) {
_s = s;
@@ -324,7 +324,7 @@ void solver_internal::step(const problem &p) {
cout << "Solver step..." << endl;
// take new random point
- double min_x = -100, min_y = -100;
+ double min_x = -800, min_y = -800;
double max_x = 800, max_y = 800;
for (auto& o: p.obstacles) {
if (o.c.c.x < min_x) min_x = o.c.c.x;
@@ -344,6 +344,10 @@ void solver_internal::step(const problem &p) {
if (s.movement.size() > 0 && !s.intersects(p)) {
paths[i][pts.size()] = s;
}
+ solution ss = solution::direct_sol(rp, pts[i]);
+ if (ss.movement.size() > 0 && !ss.intersects(p)) {
+ paths[pts.size()][i] = ss;
+ }
}
pts.push_back(rp);
}
diff --git a/problem.hpp b/problem.hpp
index 5d9269a..2fe70b1 100644
--- a/problem.hpp
+++ b/problem.hpp
@@ -26,28 +26,25 @@ struct hilare_a { // System A
vec pos() const { return vec(x, y); }
vec dir() const { return vec::from_polar(1, theta); }
- vec dir_trolley() const {
- return vec::from_polar(1, theta + phi + M_PI);
- }
vec pos_trolley() const {
- return pos() + param->l * dir_trolley();
+ return pos() - vec::from_polar(param->l, theta + phi);
}
vec canon_curve_center() const {
- double delta = cos(theta) * sin(theta + phi) - sin(theta) * cos(theta + phi);
- assert(delta != 0);
-
vec a = pos();
vec b = pos_trolley();
- vec eth = vec::from_polar(1, theta);
- vec ethph = vec::from_polar(1, theta + phi);
- double xc = (vec::dot(a, eth) * sin(theta + phi) - vec::dot(b, ethph) * sin(theta)) / delta;
- double yc = (cos(theta) * vec::dot(b, ethph) - cos(theta + phi) * vec::dot(a, eth));
- return vec(xc, yc);
- }
- bool intersects(const obstacle &o) const; // intersects an obstacle ?
+ double u = b.x - a.x;
+ double v = b.y - a.y;
+
+ double dd = sin(theta) * (a.x - b.x) + cos(theta) * (b.y - a.y);
+ assert(dd != 0);
+ double lambda = (u * u + v * v) / dd;
+ return a + lambda * vec(- sin(theta), cos(theta));
+ }
+
+ bool intersects(const obstacle &o) const; // intersects an obstacle ?
};
struct problem {
diff --git a/ui.cpp b/ui.cpp
index a2ec620..1dbc586 100644
--- a/ui.cpp
+++ b/ui.cpp
@@ -213,6 +213,12 @@ void UI::render_pos(const hilare_a &pos, sf::Color c) {
_win.draw(l);
render_circle(circle(pos.pos_trolley(), pos.param->r_c_trolley), c, sf::Color::Transparent, 2);
+
+ if (fabs(pos.phi) > 0.01) {
+ vec cc = pos.canon_curve_center();
+ //render_circle(circle(cc, (pos.pos() - cc).norm()), c, sf::Color::Transparent, 2);
+ //render_circle(circle(cc, (pos.pos_trolley() - cc).norm()), c, sf::Color::Transparent, 2);
+ }
}
void UI::render_obstacle(const obstacle &o) {
@@ -221,25 +227,27 @@ void UI::render_obstacle(const obstacle &o) {
void UI::render_mvt(const hilare_a_mvt &m, sf::Color c) {
if (m.is_arc) {
- sf::Vertex l[20];
+ const int nd = 42;
+
+ sf::Vertex l[nd];
l[0] = sf::Vertex(to_view(m.from.pos()), c);
double th = (m.from.pos() - m.center).angle();
double r = (m.from.pos() - m.center).norm();
- for (int i = 1; i < 19; i++) {
- l[i] = sf::Vertex(to_view(m.center + vec::from_polar(r, th + m.domega * i / 20)), c);
+ for (int i = 1; i < nd - 1; i++) {
+ l[i] = sf::Vertex(to_view(m.center + vec::from_polar(r, th + m.domega * i / nd)), c);
}
- l[19] = sf::Vertex(to_view(m.to.pos()), c);
+ l[nd - 1] = sf::Vertex(to_view(m.to.pos()), c);
- _win.draw(l, 20, sf::Lines);
+ _win.draw(l, nd, sf::LinesStrip);
} else {
sf::Vertex l[] = {
sf::Vertex(to_view(m.from.pos()), c),
sf::Vertex(to_view(m.to.pos()), c),
};
- _win.draw(l, 2, sf::Lines);
+ _win.draw(l, 2, sf::LinesStrip);
}
}