aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 22:11:23 +0100
committerAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 22:11:23 +0100
commit499d8b00d8017a32ae31deafc88416c452b48c8c (patch)
tree236751c2324332d5695549efa36806ae8a2f1d23
parentd708798dbcccd0a7ffe5e9b4255bb458874077c4 (diff)
downloadRobotique-Projet-499d8b00d8017a32ae31deafc88416c452b48c8c.tar.gz
Robotique-Projet-499d8b00d8017a32ae31deafc88416c452b48c8c.zip
Add direct sol ; does not work.
-rw-r--r--Makefile4
-rw-r--r--problem.cpp89
2 files changed, 81 insertions, 12 deletions
diff --git a/Makefile b/Makefile
index b0c9fe9..d37b2bc 100644
--- a/Makefile
+++ b/Makefile
@@ -3,7 +3,7 @@ BIN=pathfind
OBJ=problem.o ui.o main.o
$(BIN): *.hpp $(OBJ)
- g++ -o $@ $(OBJ) -lm -lsfml-system -lsfml-window -lsfml-graphics -O2 -std=c++11
+ g++ -o $@ $(OBJ) -lm -lsfml-system -lsfml-window -lsfml-graphics -O2 -std=c++11 -g
%.o: %.cpp *.hpp
- g++ -c -o $@ $< -std=c++11 -O2 -Wall -Wextra
+ g++ -c -o $@ $< -std=c++11 -O2 -Wall -Wextra -g
diff --git a/problem.cpp b/problem.cpp
index dec9776..c733200 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -1,4 +1,5 @@
#include <deque>
+#include <iostream>
#include "problem.hpp"
@@ -80,7 +81,6 @@ bool hilare_a_mvt::intersects(const problem &p) const {
// ================================= //
solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
- vector<hilare_a_mvt> sol;
// première famille de mouvements :
// - trouver les quatre droites tangentes aux deux cercles canoniques
@@ -96,7 +96,6 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
vec ccb = pos_b.canon_curve_center();
double rcb = (ccb - pos_b.pos_trolley()).norm();
- vector<line> tgt_ls;
int eps[4][2] = { { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } };
double delta = cca.x * ccb.y - cca.y * ccb.x;
assert(delta != 0);
@@ -107,12 +106,74 @@ solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) {
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;
- tgt_ls.push_back(line(a, b, 1));
- }
- //TODO
+ line l(a, b, 1);
+
+ vec uv = vec(a, b).normalize();
+
+ 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
+ }
+
+ 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);
+ }
- return solution(sol);
+ 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;
+
+ 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.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 = pb.x; t.to.y = pb.y;
+ t.is_arc = false;
+ t.ds = (pb - pa).norm();
+ t.dtheta_before = t.from.phi;
+ sol.push_back(t);
+
+ hilare_a_mvt r2;
+ r2.from = t.to;
+ r2.to = pos_b;
+ r2.is_arc = true;
+ r2.dtheta_before = -pos_b.phi;
+ r2.center = ccb;
+ r2.domega = domega2;
+ sol.push_back(r2);
+
+ return solution(sol);
+ }
+ }
+
+ return solution(); // empty solution
}
bool solution::intersects(const problem &p) const {
@@ -164,7 +225,7 @@ void solver::run() {
break;
}
- if (!_please_stop) break;
+ if (_please_stop) break;
d.step(p);
@@ -196,6 +257,8 @@ solver_internal solver::peek_internal() {
}
void solver_internal::initialize(const problem &p) {
+ cout << "Initializing solver..." << endl;
+
paths.clear();
pts.clear();
@@ -203,12 +266,13 @@ void solver_internal::initialize(const problem &p) {
pts.push_back(p.end_pos);
solution ts = solution::direct_sol(p.begin_pos, p.end_pos);
- if (!ts.intersects(p)) {
+ if (ts.movement.size() > 0 && !ts.intersects(p)) {
paths[0][1] = ts;
}
}
solution solver_internal::try_find_solution() {
+ cout << "Looking for solution in current graph..." << endl;
// Simple graph search algorithm
vector<int> par(pts.size(), -1);
@@ -234,6 +298,8 @@ solution solver_internal::try_find_solution() {
}
if (par[1] != -1) {
+ cout << "...found!" << endl;
+
vector<hilare_a_mvt> sol;
int b = 1;
@@ -250,13 +316,16 @@ solution solver_internal::try_find_solution() {
return solution(sol);
}
+ cout << "...not found." << endl;
return solution(); // not found
}
void solver_internal::step(const problem &p) {
+ cout << "Solver step..." << endl;
+
// 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;
+ double min_x = -100, min_y = -100;
+ 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;
if (o.c.c.y < min_y) min_y = o.c.c.y;