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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
|
#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 pos_trolley() const {
return pos() - vec::from_polar(param->l, theta + phi);
}
vec canon_curve_center() const {
vec a = pos();
vec b = pos_trolley();
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 {
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 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;
double length();
};
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);
// internal
void find_direct_path(int a, int b, 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 :*/
|
