Indoor/floorplan/v2/FloorplanHelper.h

/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef FLOORPLANHELPER2_H
#define FLOORPLANHELPER2_H

#include "../../geo/BBox3.h"
#include "../../geo/Line2.h"
#include "../../Assertions.h"

#include "Floorplan.h"

#include "../../sensors/MACAddress.h"

/**
 * helper methods for the floorplan
 */
class FloorplanHelper {

public:


	/** get the AP for the given MAC [if available] */
	static std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> getAP(const Floorplan::IndoorMap* map, const MACAddress& mac) {
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {
				if (MACAddress(ap->mac) == mac) {
					return std::make_pair(ap, f);
				}
			}
		}
		return std::make_pair(nullptr, nullptr);
	}

	/** get the AP for the given Name [if available] */
	static std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> getAPByName(const Floorplan::IndoorMap* map, const std::string& name) {
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {
				if (name == ap->name) {
					return std::make_pair(ap, f);
				}
			}
		}
		return std::make_pair(nullptr, nullptr);
	}

	/** get all APs within the map */
	static std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> getAPs(const Floorplan::IndoorMap* map) {
		std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> res;
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {
				res.push_back(std::make_pair(ap,f));
			}
		}
		return res;
	}

	/** get the Fingerprint-Loation for the given Name [if available] */
	static std::pair<Floorplan::FingerprintLocation*, Floorplan::Floor*> getFingerprintLocationByName(const Floorplan::IndoorMap* map, const std::string& name) {
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::FingerprintLocation* fpl : f->fpLocations) {
				if (name == fpl->name) {
					return std::make_pair(fpl, f);
				}
			}
		}
		return std::make_pair(nullptr, nullptr);
	}

	/** get all ground-truth points within the map as hash-map: id->pos */
	static std::unordered_map<int, Point3> getGroundTruthPoints(const Floorplan::IndoorMap* map) {
		std::unordered_map<int, Point3> res;
		for (const Floorplan::Floor* f : map->floors) {
			for (const Floorplan::GroundTruthPoint* gtp : f->gtpoints) {
				res[gtp->id] = gtp->getPosition(*f);
			}
		}
		return res;
	}

	/** get all ground-truth points, identified by the given indices */
	static std::vector<Point3> getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> indices) {

		// get a map id->pos for all ground-truth-points within the map
		const std::unordered_map<int, Point3> src = getGroundTruthPoints(map);
		std::vector<Point3> res;
		for (const int idx : indices) {
			const auto& it = src.find(idx);
			if (it == src.end()) {throw Exception("map does not contain a ground-truth-point with ID " + std::to_string(idx));}
			res.push_back(it->second);
		}
		return res;
	}

public:

	/** align all floorplan values to the given grid size. needed for the grid factory */
	static void align(Floorplan::IndoorMap* map, const int gridSize_cm) {
		for (Floorplan::Floor* floor : map->floors) {
			floor->atHeight = align_m(floor->atHeight, gridSize_cm);
			floor->height = align_m(floor->height, gridSize_cm);
			for (Floorplan::Stair* stair : floor->stairs) {
				for (Floorplan::StairPart& part : ((Floorplan::StairFreeform*)stair)->parts) {
					part.start.z = align_m(part.start.z, gridSize_cm);
					part.end.z = align_m(part.end.z, gridSize_cm);
				}
			}
		}
	}

	static inline float align_m(const float val_m, const int gridSize_cm) {
		const float val_cm = val_m * 100;
		const int snapped = std::round(val_cm / gridSize_cm);
		const float res_cm = snapped * gridSize_cm;
		return res_cm / 100.0f;
	}

	/** get a BBox for the whole map */
	static BBox3 getBBox(const Floorplan::IndoorMap* map) {

		BBox3 bbox;

		for (const Floorplan::Floor* floor : map->floors) {
			const BBox3 bbFloor = getBBox(floor);
			bbox.add(bbFloor);
		}

		return bbox;

	}

	/** get a BBox for the whole floor */
	static BBox3 getBBox(const Floorplan::Floor* floor) {

		BBox3 bbox;

		Assert::isNot0(floor->outline.size(), "floor " + floor->name + " has no outline!");

		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
			for (const Point2 p2 : poly->poly.points) {
				bbox.add(Point3(p2.x, p2.y, floor->atHeight));
				bbox.add(Point3(p2.x, p2.y, floor->atHeight + floor->height));
			}
		}

		return bbox;

	}

	/** does the map contain an obstacle that intersects the given line? (in meter!) */
	static bool intersectsObstacle(const Floorplan::IndoorMap* map, const Point3& p1, const Point3& p2) {

		// sanity check
		if (std::abs(p2.z-p1.z) > 0.5) {
			return false;}

		for (const Floorplan::Floor* floor : map->floors) {
			if (intersectsObstacle(floor, p1, p2)) {return true;}
		}

		return false;

	}

	/** does the floor contain an obstacle that intersects the given line? (in meter!) */
	static bool intersectsObstacle(const Floorplan::Floor* floor, const Point3& p1, const Point3& p2) {

		// sanity check
		if (std::abs(p2.z-p1.z) > 0.5) {
			return false;}

		// only inspect elements that lie near the ground
		if (p1.z < floor->getStartingZ() - 0.15)	{return false;}
		if (p1.z > floor->getStartingZ() + 0.15)	{return false;}

		const Line2 line1(p1.x,p1.y,   p2.x,p2.y);

		for (const Floorplan::FloorObstacle* obs : floor->obstacles) {

			if (dynamic_cast<const Floorplan::FloorObstacleLine*>(obs)) {
				const Floorplan::FloorObstacleLine* obsLine = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
				const Line2 line2(obsLine->from.x, obsLine->from.y, obsLine->to.x, obsLine->to.y);
				if (line1.getSegmentIntersection(line2)) {return true;}
			} else if (dynamic_cast<const Floorplan::FloorObstacleDoor*>(obs)) {
				;
			} else {
				throw "not yet supported";
			}

		}

		return false;

	}

};

#endif // FLOORPLANHELPER2_H