frank
1c2081d406
adjusted grid factory adjusted nav mesh factory minoor changes/fixes new helper classes refactoring
142 lines
3.4 KiB
C++
142 lines
3.4 KiB
C++
#ifndef INDOOR_SYNTHETICPATH_H
|
|
#define INDOOR_SYNTHETICPATH_H
|
|
|
|
#include "../math/Interpolator.h"
|
|
#include "../floorplan/v2/Floorplan.h"
|
|
#include "../floorplan/v2/FloorplanHelper.h"
|
|
#include "../floorplan/v2/FloorplanHelper.h"
|
|
|
|
/** allows interpolation along a synthetic path */
|
|
class SyntheticPath : private Interpolator<float, Point3> {
|
|
|
|
const Floorplan::IndoorMap* map;
|
|
|
|
public:
|
|
|
|
using Base = Interpolator<float, Point3>;
|
|
using Entry = Base::InterpolatorEntry;
|
|
|
|
/** create path using the given ground-truth points from the map */
|
|
void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
|
|
|
|
this->map = map;
|
|
|
|
// get all ground-truth points from the map
|
|
auto gtps = FloorplanHelper::getGroundTruthPoints(map);
|
|
float dist = 0;
|
|
|
|
// create distance-based entries within the interpolator
|
|
for (int i = 0; i < ids.size(); ++i) {
|
|
const int id = ids[i];
|
|
Point3 gtp = gtps[id];
|
|
if (i >= 1) {
|
|
Point3 gtpPrev = gtps[id-1];
|
|
dist += gtpPrev.getDistance(gtp);
|
|
}
|
|
Base::add(dist, gtp);
|
|
}
|
|
|
|
}
|
|
|
|
/** get the path's length */
|
|
float getLength() const {
|
|
//return Base::getEntries().back().key;
|
|
float dist = 0;
|
|
for (size_t i = 0; i < getEntries().size()-1; ++i) {
|
|
const auto& e1 = getEntries()[i];
|
|
const auto& e2 = getEntries()[i+1];
|
|
dist += e1.value.getDistance(e2.value);
|
|
}
|
|
return dist;
|
|
}
|
|
|
|
|
|
/** get all individual entries from the underlying data-structure */
|
|
const std::vector<Entry>& getEntries() const {
|
|
return Base::getEntries();
|
|
}
|
|
|
|
|
|
|
|
/** smooth harsh angles */
|
|
void smooth(float delta = 1, int numRuns = 1) {
|
|
|
|
float t = delta*2;
|
|
|
|
for (int j = 0; j < numRuns; ++j) {
|
|
|
|
t/=2;
|
|
|
|
for (int i = 1; i < (int)entries.size()-1; ++i) {
|
|
|
|
// the entry to-be-replaced by two others
|
|
const Entry& e = entries[i];
|
|
const float key = e.key;
|
|
|
|
const Entry e1(key-t, Base::get(key-t));
|
|
const Entry e2(key+t, Base::get(key+t));
|
|
|
|
entries.erase(entries.begin()+i); --i;
|
|
++i; entries.insert(entries.begin()+i, e1);
|
|
++i; entries.insert(entries.begin()+i, e2);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// /** smooth harsh angles */
|
|
// void smooth(float delta = 1, int numRuns = 1) {
|
|
|
|
// float t = delta/numRuns;
|
|
|
|
// for (int i = 1; i < (int)entries.size()-1; ++i) {
|
|
|
|
// // the entry to-be-replaced by several others
|
|
// const Entry& e = entries[i];
|
|
// const float key = e.key;
|
|
|
|
// std::vector<Entry> newEntries;
|
|
// for (int x = -numRuns; x <= +numRuns; x+= 2) {
|
|
// const float percent = (float)x / (float)numRuns;
|
|
// const float keyO = key + percent*t;
|
|
// const Point3 pos1 = Base::get(keyO-t*2);
|
|
// const Point3 pos2 = Base::get(keyO+t*2);
|
|
// const Point3 posO = (pos1+pos2) / 2;
|
|
// Entry e(keyO, posO);
|
|
// newEntries.push_back(e);
|
|
// }
|
|
|
|
// entries.erase(entries.begin()+i); --i;
|
|
|
|
// for (const Entry& e : newEntries) {
|
|
// ++i;
|
|
// entries.insert(entries.begin()+i, e);
|
|
// }
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
/** get the position along the path after the given walking distance */
|
|
Point3 getPosAfterDistance(const float distance) const {
|
|
return Base::get(distance);
|
|
}
|
|
|
|
bool doneAtDistance(const float distance) const {
|
|
return Base::getMaxKey() < distance;
|
|
}
|
|
|
|
/** at the given distance: are we walking on a plain surface or up/down? */
|
|
bool isPlain(const float distance) const {
|
|
const Point3 pos1 = getPosAfterDistance(distance);
|
|
const Point3 pos2 = getPosAfterDistance(distance + 0.1);
|
|
const float delta = std::abs(pos1.z - pos2.z);
|
|
return delta < 0.01;
|
|
}
|
|
|
|
};
|
|
|
|
#endif // INDOOR_SYNTEHTICPATH_H
|