current revision

ipin/Scaler.h

@@ -0,0 +1,104 @@
+#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