165 lines
4.2 KiB
C++
165 lines
4.2 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Einige Aenderungen beigetragen von Toni Fetzer
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef FLOORPLANCEILINGS_H
|
||
#define FLOORPLANCEILINGS_H
|
||
|
||
#include "Floorplan.h"
|
||
|
||
namespace Floorplan {
|
||
|
||
/**
|
||
* helper-class for floorplan ceilings
|
||
* e.g. to determine the number of ceilings between two given positions
|
||
*/
|
||
class Ceilings {
|
||
|
||
private:
|
||
|
||
/** position (height) of all ceilings (in meter) */
|
||
std::vector<float> ceilingsAtHeight_m;
|
||
|
||
public:
|
||
|
||
/** empty ctor */
|
||
Ceilings() {
|
||
;
|
||
}
|
||
|
||
/** ctor with the map to work with */
|
||
Ceilings(const IndoorMap* map) {
|
||
|
||
// sanity checks
|
||
Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
|
||
|
||
// get position of all ceilings
|
||
for (Floorplan::Floor* f : map->floors) {
|
||
|
||
const float h1 = f->atHeight;
|
||
const float h2 = f->atHeight + f->height;
|
||
|
||
if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h1) == ceilingsAtHeight_m.end()) {
|
||
ceilingsAtHeight_m.push_back(h1);
|
||
}
|
||
if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h2) == ceilingsAtHeight_m.end()) {
|
||
ceilingsAtHeight_m.push_back(h2);
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
std::vector<float> getCeilings() const {
|
||
return ceilingsAtHeight_m;
|
||
}
|
||
|
||
void addCeiling(const float height_m) {
|
||
ceilingsAtHeight_m.push_back(height_m);
|
||
}
|
||
|
||
void clear() {
|
||
ceilingsAtHeight_m.clear();
|
||
}
|
||
|
||
/** get a fading number of floors between ap and person using sigmod in the area where the ceiling is */
|
||
float numCeilingsBetweenFloat(const Point3 ap, const Point3 person) const {
|
||
|
||
// sanity checks
|
||
Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
|
||
|
||
// fading between floors using sigmoid
|
||
const float near = 1.0;
|
||
float cnt = 0;
|
||
|
||
|
||
for (const float z : ceilingsAtHeight_m) {
|
||
|
||
const float myDistToCeil = (ap.z < person.z) ? (person.z - z) : (z - person.z);
|
||
if ( std::abs(myDistToCeil) < near ) {
|
||
cnt += sigmoid(myDistToCeil * 6);
|
||
} else if (ap.z < z && person.z >= z+near) { // AP below celing, me above ceiling
|
||
cnt += 1;
|
||
} else if (ap.z > z && person.z <= z-near) { // AP above ceiling, me below ceiling
|
||
cnt += 1;
|
||
}
|
||
|
||
}
|
||
|
||
return cnt;
|
||
|
||
}
|
||
|
||
float numCeilingsBetweenLinearInt(const Point3 ap, const Point3 person) const {
|
||
|
||
// sanity checks
|
||
Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
|
||
|
||
int cnt = 0;
|
||
float sum = 0;
|
||
for (float z = -1.0; z <= +1.0; z+= 0.25) {
|
||
sum += numCeilingsBetween(ap, person + Point3(0,0,z));
|
||
++cnt;
|
||
}
|
||
|
||
return sum/cnt;
|
||
|
||
}
|
||
|
||
/** get the number of ceilings between z1 and z2 */
|
||
int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
|
||
|
||
// sanity checks
|
||
Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
|
||
|
||
// find min and max height given the to-be-compared points
|
||
const float zMin = std::min(pos1.z, pos2.z);
|
||
const float zMax = std::max(pos1.z, pos2.z);
|
||
|
||
#ifdef WITH_ASSERTIONS
|
||
|
||
static uint64_t numNear = 0;
|
||
static uint64_t numFar = 0;
|
||
|
||
for (const float z : ceilingsAtHeight_m) {
|
||
const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
|
||
if (diff < 0.1) {++numNear;} else {++numFar;}
|
||
}
|
||
if ((numNear + numFar) > 150000) {
|
||
Assert::isTrue(numNear < numFar*0.5,
|
||
"many requests to Floorplan::Ceilings::numCeilingsBetween address nodes (very) near to a ground! \
|
||
due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
|
||
expect very wrong outputs! \
|
||
consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
|
||
);
|
||
}
|
||
#endif
|
||
|
||
int cnt = 0;
|
||
for (const float z : ceilingsAtHeight_m) {
|
||
if (zMin < z && zMax > z) {++cnt;}
|
||
}
|
||
|
||
return cnt;
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
static inline float sigmoid(const float val) {
|
||
return 1.0f / (1.0f + std::exp(-val));
|
||
}
|
||
|
||
};
|
||
|
||
}
|
||
|
||
#endif // FLOORPLANCEILINGS_H
|