76 lines
1.5 KiB
C++
76 lines
1.5 KiB
C++
#ifndef EARTHMAPPING_H
|
|
#define EARTHMAPPING_H
|
|
|
|
#include "Point3.h"
|
|
#include "EarthPos.h"
|
|
|
|
/**
|
|
* mapping between positions on earth and positions within the floorplan
|
|
*/
|
|
class EarthMapping {
|
|
|
|
private:
|
|
|
|
/** one 3D position within the floorplan */
|
|
Point3 center_map_m;
|
|
|
|
/** corresponding 3D position on earth */
|
|
EarthPos center_earth;
|
|
|
|
/** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */
|
|
float rotation_deg;
|
|
|
|
double m_per_deg_lat;
|
|
double m_per_deg_lon;
|
|
|
|
|
|
public:
|
|
|
|
void build() {
|
|
|
|
// TODO
|
|
|
|
}
|
|
|
|
/** convert from map-coordinates to earth-coordinates */
|
|
EarthPos mapToWorld(const Point3 mapPos_m) const {
|
|
|
|
Point3 pos = mapPos_m;
|
|
|
|
// move to (0,0,0)
|
|
pos -= center_map_m;
|
|
|
|
// undo the rotation
|
|
const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI);
|
|
|
|
// convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center
|
|
const double lat = cenLat + (xy.y / m_per_deg_lat);
|
|
const double lon = cenLon + (xy.x / m_per_deg_lon);
|
|
const float height = pos.z;
|
|
|
|
// done
|
|
return EarthPos(lat, lon, height);
|
|
|
|
}
|
|
|
|
/** convert from earth-coordinates to map-coordinates */
|
|
Point3 worldToMap(const EarthPos earthPos) const {
|
|
|
|
const double y_m = +(lat-cenLat) * m_per_deg_lat;
|
|
const double x_m = +(lon-cenLon) * m_per_deg_lon;
|
|
|
|
// rotate (our map is axis aligned)
|
|
Point2 pos(x_m, y_m);
|
|
pos = pos.rotated(rotDeg / 180 * M_PI);
|
|
|
|
// apply movement
|
|
pos += mapCenter_m;
|
|
|
|
return pos;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // EARTHMAPPING_H
|