Optimisations ; follow mode

4 files changed, 104 insertions, 19 deletions

diff --git a/problem.cpp b/problem.cpp
index e51aa95..c919710 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -24,6 +24,13 @@ bool hilare_a::intersects(const obstacle &o) const {
 	return false ;
 }
 
+bool hilare_a::intersects(const problem &o) const {
+	for (auto& a: o.obstacles) {
+		if (intersects(a)) return true;
+	}
+	return false ;
+}
+
 bool hilare_a_mvt::intersects(const obstacle &o) const {
 	hilare_a_param *p = from.param;
 	vec pos_init = from.pos();
@@ -140,9 +147,6 @@ vector<solution> solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos
 		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<hilare_a_mvt> sol;
@@ -287,8 +291,6 @@ void solver::run() {
 
 	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;
@@ -392,19 +394,22 @@ void solver_internal::step(const problem &p) {
 	cout << "Solver step..." << endl;
 
 	// take new random point
-	double min_x = -200, min_y = -200;
-	double max_x = 200, max_y = 200;
-	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);
+
+	do {
+		double min_x = -200, min_y = -200;
+		double max_x = 200, max_y = 200;
+		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;
+		}
+		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);
+	} while (rp.intersects(p));
 
 	pts.push_back(rp);
 
diff --git a/problem.hpp b/problem.hpp
index e51b722..56ddb34 100644
--- a/problem.hpp
+++ b/problem.hpp
@@ -17,6 +17,8 @@ struct hilare_a_param {
 	double r_c_car, r_c_trolley;
 };
 
+struct problem;
+
 struct hilare_a {	// System A
 	hilare_a_param *param;
 
@@ -45,6 +47,7 @@ struct hilare_a {	// System A
 	}
 
 	bool intersects(const obstacle &o) const;	// intersects an obstacle ?
+	bool intersects(const problem &o) const;	// intersects an obstacle ?
 };
 
 struct problem {
diff --git a/ui.cpp b/ui.cpp
index 1dbc586..f745cb0 100644
--- a/ui.cpp
+++ b/ui.cpp
@@ -12,6 +12,7 @@
 		- a : add obstacle
 		- d : delete obstacle under mouse pointer
 		- g : start solving
+		- f : follow trajectory mode
 	Select modes : no other keys
 */
 
@@ -28,14 +29,15 @@ UI::UI(hilare_a_param *p) : _sel_obs(vec(0,0), 0) {
 	_mode = M_NORMAL;
 
 	_got_sol = true;
+	_follow = false;
 }
 
 void UI::run() {
 	_settings.antialiasingLevel = 8;
 
-	_win.create(sf::VideoMode(800, 600), "PR", sf::Style::Default, _settings);
+	_win.create(sf::VideoMode(1000, 700), "PR", sf::Style::Default, _settings);
 	_win.setVerticalSyncEnabled(true);
-	_win.setFramerateLimit(30);
+	_win.setFramerateLimit(60);
 
 	while (_win.isOpen()) {
 		sf::Event ev;
@@ -61,6 +63,10 @@ void UI::run() {
 				} else if (k == 'g') {
 					_solver.start(_p);
 					_got_sol = false;
+				} else if (k == 'f') {
+					_follow = !_follow;
+					_follow_s = 0;
+					_follow_d = 0;
 				}
 			}
 
@@ -74,6 +80,8 @@ void UI::run() {
 			_got_sol = true;
 		}
 
+		if (_follow) do_follow();
+
 		_win.clear(sf::Color::Black);
 
 		render_internal();
@@ -85,6 +93,8 @@ void UI::run() {
 		if (_mode == M_SEL_BEGIN || _mode == M_SEL_END)
 			render_pos(_sel_pos, sf::Color::White);
 
+		if (_follow) render_pos(_follow_pos, sf::Color::Cyan);
+
 		_win.display();
 	}
 }
@@ -288,6 +298,65 @@ void UI::render_internal() {
 	}
 }
 
+void UI::do_follow() {
+	if (_s.movement.size() == 0) return;
+
+	hilare_a_mvt &m = _s.movement[_follow_s];
+
+	int k = m.length() * _view.zoom / 2;
+	const int divide = (k >= 40 ? k : 40);
+
+	int s = _follow_d;
+
+	if (m.dtheta_before != 0) {
+		_follow_pos = m.from;
+		if (s < divide / 2) {
+			_follow_pos.theta += m.dtheta_before * s / (divide / 2);
+			_follow_pos.phi -= m.dtheta_before * s / (divide / 2);
+		} else {
+			s -= divide / 2;
+			s *= 2;
+
+			_follow_pos.theta += m.dtheta_before;
+			_follow_pos.phi -= m.dtheta_before;
+
+			if (m.is_arc) {
+				_follow_pos.theta += m.domega * s / divide;
+
+				vec p = m.center + vec::from_polar((m.from.pos() - m.center).norm(), (m.from.pos() - m.center).angle() + m.domega * s / divide);
+				_follow_pos.x = p.x;
+				_follow_pos.y = p.y;
+			} else {
+				_follow_pos.x = (s * m.to.x + (divide - s) * m.from.x) / divide;
+				_follow_pos.y = (s * m.to.y + (divide - s) * m.from.y) / divide;
+			}
+		}
+	} else {
+		_follow_pos = m.from;
+
+		if (m.is_arc) {
+			_follow_pos.theta += m.domega * s / divide;
+
+			vec p = m.center + vec::from_polar((m.from.pos() - m.center).norm(), (m.from.pos() - m.center).angle() + m.domega * s / divide);
+			_follow_pos.x = p.x;
+			_follow_pos.y = p.y;
+		} else {
+			_follow_pos.x = (s * m.to.x + (divide - s) * m.from.x) / divide;
+			_follow_pos.y = (s * m.to.y + (divide - s) * m.from.y) / divide;
+		}
+	}
+
+	_view.x0 = _follow_pos.x;
+	_view.y0 = _follow_pos.y;
+
+	_follow_d++;
+	if (_follow_d == divide) {
+		_follow_s++;
+		_follow_d = 0;
+		if (_follow_s == _s.movement.size()) _follow = false;
+	}
+}
+
 sf::Vector2f UI::to_view(const vec &p) {
 	sf::Vector2u s = _win.getSize();
 	return sf::Vector2f((p.x - _view.x0) * _view.zoom + (s.x/2), (p.y - _view.y0) * _view.zoom + (s.y/2));
diff --git a/ui.hpp b/ui.hpp
index 07383fd..de612b2 100644
--- a/ui.hpp
+++ b/ui.hpp
@@ -37,6 +37,12 @@ class UI {
 	sf::ContextSettings _settings;
 	sf::RenderWindow _win;
 
+	// solution follow mode
+	bool _follow;
+	int _follow_s;
+	int _follow_d;
+	hilare_a _follow_pos;
+
 	// interaction mode
 	int _mode, _sel_step;
 	hilare_a _sel_pos;
@@ -60,6 +66,8 @@ class UI {
 	void render_solution();
 	void render_internal();
 
+	void do_follow();
+
 	sf::Vector2f to_view(const vec &p);
 	vec from_view(const sf::Vector2f &p);
 	vec from_view(const sf::Vector2i &p);