105 lines
2.1 KiB
C++
105 lines
2.1 KiB
C++
#ifndef IPIN_SCALER_H
|
|
#define IPIN_SCALER_H
|
|
|
|
#include <Indoor/geo/Point2.h>
|
|
#include <Indoor/geo/Point3.h>
|
|
|
|
struct IPIN {
|
|
double lat;
|
|
double lon;
|
|
double floorNr;
|
|
IPIN(const double lat, const double lon, const double floorNr) : lat(lat), lon(lon), floorNr(floorNr) {;}
|
|
};
|
|
|
|
class IPINScaler {
|
|
|
|
private:
|
|
|
|
int w;
|
|
int h;
|
|
double cenLat;
|
|
double cenLon;
|
|
double rotDeg;
|
|
double mPerPx;
|
|
|
|
Point2 mapCenter_m;
|
|
|
|
double refLat;
|
|
double m_per_deg_lat;
|
|
double m_per_deg_lon;
|
|
|
|
const float fixedFloorHeight = 4.0f;
|
|
|
|
public:
|
|
|
|
|
|
/** image width, image height, map-center (lat/lon), map-rotation, meter<->pixel conversion factor */
|
|
IPINScaler(int w, int h, double cenLat, double cenLon, double rotDeg, double mPerPx) : w(w), h(h), cenLat(cenLat), cenLon(cenLon), rotDeg(rotDeg), mPerPx(mPerPx) {
|
|
|
|
mapCenter_m = Point2( (w*mPerPx/2), (h*mPerPx/2) );
|
|
|
|
refLat = cenLat / 180.0 * M_PI;
|
|
m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat);
|
|
m_per_deg_lon = 111132.954 * std::cos ( refLat );
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** convert from (x,y,z) to (lat,lon,floorNr) */
|
|
IPIN toIPIN3(const float x, const float y, const float z) const {
|
|
|
|
Point2 pos(x,y);
|
|
|
|
// move to (0,0)
|
|
pos -= mapCenter_m;
|
|
|
|
// undo the rotation
|
|
pos = pos.rotated(-rotDeg / 180 * M_PI);
|
|
|
|
const double lat = cenLat + (pos.y / m_per_deg_lat);
|
|
const double lon = cenLon + (pos.x / m_per_deg_lon);
|
|
const double floorNr = z / fixedFloorHeight;
|
|
|
|
return IPIN(lat, lon, floorNr);
|
|
|
|
}
|
|
|
|
IPIN toIPIN3(const Point3 p) const {
|
|
return toIPIN3(p.x, p.y, p.z);
|
|
}
|
|
|
|
Point3 convert3D(const IPIN& ipin) const {
|
|
return convert3D(ipin.lat, ipin.lon, ipin.floorNr);
|
|
}
|
|
|
|
Point3 convert3D(double lat, double lon, float floorNr) const {
|
|
|
|
Point2 p2 = convert2D(lat, lon);
|
|
|
|
return Point3(p2.x, p2.y, floorNr * fixedFloorHeight);
|
|
|
|
}
|
|
|
|
Point2 convert2D(double lat, double lon) 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 // IPIN_SCALER_H
|