PathsBuilder.h 11.8 KB
Newer Older
1
2
3
4
5
/**
 * calculate obstacles' voronoi edge paths
 * @author Tobias Weber <tweber@ill.fr>
 * @date jun-2021
 * @license GPLv3, see 'LICENSE' file
6
7
8
 *
 * ----------------------------------------------------------------------------
 * TAS-Paths (part of the Takin software suite)
Tobias WEBER's avatar
Tobias WEBER committed
9
 * Copyright (C) 2021  Tobias WEBER (Institut Laue-Langevin (ILL),
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 *                     Grenoble, France).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
24
25
26
27
28
 */

#ifndef __GEO_PATHS_BUILDER_H__
#define __GEO_PATHS_BUILDER_H__

29
#include <memory>
30
31
#include <iostream>

32
33
#include <boost/signals2/signal.hpp>

34
35
36
37
38
39
40
#include "src/libs/voronoi_lines.h"
#include "src/libs/voronoi.h"
#include "src/libs/img.h"
#include "src/libs/graphs.h"
#include "src/libs/hull.h"
#include "tlibs2/libs/maths.h"

41
#include "types.h"
Tobias WEBER's avatar
Tobias WEBER committed
42
#include "InstrumentSpace.h"
Tobias WEBER's avatar
Tobias WEBER committed
43
#include "TasCalculator.h"
44
#include "PathsExporter.h"
Tobias WEBER's avatar
Tobias WEBER committed
45

46

Tobias WEBER's avatar
Tobias WEBER committed
47
48
struct InstrumentPath
{
49
	// path mesh ok?
Tobias WEBER's avatar
Tobias WEBER committed
50
51
	bool ok = false;

52
	// initial and final vertices on path (in pixel coordinates)
53
54
55
56
57
58
	t_vec2 vec_i;
	t_vec2 vec_f;

	// are the initial and final vertices on a linear or a quadratic bisector?
	bool is_linear_i = true;
	bool is_linear_f = true;
Tobias WEBER's avatar
Tobias WEBER committed
59

60
	// indices of the voronoi vertices on the path mesh
Tobias WEBER's avatar
Tobias WEBER committed
61
	std::vector<std::size_t> voronoi_indices;
Tobias WEBER's avatar
Tobias WEBER committed
62
63

	// position parameter along the entry and exit path
64
65
	t_real param_i = 0;
	t_real param_f = 1;
Tobias WEBER's avatar
Tobias WEBER committed
66
67
68
};


Tobias WEBER's avatar
Tobias WEBER committed
69
70
71
72
73
74
75
76
77
78
79
80
81
/**
 * strategy for finding the path
 */
enum class PathStrategy
{
	// find shortest path
	SHORTEST,

	// avoid paths close to walls
	PENALISE_WALLS,
};


82
83
84
85
86
87
88
89
90
91
92
93
94
/**
 * backend to use for voronoi diagram calculation
 */
enum class VoronoiBackend
{
	// boost.polygon
	BOOST,

	// cgal
	CGAL,
};


95
96
class PathsBuilder
{
97
public:
98
	// contour point
99
	using t_contourvec = t_vec2_int;
100
101

	// line segment
Tobias WEBER's avatar
Tobias WEBER committed
102
	using t_line = std::pair<t_vec2, t_vec2>;
103

104
	// voronoi edges
Tobias WEBER's avatar
Tobias WEBER committed
105
106
107
	using t_voronoiedge_linear =
		std::tuple<t_line,
		std::optional<std::size_t>,
Tobias WEBER's avatar
Tobias WEBER committed
108
		std::optional<std::size_t>>;
Tobias WEBER's avatar
Tobias WEBER committed
109
	using t_voronoiedge_parabolic =
Tobias WEBER's avatar
Tobias WEBER committed
110
		std::tuple<std::vector<t_vec2>,
Tobias WEBER's avatar
Tobias WEBER committed
111
		std::size_t, std::size_t>;
112
113
114

