Some of it works...

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);
 	}
 }