diff --git a/CMakeLists.txt b/CMakeLists.txt
index 93813577..1dfb5def 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,7 +35,7 @@ set(LIBS)
 
 
 # Sources
-set(PIP_FOLDERS "." "core" "containers" "thread" "system" "io" "console" "math" "code")
+set(PIP_FOLDERS "." "core" "containers" "thread" "system" "io" "console" "math" "code" "geo")
 include_directories("src")
 foreach(F ${PIP_FOLDERS})
 	include_directories("src/${F}")
diff --git a/src/geo/piellipsoidmodel.cpp b/src/geo/piellipsoidmodel.cpp
new file mode 100644
index 00000000..9b6cb7bf
--- /dev/null
+++ b/src/geo/piellipsoidmodel.cpp
@@ -0,0 +1,37 @@
+#include "piellipsoidmodel.h"
+
+
+PIEllipsoidModel::PIEllipsoidModel() {
+	a = 0.0;
+	flattening = 0.0;
+	eccentricity = 0.0;
+	angVelocity = 0.0;
+}
+
+
+PIEllipsoidModel PIEllipsoidModel::WGS84Ellipsoid() {
+	PIEllipsoidModel v;
+	v.a = 6378137.0;
+	v.flattening = 0.335281066475e-2;
+	v.eccentricity = 8.1819190842622e-2;
+	v.angVelocity = 7.292115e-5;
+	return v;
+}
+
+
+PIEllipsoidModel PIEllipsoidModel::PZ90Ellipsoid() {
+	PIEllipsoidModel v;
+	v.a = 6378136.0;
+	v.flattening = 3.35280373518e-3;
+	v.eccentricity = 8.1819106432923e-2;
+	v.angVelocity = 7.292115e-5;
+	return v;
+}
+
+
+PIEllipsoidModel PIEllipsoidModel::GPSEllipsoid() {
+	PIEllipsoidModel v = WGS84Ellipsoid();
+	v.angVelocity = 7.2921151467e-5;
+	return v;
+}
+
diff --git a/src/geo/piellipsoidmodel.h b/src/geo/piellipsoidmodel.h
new file mode 100644
index 00000000..db20d80b
--- /dev/null
+++ b/src/geo/piellipsoidmodel.h
@@ -0,0 +1,46 @@
+/*! \file piellipsoidmodel.h
+ * \brief Contains geo ellipsoid models
+*/
+/*
+    PIP - Platform Independent Primitives
+    Contains geo ellipsoid models
+    Copyright (C) 2015 Andrey Bychkov work.a.b@yandex.ru
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef PIELLIPSOIDMODEL_H
+#define PIELLIPSOIDMODEL_H
+
+
+#include "pimathbase.h"
+
+class PIEllipsoidModel {
+public:
+	PIEllipsoidModel();
+	double eccSquared() const { return eccentricity * eccentricity; } // eccentricity squared
+
+	static PIEllipsoidModel WGS84Ellipsoid();
+	static PIEllipsoidModel PZ90Ellipsoid();
+	static PIEllipsoidModel GPSEllipsoid();
+
+	double a; /// Major axis of Earth in meters
+	double flattening; /// Flattening (ellipsoid parameter)
+	double eccentricity; /// Eccentricity (ellipsoid parameter)
+	double angVelocity; /// Angular velocity of Earth in radians/sec
+};
+
+
+
+#endif // PIELLIPSOIDMODEL_H
diff --git a/src/geo/pigeoposition.cpp b/src/geo/pigeoposition.cpp
new file mode 100644
index 00000000..f7f240a5
--- /dev/null
+++ b/src/geo/pigeoposition.cpp
@@ -0,0 +1,593 @@
+#include "pigeoposition.h"
+
+const double PIGeoPosition::one_cm_tolerance = 0.01; // One centimeter tolerance.
+const double PIGeoPosition::one_mm_tolerance = 0.001; // One milimeter tolerance.
+const double PIGeoPosition::one_um_tolerance = 0.000001; // One micron tolerance.
+double PIGeoPosition::position_tolerance = PIGeoPosition::one_mm_tolerance; // Default tolerance in meters.
+
+
+PIGeoPosition::PIGeoPosition() {
+	initialize(PIMathVectorT3d());
+}
+
+
+PIGeoPosition::PIGeoPosition(double a, double b, double c, PIGeoPosition::CoordinateSystem s, PIEllipsoidModel ell) {
+	PIMathVectorT3d v;
+	v[0] = a;
+	v[1] = b;
+	v[2] = c;
+	initialize(v, s, ell);
+}
+
+
+PIGeoPosition::PIGeoPosition(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem s, PIEllipsoidModel ell) {
+	initialize(v, s, ell);
+}
+
+
+PIGeoPosition PIGeoPosition::transformTo(PIGeoPosition::CoordinateSystem sys) {
+	if(sys == Unknown || sys == s) return *this;
+	PIGeoPosition tmp(*this);
+	switch(s) {
+		case Unknown:
+		return *this;
+		case Geodetic:
+			switch(sys) {
+				case Unknown: case Geodetic: return *this;
+				case Geocentric:
+					convertGeodeticToGeocentric(*this, tmp, el);
+					tmp.s = Geocentric;
+				break;
+				case Cartesian:
+					convertGeodeticToCartesian(*this, tmp, el);
+					tmp.s = Cartesian;
+				break;
+				case Spherical:
+					convertGeodeticToGeocentric(*this, tmp, el);
+					tmp[0] = 90 - tmp[0];   // geocen -> sph
+					tmp.s = Spherical;
+				break;
+			}
+		break;
+		case Geocentric:
+			switch(sys) {
+				case Unknown: case Geocentric: return *this;
+				case Geodetic:
+					convertGeocentricToGeodetic(*this, tmp, el);
+					tmp.s = Geodetic;
+				break;
+				case Cartesian:
+					convertGeocentricToCartesian(*this, tmp);
+					tmp.s = Cartesian;
+				break;
+				case Spherical:
+					tmp[0] = 90 - tmp[0];   // geocen -> sph
+					tmp.s = Spherical;
+				break;
+			}
+		break;
+		case Cartesian:
+			switch(sys) {
+				case Unknown: case Cartesian: return *this;
+				case Geodetic:
+					convertCartesianToGeodetic(*this, tmp, el);
+					tmp.s = Geodetic;
+				break;
+				case Geocentric:
+					convertCartesianToGeocentric(*this, tmp);
+					tmp.s = Geocentric;
+				break;
+				case Spherical:
+					convertCartesianToSpherical(*this, tmp);
+					tmp.s = Spherical;
+				break;
+			}
+		break;
+		case Spherical:
+			switch(sys) {
+				case Unknown: case Spherical: return *this;
+				case Geodetic:
+					(*this)[0] = 90 - (*this)[0];   // sph -> geocen
+					convertGeocentricToGeodetic(*this, tmp, el);
+					tmp.s = Geodetic;
+				break;
+				case Geocentric:
+					tmp[0] = 90 - tmp[0];   // sph -> geocen
+					tmp.s = Geocentric;
+				break;
+				case Cartesian:
+					convertSphericalToCartesian(*this, tmp);
+					tmp.s = Cartesian;
+				break;
+			}
+		break;
+	}
+	*this = tmp;
+	return *this;
+}
+
+double PIGeoPosition::x() const {
+	if(s == Cartesian) return (*this)[0];
+	PIGeoPosition t(*this);
+	t.transformTo(Cartesian);
+	return t[0];
+}
+
+
+double PIGeoPosition::y() const {
+	if(s == Cartesian) return (*this)[1];
+	PIGeoPosition t(*this);
+	t.transformTo(Cartesian);
+	return t[1];
+}
+
+
+double PIGeoPosition::z() const {
+	if(s == Cartesian) return (*this)[2];
+	PIGeoPosition t(*this);
+	t.transformTo(Cartesian);
+	return t[2];
+}
+
+
+double PIGeoPosition::latitudeGeodetic() const {
+	if(s == Geodetic) return (*this)[0];
+	PIGeoPosition t(*this);
+	t.transformTo(Geodetic);
+	return t[0];
+}
+
+
+double PIGeoPosition::latitudeGeocentric() const {
+	if(s == Geocentric) return (*this)[0];
+	PIGeoPosition t(*this);
+	t.transformTo(Geocentric);
+	return t[0];
+}
+
+
+double PIGeoPosition::longitude() const {
+	if(s != Cartesian) return (*this)[1];
+	PIGeoPosition t(*this);
+	t.transformTo(Spherical);
+	return t[1];
+}
+
+
+double PIGeoPosition::theta() const {
+	if(s == Spherical) return (*this)[0];
+	PIGeoPosition t(*this);
+	t.transformTo(Spherical);
+	return t[0];
+}
+
+
+double PIGeoPosition::phi() const {
+	return longitude();
+}
+
+
+double PIGeoPosition::radius() const {
+	if(s == Spherical || s == Geocentric) return (*this)[2];
+	PIGeoPosition t(*this);
+	t.transformTo(Spherical);
+	return t[2];
+}
+
+
+double PIGeoPosition::height() const {
+	if(s == Geodetic) return (*this)[2];
+	PIGeoPosition t(*this);
+	t.transformTo(Geodetic);
+	return t[2];
+}
+
+
+PIGeoPosition &PIGeoPosition::setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell) {
+	if(lat > 90 || lat < -90) {
+		piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in setGeodetic:" << lat;
+		assert(lat <= 90 && lat >= -90);
+	}
+	(*this)[0] = lat;
+	(*this)[1] = lon;
+	if((*this)[1] < 0) (*this)[1] += 360 * (1 + (unsigned long)((*this)[1]/360));
+	else if((*this)[1] >= 360) (*this)[1] -= 360 * (unsigned long)((*this)[1]/360);
+	(*this)[2] = ht;
+	el = ell;
+	s = Geodetic;
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::setGeocentric(double lat, double lon, double rad) {
+	if(lat > 90 || lat < -90) {
+		piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in setGeocentric:" << lat;
+		assert(lat <= 90 && lat >= -90);
+	}
+	if(rad < 0) {
+		piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in setGeocentric:" << rad;
+		assert(rad >= 0);
+	}
+	(*this)[0] = lat;
+	(*this)[1] = lon;
+	(*this)[2] = rad;
+	if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
+	else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
+	s = Geocentric;
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::setSpherical(double theta, double phi, double rad) {
+	if(theta < 0 || theta > 180) {
+		piCout << "[PIGeoPosition]" << "Achtung! Invalid theta in setSpherical:" << theta;
+		assert(theta <= 180 && theta >= 0);
+	}
+	if(rad < 0) {
+		piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in setSpherical:" << rad;
+		assert(rad >= 0);
+	}
+	(*this)[0] = theta;
+	(*this)[1] = phi;
+	(*this)[2] = rad;
+	if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
+	else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
+	s = Spherical;
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::setECEF(double x, double y, double z) {
+	(*this)[0] = x;
+	(*this)[1] = y;
+	(*this)[2] = z;
+	s = Cartesian;
+	return *this;
+}
+
+
+void PIGeoPosition::convertSphericalToCartesian(const PIMathVectorT3d &tpr, PIMathVectorT3d &xyz) {
+	double st = sin(tpr[0] * deg2rad);
+	xyz[0] = tpr[2] * st * cos(tpr[1] * deg2rad);
+	xyz[1] = tpr[2] * st * sin(tpr[1] * deg2rad);
+	xyz[2] = tpr[2] * cos(tpr[0] * deg2rad);
+}
+
+
+void PIGeoPosition::convertCartesianToSpherical(const PIMathVectorT3d &xyz, PIMathVectorT3d &tpr) {
+	tpr[2] = xyz.length();
+	if(tpr[2] <= PIGeoPosition::position_tolerance / 5) { // zero-length Cartesian vector
+		tpr[0] = 90.0;
+		tpr[1] = 0.0;
+		return;
+	}
+	tpr[0] = acos(xyz[2] / tpr[2]) * rad2deg;
+	if(sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]) < PIGeoPosition::position_tolerance / 5) { // pole
+		tpr[1] = 0.0;
+		return;
+	}
+	tpr[1] = atan2(xyz[1],xyz[0]) * rad2deg;
+	if(tpr[1] < 0.0) tpr[1] += 360.0;
+}
+
+
+void PIGeoPosition::convertCartesianToGeodetic(const PIMathVectorT3d &xyz, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
+	double p,slat,nn,htold,latold;
+	p = sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]);
+	if(p < PIGeoPosition::position_tolerance / 5) { // pole or origin
+		llh[0] = (xyz[2] > 0.0 ? 90.0: -90.0);
+		llh[1] = 0.0; // lon undefined, really
+		llh[2] = piAbsd(xyz[2]) - ell.a * sqrt(1.0-ell.eccSquared());
+		return;
+	}
+	llh[0] = atan2(xyz[2], p*(1.0-ell.eccSquared()));
+	llh[2] = 0;
+	for(int i=0; i<5; i++) {
+		slat = sin(llh[0]);
+		nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
+		htold = llh[2];
+		llh[2] = p / cos(llh[0]) - nn;
+		latold = llh[0];
+		llh[0] = atan2(xyz[2], p*(1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
+		if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
+	}
+	llh[1] = atan2(xyz[1], xyz[0]);
+	if(llh[1] < 0.0) llh[1] += M_2PI;
+	llh[0] *= rad2deg;
+	llh[1] *= rad2deg;
+}
+
+
+void PIGeoPosition::convertGeodeticToCartesian(const PIMathVectorT3d &llh, PIMathVectorT3d &xyz, PIEllipsoidModel ell) {
+	double slat = sin(llh[0] * deg2rad);
+	double clat = cos(llh[0] * deg2rad);
+	double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
+	xyz[0] = (nn + llh[2]) * clat * cos(llh[1] * deg2rad);
+	xyz[1] = (nn + llh[2]) * clat * sin(llh[1] * deg2rad);
+	xyz[2] = (nn * (1.0 - ell.eccSquared()) + llh[2]) * slat;
+}
+
+
+void PIGeoPosition::convertCartesianToGeocentric(const PIMathVectorT3d &xyz, PIMathVectorT3d &llr) {
+	convertCartesianToSpherical(xyz, llr);
+	llr[0] = 90.0 - llr[0]; // convert theta to latitude
+}
+
+
+void PIGeoPosition::convertGeocentricToCartesian(const PIMathVectorT3d &llr, PIMathVectorT3d &xyz) {
+	PIMathVectorT3d llh(llr);
+	llh[0] = 90.0 - llh[0]; // convert latitude to theta
+	convertSphericalToCartesian(llh, xyz);
+}
+
+
+void PIGeoPosition::convertGeocentricToGeodetic(const PIMathVectorT3d &llr, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
+	double cl, p, sl, slat, nn, htold, latold;
+	llh[1] = llr[1]; // longitude is unchanged
+	cl = sin((90.0 - llr[0]) * deg2rad);
+	sl = cos((90.0 - llr[0]) * deg2rad);
+	if(llr[2] <= PIGeoPosition::position_tolerance / 5) { // radius is below tolerance, hence assign zero-length, arbitrarily set latitude = longitude = 0
+		llh[0] = llh[1] = 0.0;
+		llh[2] = -ell.a;
+		return;
+	} else if(cl < 1.e-10) { // near pole ... note that 1mm/radius(Earth) = 1.5e-10
+		if(llr[0] < 0.0) llh[0] = -90.0;
+		else llh[0] =  90.0;
+		llh[1] = 0.0;
+		llh[2] = llr[2] - ell.a * sqrt(1.0 - ell.eccSquared());
+		return;
+	}
+	llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared()));
+	p = cl * llr[2];
+	llh[2] = 0.0;
+	for(int i=0; i<5; i++) {
+		slat = sin(llh[0]);
+		nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
+		htold = llh[2];
+		llh[2] = p / cos(llh[0]) - nn;
+		latold = llh[0];
+		llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
+		if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
+	}
+	llh[0] *= rad2deg;
+}
+
+
+void PIGeoPosition::convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMathVectorT3d &llr, PIEllipsoidModel ell) {
+	double slat = sin(llh[0] * deg2rad);
+	double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
+	llr[1] = llh[1]; // longitude is unchanged
+	llr[2] = sqrt((nn+llh[2])*(nn+llh[2]) + nn*ell.eccSquared()*(nn*ell.eccSquared()-2*(nn+llh[2]))*slat*slat); // radius
+	if(llr[2] <= PIGeoPosition::position_tolerance/5) { // radius is below tolerance, hence assign zero-length
+		llr[0] = llr[1] = llr[2] = 0; // arbitrarily set latitude = longitude = 0
+		return;
+	}
+	if(1 - piAbsd(slat) < 1.e-10) { // at the pole
+		if(slat < 0) llr[0] = -90.0;
+		else llr[0] =  90.0;
+		llr[1] = 0.0;
+		return;
+	}
+	llr[0] = acos((nn * (1.0 - ell.eccSquared()) + llh[2]) * slat / llr[2]); // theta
+	llr[0] *= rad2deg;
+	llr[0] = 90.0 - llr[0];
+}
+
+
+double PIGeoPosition::radiusEarth(double geolat, PIEllipsoidModel ell) {
+	double slat = sin(geolat * deg2rad);
+	double e = (1.0 - ell.eccSquared());
+	double f = (1.0 + (e * e - 1.0) * slat * slat) / (1.0 - ell.eccSquared() * slat * slat);
+	return (ell.a * sqrt(f));
+}
+
+
+PIGeoPosition &PIGeoPosition::operator=(const PIGeoPosition &v) {
+	*((PIMathVectorT3d*)(this)) = *((PIMathVectorT3d*)&v);
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::operator=(const PIMathVectorT3d &v) {
+	*((PIMathVectorT3d*)(this)) = v;
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::operator-=(const PIGeoPosition &right) {
+	PIGeoPosition r(right);
+	CoordinateSystem saves = s;
+	transformTo(Cartesian);
+	r.transformTo(Cartesian);
+	(*(PIMathVectorT3d*)(this)) -= r;
+	transformTo(saves);
+	return *this;
+}
+
+
+PIGeoPosition &PIGeoPosition::operator+=(const PIGeoPosition &right) {
+	PIGeoPosition r(right);
+	CoordinateSystem saves = s;
+	transformTo(Cartesian);
+	r.transformTo(Cartesian);
+	(*(PIMathVectorT3d*)(this)) += r;
+	transformTo(saves);
+	return *this;
+}
+
+
+bool PIGeoPosition::operator==(const PIGeoPosition &right) const {
+	if(el.a != right.el.a || el.eccSquared() != right.el.eccSquared()) return false;
+	if(range(*this, right) < position_tolerance) return true;
+	else return false;
+}
+
+
+void PIGeoPosition::initialize(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem sys, PIEllipsoidModel ell) {
+	double a(v[0]), b(v[1]), c(v[2]);
+	if(sys == Geodetic || sys==Geocentric) {
+		if(a > 90 || a < -90) {
+			piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in constructor:" << a;
+			assert(a <= 90 && a >= -90);
+		}
+		if(b < 0) b += 360*(1+(unsigned long)(b/360));
+		else if(b >= 360) b -= 360*(unsigned long)(b/360);
+	}
+	if(sys==Geocentric || sys==Spherical) {
+		if(c < 0) {
+			piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in constructor:" << c;
+			assert(c >= 0);
+		}
+	}
+	if(sys==Spherical) {
+		if(a < 0 || a > 180) {
+			piCout << "[PIGeoPosition]" << "Achtung! Invalid theta in constructor:" << a;
+			assert(a >= 0 && a <= 180);
+		}
+		if(b < 0) b += 360*(1+(unsigned long)(b/360));
+		else if(b >= 360) b -= 360*(unsigned long)(b/360);
+	}
+	(*this)[0] = a;
+	(*this)[1] = b;
+	(*this)[2] = c;
+	el = ell;
+	s = sys;
+}
+
+
+PIGeoPosition operator+(const PIGeoPosition &left, const PIGeoPosition &right) {
+	PIGeoPosition l(left),r(right);
+	l.transformTo(PIGeoPosition::Cartesian);
+	r.transformTo(PIGeoPosition::Cartesian);
+	l += r;
+	return l;
+}
+
+
+PIGeoPosition operator-(const PIGeoPosition &left, const PIGeoPosition &right) {
+	PIGeoPosition l(left),r(right);
+	l.transformTo(PIGeoPosition::Cartesian);
+	r.transformTo(PIGeoPosition::Cartesian);
+	l -= r;
+	return l;
+}
+
+
+double PIGeoPosition::range(const PIGeoPosition &a, const PIGeoPosition &b) {
+	PIGeoPosition l(a),r(b);
+	l.transformTo(PIGeoPosition::Cartesian);
+	r.transformTo(PIGeoPosition::Cartesian);
+	return (l - r).length();
+}
+
+
+double PIGeoPosition::elevation(const PIGeoPosition &p) const {
+	PIGeoPosition r(*this), s(p);
+	r.transformTo(Cartesian);
+	s.transformTo(Cartesian);
+	return r.angleElevation(s);
+}
+
+
+double PIGeoPosition::elevationGeodetic(const PIGeoPosition &p) const {
+	PIGeoPosition r(*this), s(p);
+	double lat = r.latitudeGeodetic() * deg2rad;
+	double lng = r.longitude() * deg2rad;
+	double local_up;
+	double cos_up;
+	r.transformTo(Cartesian);
+	s.transformTo(Cartesian);
+	PIMathVectorT3d z = s - r;
+	if (z.length() <= 1e-4) { // if the positions are within .1 millimeter
+		piCout << "[PIGeoPosition]" << "Positions are within .1 millimeter" << z;
+		assert(z.length() > 1e-4);
+	}
+	PIMathVectorT3d kv; // Compute k vector in local North-East-Up (NEU) system
+	kv[0] = cos(lat) * cos(lng);
+	kv[1] = cos(lat) * sin(lng);
+	kv[2] = sin(lat);
+	local_up = z.dot(kv); // Take advantage of dot method to get Up coordinate in local NEU system
+	cos_up = local_up / z.length(); // Let's get cos(z), being z the angle with respect to local vertical (Up);
+	return 90.0 - ((acos(cos_up)) * rad2deg);
+}
+
+
+double PIGeoPosition::azimuth(const PIGeoPosition &p) const {
+	PIGeoPosition r(*this), s(p);
+	r.transformTo(Cartesian);
+	s.transformTo(Cartesian);
+	double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
+	double z1, z2, z3, p1, p2, test, alpha;
+	xy = r[0] * r[0] + r[1] * r[1];
+	xyz = xy + r[2] * r[2];
+	xy = sqrt(xy);
+	xyz = sqrt(xyz);
+	if (xy <= 1e-14 || xyz <=1e-14) {
+		piCout << "[PIGeoPosition]" << "Divide by Zero Error";
+		assert(xy > 1e-14 && xyz > 1e-14);
+	}
+	cosl = r[0] / xy;
+	sinl = r[1] / xy;
+	sint = r[2] / xyz;
+	xn1 = -sint * cosl;
+	xn2 = -sint * sinl;
+	xn3 = xy / xyz;
+	xe1 = -sinl;
+	xe2 = cosl;
+	z1 = s[0] - r[0];
+	z2 = s[1] - r[1];
+	z3 = s[2] - r[2];
+	p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3) ;
+	p2 = (xe1 * z1) + (xe2 * z2) ;
+	test = piAbsd(p1) + piAbsd(p2);
+	if (test < 1.0e-16) {
+		piCout << "[PIGeoPosition]" << "azAngle(), failed p1+p2 test" << test;
+		assert(test >= 1.0e-16);
+	}
+	alpha = 90 - atan2(p1, p2) * rad2deg;
+	if (alpha < 0) return alpha + 360;
+	else  return alpha;
+}
+
+
+double PIGeoPosition::azimuthGeodetic(const PIGeoPosition &p) const {
+	PIGeoPosition r(*this), s(p);
+	double lat = r.latitudeGeodetic() * deg2rad;
+	double lng = r.longitude() * deg2rad;
+	r.transformTo(Cartesian);
+	s.transformTo(Cartesian);
+	PIMathVectorT3d z;
+	z = s - r;
+	if (z.length() <= 1e-4) { // if the positions are within .1 millimeter
+		piCout << "[PIGeoPosition]" << "Positions are within 0.1 millimeter" << z.length();
+		assert(z.length() > 1e-4);
+	}
+	PIMathVectorT3d iv; // Compute i vector in local North-East-Up (NEU) system
+	iv[0] = -sin(lat) * cos(lng);
+	iv[1] = -sin(lat) * sin(lng);
+	iv[2] = cos(lat);
+	PIMathVectorT3d jv; // Compute j vector in local North-East-Up (NEU) system
+	jv[0] = -sin(lng);
+	jv[1] = cos(lng);
+	jv[2] = 0.0;
+	double local_n = z.dot(iv) / z.length(); // Now, let's use dot product to get localN unitary vectors
+	double local_e = z.dot(jv) / z.length(); // Now, let's use dot product to get localE unitary vector
+	double test = piAbsd(local_n) + piAbsd(local_e); // Let's test if computing azimuth has any sense
+	if (test < 1.0e-16) return 0.0; // Warning: If elevation is very close to 90 degrees, we will return azimuth = 0.0
+	double alpha = atan2(local_e, local_n) * rad2deg;
+	if (alpha < 0.0) return alpha + 360.0;
+	else return alpha;
+}
+
+double PIGeoPosition::getCurvMeridian() const {
+	double slat = sin(latitudeGeodetic() * deg2rad);
+	double w = 1.0 / sqrt(1.0 - el.eccSquared() * slat * slat);
+	return el.a * (1.0 - el.eccSquared()) * w * w * w;
+}
+
+
+double PIGeoPosition::getCurvPrimeVertical() const {
+	double slat = sin(latitudeGeodetic() * deg2rad);
+	return el.a / sqrt(1.0 - el.eccSquared() * slat * slat);
+}
+
diff --git a/src/geo/pigeoposition.h b/src/geo/pigeoposition.h
new file mode 100644
index 00000000..9810a5d3
--- /dev/null
+++ b/src/geo/pigeoposition.h
@@ -0,0 +1,171 @@
+/*! \file pigeoposition.h
+ * \brief Class for geo position storage and conversions
+*/
+/*
+	PIP - Platform Independent Primitives
+	Class for geo position storage and conversions
+	Copyright (C) 2015 Andrey Bychkov work.a.b@yandex.ru
+
+	This program is free software: you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation, either version 3 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef PIGEOPOSITION_H
+#define PIGEOPOSITION_H
+
+#include "piellipsoidmodel.h"
+#include "pimathvector.h"
+
+class PIGeoPosition : public PIMathVectorT3d
+{
+public:
+
+	enum CoordinateSystem
+	{
+		Unknown=0,  /// Unknown coordinate system
+		Geodetic,   /// Geodetic latitude, longitude, and height above ellipsoid
+		Geocentric, /// Geocentric (regular spherical coordinates)
+		Cartesian,  /// Cartesian (Earth-centered, Earth-fixed)
+		Spherical   /// Spherical coordinates (theta,phi,radius)
+	};
+
+	static const double one_cm_tolerance;/// One centimeter tolerance.
+	static const double one_mm_tolerance;/// One millimeter tolerance.
+	static const double one_um_tolerance;/// One micron tolerance.
+	static double position_tolerance;/// Default tolerance (default 1mm)
+	static double setPositionTolerance(const double tol) {position_tolerance = tol; return position_tolerance;}
+	static double getPositionTolerance() {return position_tolerance;}
+
+	PIGeoPosition();
+	PIGeoPosition(double a, double b, double c, CoordinateSystem s = Cartesian, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+	PIGeoPosition(PIMathVectorT3d v, CoordinateSystem s = Cartesian, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+
+	PIGeoPosition transformTo(CoordinateSystem sys);
+	PIGeoPosition asGeodetic() {transformTo(Geodetic); return *this; } /// Convert to geodetic coordinate
+	PIGeoPosition asGeodetic(const PIEllipsoidModel &ell) {setEllipsoidModel(ell); transformTo(Geodetic); return *this;} /// Convert to another ell, then to geodetic coordinates
+	PIGeoPosition asECEF() {transformTo(Cartesian); return *this; } /// Convert to cartesian coordinates
+
+	double x() const;
+	double y() const;
+	double z() const;
+	double latitudeGeodetic() const;
+	double latitudeGeocentric() const;
+	double longitude() const;
+	double theta() const;
+	double phi() const;
+	double radius() const;
+	double height() const;
+
+	/// Set the ellipsoid values for this PIGeoPosition given a ellipsoid.
+	void setEllipsoidModel(const PIEllipsoidModel &ell) {el = ell;}
+
+	/// Set the \a PIGeoPosition given geodetic coordinates. \a CoordinateSystem is set to \a Geodetic.
+	PIGeoPosition &setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	/// Set the \a PIGeoPosition given geocentric coordinates. \a CoordinateSystem is set to \a Geocentric
+	PIGeoPosition &setGeocentric(double lat, double lon, double rad);
+
+	/// Set the \a PIGeoPosition given spherical coordinates. \a CoordinateSystem is set to \a Spherical
+	PIGeoPosition &setSpherical(double theta, double phi, double rad);
+
+	/// Set the \a PIGeoPosition given ECEF coordinates. \a CoordinateSystem is set to \a Cartesian.
+	PIGeoPosition &setECEF(double x, double y, double z);
+
+	/// Fundamental conversion from spherical to cartesian coordinates.
+	static void convertSphericalToCartesian(const PIMathVectorT3d &tpr, PIMathVectorT3d &xyz);
+
+	/// Fundamental routine to convert cartesian to spherical coordinates.
+	static void convertCartesianToSpherical(const PIMathVectorT3d &xyz, PIMathVectorT3d &tpr);
+
+	/// Fundamental routine to convert ECEF (cartesian) to geodetic coordinates,
+	static void convertCartesianToGeodetic(const PIMathVectorT3d &xyz, PIMathVectorT3d &llh, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	/// Fundamental routine to convert geodetic to ECEF (cartesian) coordinates,
+	static void convertGeodeticToCartesian(const PIMathVectorT3d &llh, PIMathVectorT3d &xyz, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	/// Fundamental routine to convert cartesian (ECEF) to geocentric
+	static void convertCartesianToGeocentric(const PIMathVectorT3d &xyz, PIMathVectorT3d &llr);
+
+	/// Fundamental routine to convert geocentric to cartesian (ECEF)
+	static void convertGeocentricToCartesian(const PIMathVectorT3d &llr, PIMathVectorT3d &xyz);
+
+	/// Fundamental routine to convert geocentric to geodetic
+	static void convertGeocentricToGeodetic(const PIMathVectorT3d &llr, PIMathVectorT3d &llh, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	/// Fundamental routine to convert geodetic to geocentric
+	static void convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMathVectorT3d &llr, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	/// Compute the radius of the ellipsoidal Earth, given the geodetic latitude.
+	static double radiusEarth(double geolat, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+	double radiusEarth() const {
+		PIGeoPosition p(*this);
+		p.transformTo(PIGeoPosition::Geodetic);
+		return PIGeoPosition::radiusEarth((*this)[0], p.el);
+	}
+
+	/// Compute the range in meters between two PIGeoPositions.
+	static double range(const PIGeoPosition &a, const PIGeoPosition &b);
+	double range(const PIGeoPosition &p) const {
+		return range((*this), p);
+	}
+
+	/// Computes the elevation of the input (p) position as seen from this PIGeoPosition.
+	double elevation(const PIGeoPosition &p) const;
+
+	/// Computes the elevation of the input (p) position as seen from this PIGeoPosition, using a Geodetic (ellipsoidal) system.
+	double elevationGeodetic(const PIGeoPosition &p) const;
+
+	/// Computes the azimuth of the input (p) position as seen from this PIGeoPosition.
+	double azimuth(const PIGeoPosition &p) const;
+
+	/// Computes the azimuth of the input (p) position as seen from this PIGeoPosition, using a Geodetic (ellipsoidal) system.
+	double azimuthGeodetic(const PIGeoPosition &p) const;
+
+	/// Computes the radius of curvature of the meridian (Rm) corresponding to this PIGeoPosition.
+	double getCurvMeridian() const;
+
+	/// Computes the radius of curvature in the prime vertical (Rn) corresponding to this PIGeoPosition.
+	double getCurvPrimeVertical() const;
+
+	PIGeoPosition &operator=(const PIGeoPosition & v);
+	PIGeoPosition &operator=(const PIMathVectorT3d & v);
+	PIGeoPosition &operator-=(const PIGeoPosition &right);
+	PIGeoPosition &operator+=(const PIGeoPosition &right);
+	friend PIGeoPosition operator-(const PIGeoPosition &left, const PIGeoPosition &right);
+	friend PIGeoPosition operator+(const PIGeoPosition &left, const PIGeoPosition &right);
+	friend PIGeoPosition operator*(const double &scale, const PIGeoPosition &right);
+	friend PIGeoPosition operator*(const PIGeoPosition &left, const double &scale);
+	friend PIGeoPosition operator*(const int &scale, const PIGeoPosition &right);
+	friend PIGeoPosition operator*(const PIGeoPosition &left, const int &scale);
+	bool operator==(const PIGeoPosition &right) const;
+	bool operator!=(const PIGeoPosition &right) const {return !(operator==(right));}
+
+
+private:
+	void initialize(PIMathVectorT3d v, CoordinateSystem sys = Cartesian, PIEllipsoidModel ell = PIEllipsoidModel::WGS84Ellipsoid());
+
+	PIEllipsoidModel el;
+	CoordinateSystem s;
+
+};
+
+
+PIGeoPosition operator-(const PIGeoPosition &left, const PIGeoPosition &right);
+PIGeoPosition operator+(const PIGeoPosition &left, const PIGeoPosition &right);
+PIGeoPosition operator*(const double &scale, const PIGeoPosition &right) {PIMathVectorT3d tmp(right); tmp *= scale; return PIGeoPosition(tmp);}
+PIGeoPosition operator*(const PIGeoPosition &left, const double &scale) {return operator* (scale, left);}
+PIGeoPosition operator*(const int &scale, const PIGeoPosition &right) {return operator* (double(scale), right);}
+PIGeoPosition operator*(const PIGeoPosition &left, const int &scale) {return operator* (double(scale), left);}
+
+#endif // PIGEOPOSITION_H
diff --git a/src/math/pimathbase.h b/src/math/pimathbase.h
index 290ddb87..96d88a66 100644
--- a/src/math/pimathbase.h
+++ b/src/math/pimathbase.h
@@ -62,10 +62,10 @@
 #  define M_1_SQRT3 0.57735026918962584208
 #endif
 #ifndef M_PI
-#  define M_PI 3.14159265358979323846
+#  define M_PI 3.141592653589793238462643383280
 #endif
 #ifndef M_2PI
-#  define M_2PI 6.28318530717958647692
+#  define M_2PI 6.283185307179586476925286766559
 #endif
 #ifndef M_PI_3
 #  define M_PI_3 1.04719755119659774615
@@ -79,12 +79,21 @@
 #ifndef M_PI_180
 #  define M_PI_180 1.74532925199432957692e-2
 #endif
+#ifndef M_SQRT_PI
+#  define M_SQRT_PI 1.772453850905516027298167483341
+#endif
 #ifndef M_E
 #  define M_E 2.7182818284590452353602874713527
 #endif
 #ifndef M_LIGHT_SPEED
 #  define M_LIGHT_SPEED 2.99792458e+8
 #endif
+#ifndef M_RELATIVE_CONST
+#  define M_RELATIVE_CONST -4.442807633e-10;
+#endif
+#ifndef M_GRAVITY_CONST
+#  define M_GRAVITY_CONST 398600.4418e9;
+#endif
 
 using std::complex;
 
diff --git a/src/math/pimathvector.h b/src/math/pimathvector.h
index d8374ff2..3841b7bf 100644
--- a/src/math/pimathvector.h
+++ b/src/math/pimathvector.h
@@ -58,9 +58,12 @@ public:
 	Type angleSin(const _CVector & v) const {Type tv = angleCos(v); return sqrt(Type(1) - tv * tv);}
 	Type angleRad(const _CVector & v) const {return acos(angleCos(v));}
 	Type angleDeg(const _CVector & v) const {return toDeg(acos(angleCos(v)));}
+	Type angleElevation(const _CVector & v) const {_CVector z = v - *this; double c = z.angleCos(*this); return 90.0 - acos(c) * rad2deg;}
 	_CVector projection(const _CVector & v) {Type tv = v.length(); return (tv == Type(0) ? _CVector() : v * (((*this) ^ v) / tv));}
 	_CVector & normalize() {Type tv = length(); if (tv == Type(1)) return *this; if (piAbs<Type>(tv) <= Type(1E-100)) {fill(Type(0)); return *this;} PIMV_FOR(i, 0) c[i] /= tv; return *this;}
 	_CVector normalized() {_CVector tv(*this); tv.normalize(); return tv;}
+	_CVector cross(const _CVector & v) {return (*this) * v;}
+	Type dot(const _CVector & v) const {return (*this) ^ v;}
 	bool isNull() const {PIMV_FOR(i, 0) if (c[i] != Type(0)) return false; return true;}
 	bool isOrtho(const _CVector & v) const {return ((*this) ^ v) == Type(0);}