aboutsummaryrefslogtreecommitdiffstats
path: root/problem.hpp
blob: 8851fa29f8ac48b7fa3c6337b3c0dd05d8517e9d (plain) (blame)
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 :*/