	// voronoi graph
	using t_graph = geo::AdjacencyList<t_real>;
Tobias WEBER's avatar
Tobias WEBER committed
115
	//using t_graph = geo::AdjacencyMatrix<t_real>;
116

117

118
119
120
121
protected:
	// get path length, taking into account the motor speeds
	t_real GetPathLength(const t_vec2& vec) const;

Tobias WEBER's avatar
Tobias WEBER committed
122
123
124
	// check if a direct path between the two vertices leads to a collision
	bool DoesDirectPathCollide(const t_vec2& vert1, const t_vec2& vert2, bool deg = false) const;

125
126
127
	// get the angular distance of a vertex to the nearest wall
	t_real GetDistToNearestWall(const t_vec2& vertex, bool deg = false) const;

Tobias WEBER's avatar
Tobias WEBER committed
128

129
130
131
132
public:
	PathsBuilder();
	~PathsBuilder();

Tobias WEBER's avatar
Tobias WEBER committed
133
134
135
	PathsBuilder(const PathsBuilder&) = default;
	PathsBuilder& operator=(const PathsBuilder&) = default;

136
	// ------------------------------------------------------------------------
137
	// input instrument
138
	// ------------------------------------------------------------------------
139
	void SetInstrumentSpace(const InstrumentSpace* instr) { m_instrspace = instr; }
Tobias WEBER's avatar
Tobias WEBER committed
140
141
	const InstrumentSpace* GetInstrumentSpace() const { return m_instrspace; }

Tobias WEBER's avatar
Tobias WEBER committed
142
143
	void SetTasCalculator(const TasCalculator* tascalc) { m_tascalc = tascalc; }
	const TasCalculator* GetTasCalculator() const { return m_tascalc; }
144
	// ------------------------------------------------------------------------
145

Tobias WEBER's avatar
Tobias WEBER committed
146
	// ------------------------------------------------------------------------
147
	// conversion functions
Tobias WEBER's avatar
Tobias WEBER committed
148
149
150
	// ------------------------------------------------------------------------
	t_vec2 PixelToAngle(const t_vec2& pix, bool deg = true, bool inc_sense = false) const;
	t_vec2 AngleToPixel(const t_vec2& angle, bool deg = true, bool inc_sense = false) const;
Tobias WEBER's avatar
Tobias WEBER committed
151
152
	t_vec2 PixelToAngle(t_real x, t_real y, bool deg = true, bool inc_sense = false) const;
	t_vec2 AngleToPixel(t_real x, t_real y, bool deg = true, bool inc_sense = false) const;
Tobias WEBER's avatar
Tobias WEBER committed
153
	// ------------------------------------------------------------------------
154

155
156
157
	// clear all data
	void Clear();

158
	// get contour image and wall contour points
159
	const geo::Image<std::uint8_t>& GetImage() const { return m_img; }
Tobias WEBER's avatar
Tobias WEBER committed
160
	const std::vector<std::vector<t_contourvec>>& GetWallContours(bool full=false) const;
161

Tobias WEBER's avatar
Tobias WEBER committed
162
	// get voronoi vertices, edges and graph
Tobias WEBER's avatar
Tobias WEBER committed
163
	const geo::VoronoiLinesResults<t_vec2, t_line, t_graph>& GetVoronoiResults() const
Tobias WEBER's avatar
Tobias WEBER committed
164
	{ return m_voro_results; }
165

166
167
168
	// ------------------------------------------------------------------------
	// path mesh calculation workflow
	// ------------------------------------------------------------------------
169
170
171
	void StartPathMeshWorkflow();
	void FinishPathMeshWorkflow(bool successful = true);

172
173
174
	bool CalculateConfigSpace(t_real da2, t_real da4,
		t_real starta2 = 0., t_real enda2 = tl2::pi<t_real>,
		t_real starta4 = 0., t_real enda4 = tl2::pi<t_real>);
175
	bool CalculateWallsIndexTree();
176
	bool CalculateWallContours(bool simplify = true, bool convex_split = false);
Tobias WEBER's avatar
Tobias WEBER committed
177
178
	bool CalculateLineSegments(bool use_region_function = false);
	bool CalculateVoronoi(bool group_lines = true,
179
180
		VoronoiBackend backend = VoronoiBackend::BOOST,
		bool use_region_function = true);
181
182
183
184
185
186
187
188
189

	// number of line segment groups -- for scripting interface
	std::size_t GetNumberOfLineSegmentRegions() const { return m_linegroups.size(); }

	// test if the region is inverted -- for scripting interface
	bool IsRegionInverted(std::size_t groupidx) { return m_inverted_regions[groupidx]; }

