diff options
| -rw-r--r-- | geom.hpp | 8 | ||||
| -rw-r--r-- | main.cpp | 2 | ||||
| -rw-r--r-- | problem.cpp | 165 | ||||
| -rw-r--r-- | problem.hpp | 18 | ||||
| -rw-r--r-- | ui.cpp | 11 | ||||
| -rw-r--r-- | ui.hpp | 1 |
6 files changed, 199 insertions, 6 deletions
@@ -1,5 +1,6 @@ #pragma once +#include <stdlib.h> #include <math.h> #include <algorithm> #include <assert.h> @@ -7,7 +8,12 @@ #define EPSILON 1e-6 #define abs(x) ((x)<0?-(x):(x)) -double canon_angle(double ref, double move_it){ +inline double frand(double a, double b) { + double r = ((double)rand()) / ((double)RAND_MAX); + return r * (b - a) + a; +} + +inline double canon_angle(double ref, double move_it){ while (ref>move_it) move_it += 2*M_PI; while (move_it >= ref + 2*M_PI) move_it -= 2*M_PI; return move_it ; @@ -3,6 +3,8 @@ #include "ui.hpp" int main() { + srand(time(0)); + hilare_a_param p; p.l = 50; p.r_c_car = 25; diff --git a/problem.cpp b/problem.cpp index 28ef8fa..67287fa 100644 --- a/problem.cpp +++ b/problem.cpp @@ -1,5 +1,8 @@ +#include <deque> + #include "problem.hpp" + using namespace std; // ===================================== // @@ -45,8 +48,8 @@ bool hilare_a_mvt::intersects(const obstacle &o) const { } bool hilare_a_mvt::intersects(const problem &p) const { - for (auto i = p.obstacles.begin(); i != p.obstacles.end(); i++) { - if (intersects(*i)) return true; + for (auto& i: p.obstacles) { + if (intersects(i)) return true; } return false; } @@ -69,16 +72,168 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { return solution(sol); } +bool solution::intersects(const problem &p) const { + for (auto& x: movement) { + if (x.intersects(p)) return true; + } + return false; +} + // =============================== // // IMPLEMENTATION FOR CLASS SOLVER // // =============================== // -solver::solver() { - // nothing ? +solver::solver() : _worker(&solver::run, this) { + _running = false; + _done = false; + _please_stop = false; +} + +void solver::start(const problem &p) { + _p = p; + + if (_running) { + _please_stop = true; + _worker.wait(); + } + + _please_stop = false; + _done = false; + _running = true; + _worker.launch(); +} + +void solver::run() { + problem p = _p; // copy problem + + solver_internal d; + d.initialize(p); + { + sf::Lock l(_d_lock); + _d = d; + } + + while (!_please_stop) { + solution s = d.try_find_solution(); + if (s.movement.size() > 0) { + _s = s; + _done = true; + break; + } + + if (!_please_stop) break; + + d.step(p); + + // Write local results to guys outside + { + sf::Lock l(_d_lock); + _d = d; + } + } + _running = false; +} + +bool solver::finished() { + return _done; +} + +solution solver::get_solution() { + if (_done) return _s; + return solution(); } solver_internal solver::peek_internal() { - return _d; + solver_internal x; + { + sf::Lock l(_d_lock); + x = _d; + } + return x; +} + +void solver_internal::initialize(const problem &p) { + paths.clear(); + pts.clear(); + + 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.intersects(p)) { + paths[0][1] = ts; + } +} + +solution solver_internal::try_find_solution() { + // Simple graph search algorithm + + vector<int> par(pts.size(), -1); + deque<int> q; + + par[0] = 0; + q.push_back(0); + while (!q.empty()) { + int x = q.front(); + q.pop_front(); + + if (paths.find(x) != paths.end()) { + auto pp = paths.find(x)->second; + + for (auto& kv: pp) { + int y = kv.first; + if (par[y] == -1) { + par[y] = x; + q.push_back(y); + } + } + } + } + + if (par[1] != -1) { + vector<hilare_a_mvt> sol; + + int b = 1; + while (b != 0) { + int a = par[b]; + + auto& x = paths[a][b]; + + sol.insert(sol.begin(), x.movement.begin(), x.movement.end()); + + b = a; + } + + return solution(sol); + } + + return solution(); // not found +} + +void solver_internal::step(const problem &p) { + // take new random point + double min_x = p.obstacles[0].c.c.x, min_y = p.obstacles[0].c.c.y; + double max_x = min_x, max_y = min_y; + for (auto& o: p.obstacles) { + if (o.c.c.x < min_x) min_x = o.c.c.x; + if (o.c.c.y < min_y) min_y = o.c.c.y; + if (o.c.c.x > max_x) max_x = o.c.c.x; + if (o.c.c.y > max_y) max_y = o.c.c.y; + } + hilare_a rp = p.begin_pos; + rp.x = frand(min_x, max_x); + rp.y = frand(min_y, max_y); + rp.theta = frand(-M_PI, M_PI); + rp.phi = frand(-M_PI, M_PI); + + // 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; + } + } + pts.push_back(rp); } /* vim: set ts=4 sw=4 tw=0 noet :*/ diff --git a/problem.hpp b/problem.hpp index f268d55..6eadd0a 100644 --- a/problem.hpp +++ b/problem.hpp @@ -2,6 +2,7 @@ #include <vector> #include <map> +#include <SFML/System.hpp> #include "geom.hpp" @@ -91,18 +92,35 @@ struct solver_internal { // represents a graph of randomly chosen positions and simple solutions between them std::vector<hilare_a> pts; std::map<int, std::map<int, solution> > paths; + + void initialize(const problem &p); + solution try_find_solution(); + void step(const problem &p); }; class solver { // mutex-protected asynchronous structure private: + + sf::Mutex _d_lock; solver_internal _d; + problem _p; + + bool _please_stop; + bool _running; + bool _done; + solution _s; + + sf::Thread _worker; public: solver(); void start(const problem &p); + + void run(); // worker thread + bool finished(); solution get_solution(); @@ -11,6 +11,7 @@ - e : select end pos - a : add obstacle - d : delete obstacle under mouse pointer + - g : start solving Select modes : no other keys */ @@ -25,6 +26,8 @@ UI::UI(hilare_a_param *p) : _sel_obs(vec(0,0), 0) { _view.zoom = 1; _mode = M_NORMAL; + + _got_sol = true; } void UI::run() { @@ -55,6 +58,9 @@ void UI::run() { _view.zoom *= 1.1; } else if (k == 'o') { _view.zoom /= 1.1; + } else if (k == 'g') { + _solver.start(_p); + _got_sol = false; } } @@ -63,6 +69,11 @@ void UI::run() { if (_mode == M_SEL_BEGIN || _mode == M_SEL_END) handle_sel_pos(ev); } + if (!_got_sol && _solver.finished()) { + _s = _solver.get_solution(); + _got_sol = true; + } + _win.clear(sf::Color::Black); render_internal(); @@ -28,6 +28,7 @@ class UI { problem _p; solution _s; solver _solver; + bool _got_sol; struct { double x0, y0, zoom; |