From 0ff287ed38956c0a15f2a8ab503cb1b8253f706d Mon Sep 17 00:00:00 2001 From: Alex Auvolat Date: Mon, 2 Feb 2015 00:16:37 +0100 Subject: Select best try --- problem.cpp | 69 ++++++++++++++++++++++++++++++++++++++++--------------------- problem.hpp | 7 ++++++- 2 files changed, 51 insertions(+), 25 deletions(-) diff --git a/problem.cpp b/problem.cpp index 5f774fd..31cebe7 100644 --- a/problem.cpp +++ b/problem.cpp @@ -80,7 +80,8 @@ bool hilare_a_mvt::intersects(const problem &p) const { // IMPLEMENTATION FOR CLASS SOLUTION // // ================================= // -solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { +vector solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { + vector ret; // première famille de mouvements : // - trouver les quatre droites tangentes aux deux cercles canoniques @@ -127,34 +128,43 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { vec w = l.proj(ccb); double domega1 = (v - cca).angle() - (pos_a.pos_trolley() - cca).angle(); + if (domega1 > M_PI) domega1 -= 2 * M_PI; + if (domega1 < -M_PI) domega1 += 2 * M_PI; double dtheta1 = pos_a.phi; double dtheta2 = -pos_b.phi; double domega2 = (pos_b.pos_trolley() - ccb).angle() - (w - ccb).angle(); + if (domega2 > M_PI) domega2 -= 2 * M_PI; + if (domega2 < -M_PI) domega2 += 2 * M_PI; + 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 sol; + vec p1 = cca + vec::from_polar((pos_a.pos() - cca).norm(), (pos_a.pos() - cca).angle() + domega1); + vec p2 = ccb + vec::from_polar((pos_b.pos() - ccb).norm(), (pos_b.pos() - ccb).angle() - domega2); + hilare_a_mvt r1; + r1.dtheta_before = 0; r1.is_arc = true; + r1.center = cca; + r1.domega = domega1; r1.from = pos_a; r1.to = pos_a; - r1.to.x = v.x; r1.to.y = v.y; + r1.to.x = p1.x; r1.to.y = p1.y; r1.to.theta = r1.from.theta + domega1; - r1.center = cca; - r1.domega = domega1; - r1.dtheta_before = 0; sol.push_back(r1); hilare_a_mvt t; - t.from = r1.to; - t.to = t.from; - t.to.x = w.x; t.to.y = w.y; t.to.phi = 0; t.is_arc = false; t.ds = (w - v).norm(); - t.dtheta_before = t.from.phi; + t.dtheta_before = r1.to.phi; + t.from = r1.to; + t.to = t.from; t.to.theta = t.from.theta + t.dtheta_before; + t.to.x = p2.x; t.to.y = p2.y; t.to.phi = 0; sol.push_back(t); hilare_a_mvt r2; @@ -166,11 +176,11 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { r2.domega = domega2; sol.push_back(r2); - return solution(sol); + ret.push_back(sol); } } - return solution(); // empty solution + return ret; } bool solution::intersects(const problem &p) const { @@ -180,6 +190,14 @@ bool solution::intersects(const problem &p) const { return false; } +double solution::length() { + double x; + for (auto& m: movement) { + x += m.length(); + } + return x; +} + // =============================== // // IMPLEMENTATION FOR CLASS SOLVER // // =============================== // @@ -265,10 +283,7 @@ void solver_internal::initialize(const problem &p) { pts.push_back(p.begin_pos); pts.push_back(p.end_pos); - solution ts = solution::direct_sol(p.begin_pos, p.end_pos); - if (ts.movement.size() > 0 && !ts.intersects(p)) { - paths[0][1] = ts; - } + find_direct_path(0, 1, p); } solution solver_internal::try_find_solution() { @@ -338,18 +353,24 @@ void solver_internal::step(const problem &p) { rp.theta = frand(-M_PI, M_PI); rp.phi = frand(-M_PI, M_PI); + pts.push_back(rp); + // try to connect to all existing points - for (unsigned i = 0; i < pts.size(); i++) { - solution s = solution::direct_sol(pts[i], rp); - 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; + for (unsigned i = 0; i < pts.size() - 1; i++) { + find_direct_path(i, pts.size() - 1, p); + find_direct_path(pts.size() - 1, i, p); + } +} + +void solver_internal::find_direct_path(int a, int b, const problem &p) { + vector s = solution::direct_sol(pts[a], pts[b]); + int best = -1; + for (int k = 0; k < s.size(); k++) { + if (s[k].movement.size() > 0 && !s[k].intersects(p)) { + if (best == -1 || s[k].length() < s[best].length()) best = k; } } - pts.push_back(rp); + if (best != -1) paths[a][b] = s[best]; } /* vim: set ts=4 sw=4 tw=0 noet :*/ diff --git a/problem.hpp b/problem.hpp index 14ba475..4933ed6 100644 --- a/problem.hpp +++ b/problem.hpp @@ -91,10 +91,12 @@ struct solution { solution(const std::vector &m) : movement(m) {} // simple direct solution from A to B, takes into account no obstacles - static solution direct_sol(const hilare_a &pos_a, const hilare_a &pos_b); + static std::vector direct_sol(const hilare_a &pos_a, const hilare_a &pos_b); // check if a solution intersects an obstacle of problem bool intersects(const problem &p) const; + + double length(); }; struct solver_internal { @@ -106,6 +108,9 @@ struct solver_internal { void initialize(const problem &p); solution try_find_solution(); void step(const problem &p); + + // internal + void find_direct_path(int a, int b, const problem &p); }; class solver { -- cgit v1.2.3