	// get line segment group -- for scripting interface
	std::vector<std::array<t_real, 4>> GetLineSegmentRegionAsArray(std::size_t groupidx) const;
190
191
192
193
194
195
	// ------------------------------------------------------------------------

	// ------------------------------------------------------------------------
	// path calculation
	// ------------------------------------------------------------------------
	// find a path from an initial (a2, a4) to a final (a2, a4)
Tobias WEBER's avatar
Tobias WEBER committed
196
197
	InstrumentPath FindPath(t_real a2_i, t_real a4_i, t_real a2_f, t_real a4_f,
		PathStrategy pathstrategy = PathStrategy::SHORTEST);
198
199

	// get individual vertices on an instrument path
Tobias WEBER's avatar
Tobias WEBER committed
200
	std::vector<t_vec2> GetPathVertices(const InstrumentPath& path,
201
202
		bool subdivide_lines = false, bool deg = false) const;

203
	// get individual vertices on an instrument path -- for scripting interface
204
205
206
207
208
209
210
211
	std::vector<std::pair<t_real, t_real>>
		GetPathVerticesAsPairs(const InstrumentPath& path,
			bool subdivide_lines = false, bool deg = false) const;
	// ------------------------------------------------------------------------

	// ------------------------------------------------------------------------
	// options
	// ------------------------------------------------------------------------
Tobias WEBER's avatar
Tobias WEBER committed
212
213
214
	t_real GetEpsilon() const { return m_eps; }
	void SetEpsilon(t_real eps) { m_eps = eps; }

Tobias WEBER's avatar
Tobias WEBER committed
215
216
217
	t_real GetAngularEpsilon() const { return m_eps_angular; }
	void SetAngularEpsilon(t_real eps) { m_eps_angular = eps; }

218
219
220
	t_real GetVoronoiEdgeEpsilon() const { return m_voroedge_eps; }
	void SetVoronoiEdgeEpsilon(t_real eps) { m_voroedge_eps = eps; }

Tobias WEBER's avatar
Tobias WEBER committed
221
222
223
	t_real GetSubdivisionLength() const { return m_subdiv_len; }
	void SetSubdivisionLength(t_real len) { m_subdiv_len = len; }

224
225
226
	t_real GetMinDistToWalls() const { return m_min_angular_dist_to_walls; }
	void SetMinDistToWalls(t_real dist) { m_min_angular_dist_to_walls = dist; }

Tobias WEBER's avatar
Tobias WEBER committed
227
228
229
	void SetRemoveBisectorsBelowMinWallDist(bool b) { m_remove_bisectors_below_min_wall_dist = b; }
	bool GetRemoveBisectorsBelowMinWallDist() const { return m_remove_bisectors_below_min_wall_dist; }

Tobias WEBER's avatar
Tobias WEBER committed
230
231
	unsigned int GetMaxNumThreads() const { return m_maxnum_threads; }
	void SetMaxNumThreads(unsigned int n) { m_maxnum_threads = n; }
232

Tobias WEBER's avatar
Tobias WEBER committed
233
234
235
	bool GetTryDirectPath() const { return m_directpath; }
	void SetTryDirectPath(bool directpath) { m_directpath = directpath; }

236
237
	bool GetVerifyPath() const { return m_verifypath; }
	void SetVerifyPath(bool verify) { m_verifypath = verify; }
238
239
240

	bool GetUseMotorSpeeds() const { return m_use_motor_speeds; }
	void SetUseMotorSpeeds(bool b) { m_use_motor_speeds = b; }
241
	// ------------------------------------------------------------------------
242

243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
	// ------------------------------------------------------------------------
	// progress status handler
	// ------------------------------------------------------------------------
	// connection to progress signal
	template<class t_slot>
	boost::signals2::connection AddProgressSlot(const t_slot& slot)
	{ return m_sigProgress->connect(slot); }

	// show progress messages on the console
	void AddConsoleProgressHandler();
	// ------------------------------------------------------------------------

