-
Notifications
You must be signed in to change notification settings - Fork 4
/
liansearch.h
executable file
·103 lines (68 loc) · 3.87 KB
/
liansearch.h
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
#ifndef LIANSEARCH_H
#define LIANSEARCH_H
#include "gl_const.h"
#include "map.h"
#include "node.h"
#include "openlist.h"
#include "search.h"
#include <chrono>
#include <cmath>
#include <list>
#include <limits>
#include <unordered_map>
#include <vector>
class LianSearch : public Search {
public:
// Constructor with parameters
LianSearch(float angleLimit_, int distance_, float weight_, unsigned int steplimit_,
float curvatureHeuristicWeight_, bool postsmoother_, float decreaseDistanceFactor_,
int distanceMin_, double PivotRadius_, int numOfParentsToIncreaseRadius_);
~LianSearch();
SearchResult startSearch(Logger *Log, const Map &map); // General searching algorithm
private:
float angleLimit; // Maximal value of deviation angle (turning limit)
int distance; // Minimal value of length of steps
int numOfParentsToIncreaseRadius;
std::vector<int> listOfDistances;
int listOfDistancesSize;
float weight; // Heuristics weight
bool postsmoother; // Smoothing the path after the algorithm
// Heurisic coefficient:
// If there is heuristic that checks deviation of trajectory from line on each
// step, this deviation is multiplyed by this coefficient
float curvatureHeuristicWeight;
float pivotRadius; // Radius of safety circle around every turn point.
unsigned int stepLimit; // Maximum number of iterations, allowed for the algorithm
unsigned int closeSize; // Number of elements in close (elements that were already examined)
float decreaseDistanceFactor; // Value for decreasing the initial distance value
int distanceMin; // Minimal distance value
std::vector<std::vector<circleNode> > circleNodes; // Virtual nodes that create circle around the cell
std::vector<std::pair<int,int> > pivotCircle; // Vector of nodes (shifts) for pivot security check
std::vector<float> angles;
std::list<Node> lppath, hppath; // Final path in two representations
OpenList open; // Open : list of nodes waiting for expanding
std::unordered_multimap<int, Node> close; // Close: list of nodes that were already expanded
// Method that calculate Bresenham's Circle (center - (0, 0)) and writing list of created nodes to circleNodes
void calculateCircle(int radius); // Radius - radius of the circle in cells
void calculatePivotCircle();
int calculatePreferableRadius(const Map &map); // Method calculates the most preferable radius depending on the parameters of the initial map
void calculateDistances();
void calculateLineSegment(std::vector<Node> &line, const Node &start, const Node &goal); // Method builds Bresenham's Line
bool checkLineSegment(const Map &map, const Node &start, const Node &goal); // Method builds Bresenham's Line and check it for impassable parts
// check that there are no obstacle in a safety radius from a turn point
bool checkPivotCircle(const Map &map, const Node ¢er);
double getCost(int a_i, int a_j, int b_i, int b_j) const;
double calcAngle(const Node &dad, const Node &node, const Node &son) const;
bool checkAngle(const Node &dad, const Node &node, const Node &son) const;
bool stopCriterion(); // Check for the ending criteria. Return true if the algorithm should be stopped
int tryToIncreaseRadius(Node curNode);
bool tryToDecreaseRadius(Node &curNode, int width);
void findSuccessors(const Node curNode,std::vector<Node> &successors, const Map &map);
void update(const Node current_node, Node new_node, bool &successors, const Map &map);
bool expand(const Node curNode, const Map &map);
std::list<Node> smoothPath(const std::list<Node>& path, const Map& map);
void makePrimaryPath(Node curNode);
void makeSecondaryPath();
double makeAngles();
};
#endif // LIANSEARCH_H