aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorJean Fabre-Monplaisir <jean-isaac-fm@live.fr>2015-02-02 01:42:52 +0100
committerJean Fabre-Monplaisir <jean-isaac-fm@live.fr>2015-02-02 01:42:52 +0100
commit582e13dcafe9a33c8d04b469bb7efafc0f6b7687 (patch)
tree0675bcce6af032fa3befd6429c2178adb9052a56
parente3420a0d3db79f0e0216c4fca2fc91078ddb4543 (diff)
parent5a91fdc1a3d4b140d75bd96a0bdf308de6789478 (diff)
downloadRobotique-Projet-582e13dcafe9a33c8d04b469bb7efafc0f6b7687.tar.gz
Robotique-Projet-582e13dcafe9a33c8d04b469bb7efafc0f6b7687.zip
Merge branch 'master' of github.com:Alexis211/Robotique
-rw-r--r--problem.cpp56
-rw-r--r--problem.hpp2
2 files changed, 55 insertions, 3 deletions
diff --git a/problem.cpp b/problem.cpp
index a6d48c5..a4c6aca 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -185,6 +185,56 @@ vector<solution> solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos
return ret;
}
+std::vector<solution> solution::direct_sol_r(const hilare_a &pos_a, const hilare_a &pos_b) {
+ std::vector<solution> ret = direect_sol(pos_a, pos_b);
+
+ const int nnn = 8;
+ const double xa[nnn] = { -1, -0.8, -0.6, -0.4, 0.4, 0.6, 0.8, 1 };
+
+ for (int aaa = 0; aaa < nnn; aaa++) {
+ double dtha = xa[aaa];
+
+ for (int bbb = 0; bbb < nnn; bbb++) {
+ double dthb = xa[bbb];
+
+ hilare_a pos_a_2 = pos_a;
+ pos_a_2.theta += dtha;
+ pos_a_2.phi -= dtha;
+
+ hilare_a pos_b_2 = pos_b;
+ pos_b_2.theta -= dthb;
+ pos_b_2.phi += dthb;
+
+ vector<solution> ss = direct_sol(pos_a_2, pos_b_2);
+ for (auto& s: ss) {
+ vector<hilare_a_mvt> mvt;
+
+ hilare_a_mvt rb;
+ rb.from = pos_a;
+ rb.to = pos_a_2;
+ rb.dtheta_before = dtha;
+ rb.is_arc = false;
+ rb.ds = 0;
+ mvt.push_back(rb);
+
+ mvt.insert(mvt.end(), s.movement.begin(), s.movement.end());
+
+ hilare_a_mvt ra;
+ ra.from = pos_b_2;
+ ra.to = pos_b;
+ ra.dtheta_before = dthb;
+ ra.is_arc = false;
+ ra.ds = 0;
+ mvt.push_back(ra);
+
+ ret.push_back(solution(mvt));
+ }
+ }
+ }
+
+ return ret;
+}
+
bool solution::intersects(const problem &p) const {
for (auto& x: movement) {
if (x.intersects(p)) return true;
@@ -341,8 +391,8 @@ void solver_internal::step(const problem &p) {
cout << "Solver step..." << endl;
// take new random point
- double min_x = -800, min_y = -800;
- double max_x = 800, max_y = 800;
+ 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;
@@ -365,7 +415,7 @@ void solver_internal::step(const problem &p) {
}
void solver_internal::find_direct_path(int a, int b, const problem &p) {
- vector<solution> s = solution::direct_sol(pts[a], pts[b]);
+ vector<solution> s = solution::direct_sol_r(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)) {
diff --git a/problem.hpp b/problem.hpp
index 4933ed6..e51b722 100644
--- a/problem.hpp
+++ b/problem.hpp
@@ -92,6 +92,8 @@ struct solution {
// simple direct solution from A to B, takes into account no obstacles
static std::vector<solution> direct_sol(const hilare_a &pos_a, const hilare_a &pos_b);
+ // same but try to rotate a bit before and after
+ static std::vector<solution> direct_sol_r(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;