	// ------------------------------------------------------------------------
	// exporting of data
	// ------------------------------------------------------------------------
	// save contour line segments to lines test tools
	bool SaveToLinesTool(std::ostream& ostr);
	bool SaveToLinesTool(const std::string& filename);

Tobias WEBER's avatar
Tobias WEBER committed
262
	// export the path to various formats using a visitor
263
	bool AcceptExporter(const PathsExporterBase *exporter,
Tobias WEBER's avatar
Tobias WEBER committed
264
		const std::vector<t_vec2>& path, bool path_in_rad = false)
265
	{ return exporter->Export(this, path, path_in_rad); }
266
267
	// ------------------------------------------------------------------------

268

269
270
private:
	const InstrumentSpace *m_instrspace{};
Tobias WEBER's avatar
Tobias WEBER committed
271
	const TasCalculator *m_tascalc{};
272

273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
	// and-combine return values for calculation progress signal
	struct combine_sigret
	{
		using result_type = bool;

		template<class t_iter>
		bool operator()(t_iter begin, t_iter end) const
		{
			result_type ret = true;

			for(t_iter iter = begin; iter != end; iter = std::next(iter))
				ret = ret && *iter;

			return ret;
		}
	};

290
	// calculation progress signal
291
292
	using t_sig_progress =
		boost::signals2::signal<
293
			bool(CalculationState, t_real progress, const std::string& message),
294
			combine_sigret>;
295
	std::shared_ptr<t_sig_progress> m_sigProgress{};
296

Tobias WEBER's avatar
Tobias WEBER committed
297
298
299
300
	// angular ranges
	t_real m_monoScatteringRange[2]{0, tl2::pi<t_real>};
	t_real m_sampleScatteringRange[2]{0, tl2::pi<t_real>};

301
	// index tree for wall positions (in pixel coordinates)
302
303
	geo::ClosestPixelTreeResults<t_contourvec> m_wallsindextree{};

304
	// wall contours in configuration space
305
	geo::Image<std::uint8_t> m_img{};
Tobias WEBER's avatar
Tobias WEBER committed
306
307
	std::vector<std::vector<t_contourvec>> m_wallcontours = {};
	std::vector<std::vector<t_contourvec>> m_fullwallcontours = {};
308

309
	// line segments (in pixel coordinates) and groups from the wall contours
310
311
	std::vector<t_line> m_lines{};
	std::vector<std::pair<std::size_t, std::size_t>> m_linegroups{};
312

313
	// arbitrary points outside the regions
Tobias WEBER's avatar
Tobias WEBER committed
314
	std::vector<t_vec2> m_points_outside_regions{};
Tobias WEBER's avatar
Tobias WEBER committed
315
	std::vector<bool> m_inverted_regions{};
Tobias WEBER's avatar
Tobias WEBER committed
316

317
	// voronoi vertices, edges and graph from the line segments
Tobias WEBER's avatar
Tobias WEBER committed
318
	geo::VoronoiLinesResults<t_vec2, t_line, t_graph> m_voro_results{};
319

Tobias WEBER's avatar
Tobias WEBER committed
320
	// general, angular and voronoi edge calculation epsilon
Tobias WEBER's avatar
Tobias WEBER committed
321
	t_real m_eps = 1e-3;
Tobias WEBER's avatar
Tobias WEBER committed
322
	t_real m_eps_angular = 1e-3;
323
	t_real m_voroedge_eps = 1e-2;
Tobias WEBER's avatar
Tobias WEBER committed
324

325
326
	// minimum distance to keep from the walls (e.g. for direct path calculation)
	t_real m_min_angular_dist_to_walls = 5. / t_real(180.) * tl2::pi<t_real>;
Tobias WEBER's avatar
Tobias WEBER committed
327

Tobias WEBER's avatar
Tobias WEBER committed
328
329
330
	// remove bisectors that are below the minimum distance given above
	bool m_remove_bisectors_below_min_wall_dist = true;

331
332
333
	// minimum distance to consider "staircase artefacts"
	t_real m_simplify_mindist = 3.;

334
335
	bool m_use_motor_speeds = true;

Tobias WEBER's avatar
Tobias WEBER committed
336
337
338
	// line segment length for subdivisions
	t_real m_subdiv_len = 0.1;

Tobias WEBER's avatar
Tobias WEBER committed
339
340
341
	// try to find a direct path if possible
	bool m_directpath = true;

342
343
344
345
	// check the generated path for collisions
	bool m_verifypath = true;

	// maximum number of threads to use in calculations
Tobias WEBER's avatar
Tobias WEBER committed
346
	unsigned int m_maxnum_threads = 4;
347
348
349
};

#endif