1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
#pragma once
#include <vector>
#include <map>
#include <SFML/System.hpp>
#include "geom.hpp"
struct obstacle {
circle c;
obstacle(circle cc) : c(cc) {}
};
struct hilare_a_param {
// paramètres globaux
double l;
double r_c_car, r_c_trolley;
};
struct hilare_a { // System A
hilare_a_param *param;
// position actuelle
double x, y, theta, phi;
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();
}
};
struct problem {
std::vector<obstacle> obstacles;
hilare_a begin_pos, end_pos;
};
struct hilare_a_mvt {
// Describes an elementary movement : rotate car and run on a circle
// Hilare se déplace sur un arc de cercle. Le chariot donne la contrainte
// par rapport à la droite sur laquelle se place le centre de ce cercle
// (c'est la droite perpendiculaire à dir_trolley() passant par pos_trolley())
// deux étapes dans le mouvement :
// - bien orienter la voiture (c'est l'angle dtheta_before)
// - avancer/reculer sur le cercle (c'est l'angle domega)
hilare_a_mvt() : center(0, 0) {}
hilare_a from, to;
bool is_arc; // true = circle arc ; false = straight line (phi = 0)
double dtheta_before; // rotation de la voiture sur elle-même avant
// CAS D'UN ARC DE CERCLE
vec center;
double domega; // angle parcouru sur le cercle de centre center
// CAS D'UN DEPLACEMENT EN LIGNE DROITE
double ds; // longueur par
double length(); // length of a movement
bool intersects(const obstacle& o) const; // intersects an obstacle ?
bool intersects(const problem &p) const; // intersects any obstacle on the map ?
};
struct solution {
std::vector<hilare_a_mvt> movement;
solution() {}
solution(const std::vector<hilare_a_mvt> &m) : movement(m) {}
// simple direct solution from A to B, takes into account no obstacles
static solution direct_sol(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;
};
struct solver_internal {
// intermediate data for the solver
// 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();
solver_internal peek_internal();
};
/* vim: set ts=4 sw=4 tw=0 noet :*/
|
