diff options
| -rw-r--r-- | problem.cpp | 64 | ||||
| -rw-r--r-- | problem.hpp | 25 | ||||
| -rw-r--r-- | ui.cpp | 20 |
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 { @@ -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); } } |