FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added stuff

Browse Source
This commit is contained in:
toni
2016-04-13 11:10:09 +02:00
parent efc7fa9c12
commit c0ab98005a
3 changed files with 16 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
geo/Heading.h
View File

@@ -46,6 +46,11 @@ public:
return (Heading(*this) += _rad); return (Heading(*this) += _rad);
} }
Heading& operator = (const float _rad) {
rad = _rad;
return *this;
}
/** compare two headings */ /** compare two headings */
bool operator == (const Heading other) const {return rad == other.rad;} bool operator == (const Heading other) const {return rad == other.rad;}
bool operator != (const Heading other) const {return rad != other.rad;} bool operator != (const Heading other) const {return rad != other.rad;}

4
grid/walk/GridWalkPathControl.h
View File

@@ -64,7 +64,7 @@ public:
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) { GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
// proportional change of the heading // proportional change of the heading
static Distribution::Normal<float> dHead(1, 0.01); static Distribution::Normal<float> dHead(1, 0.01);
// proportional change of the to-be-walked distance // proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10); static Distribution::Normal<float> dWalk(1, 0.10);
@@ -97,7 +97,7 @@ private:
const float diff = possibleHead.getDiffHalfRAD(head); const float diff = possibleHead.getDiffHalfRAD(head);
// // compare this heading with the requested one // // compare this heading with the requested one
const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff); const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally // const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance // nodes own importance

10
nav/a-star/AStar.h
View File

@@ -43,9 +43,15 @@ public:
distance[source] = 0.0f; distance[source] = 0.0f;
Q.push(std::make_pair(source,distance[source])); Q.push(std::make_pair(source,distance[source]));
int iter = 0;
//std::cout << (std::string)*source << std::endl;
//std::cout << (std::string)*destination << std::endl;
// proceed until there are now new nodes to follow // proceed until there are now new nodes to follow
while(!Q.empty()) { while(!Q.empty()) {
++iter;
// fetch the next-nearest node from the queue // fetch the next-nearest node from the queue
const T* u = Q.top().first; const T* u = Q.top().first;
@@ -66,12 +72,14 @@ public:
if (distance[&v] > distance[u] + w) { if (distance[&v] > distance[u] + w) {
distance[&v] = distance[u] + w; distance[&v] = distance[u] + w;
parent[&v] = u; parent[&v] = u;
Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source))); Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination)));
} }
} }
} }
//std::cout << iter << std::endl;
// construct the path // construct the path
std::vector<const T*> path; std::vector<const T*> path;