GeoPosition.cpp

00001 #include "GeoPosition.h"
00002 #include <math.h>
00003 
00004 // Earth's mean radius in meters
00005 #define EARTHRADIUS 6371000.0
00006 // TODO: 2 altitude
00007 
00008 GeoPosition::GeoPosition(): _R(EARTHRADIUS), _latitude(0.0), _longitude(0.0)
00009 {
00010 }
00011 
00012 GeoPosition::GeoPosition(double latitude, double longitude): _R(EARTHRADIUS), _latitude(latitude), _longitude(longitude)
00013 {
00014 }
00015 
00016 /*
00017 double GeoPosition::easting()
00018 {
00019 }
00020 
00021 double GeoPosition::northing()
00022 {
00023 }
00024 */
00025 
00026 double GeoPosition::latitude()
00027 {
00028     return _latitude;
00029 }
00030 
00031 double GeoPosition::longitude()
00032 {
00033     return _longitude;
00034 }
00035 
00036 void GeoPosition::set(double latitude, double longitude)
00037 {
00038     _latitude = latitude;
00039     _longitude = longitude;
00040 }
00041 
00042 void GeoPosition::set(GeoPosition pos)
00043 {
00044     _latitude = pos.latitude();
00045     _longitude = pos.longitude();
00046 }
00047 
00048 /*
00049 void GeoPosition::set(UTM coord)
00050 {
00051 }
00052 */
00053 
00054 void GeoPosition::move(float course, float distance)
00055 {
00056     double d = distance / _R;
00057     double c = radians(course);
00058     double rlat1 = radians(_latitude);
00059     double rlon1 = radians(_longitude);
00060 
00061     // Precompute to improve performance
00062     double cd = cos(d);
00063     double sd = sin(d);
00064     double srlat1 = sin(rlat1);
00065     double crlat1 = cos(rlat1);
00066 
00067     double rlat2 = asin(srlat1*cd + crlat1*sd*cos(c));
00068     double rlon2 = rlon1 + atan2(sin(c)*sd*crlat1, cd-srlat1*sin(rlat2));
00069 
00070     _latitude  = degrees(rlat2);
00071     _longitude = degrees(rlon2);
00072 
00073     // bring back within the range -180 to +180
00074     while (_longitude < -180.0) _longitude += 360.0;
00075     while (_longitude > 180.0) _longitude -= 360.0;
00076 }
00077 
00078 /*
00079 void GeoPosition::move(Direction toWhere)
00080 {
00081 }
00082 
00083 Direction GeoPosition::direction(GeoPosition startingPoint)
00084 {
00085 }
00086 */
00087 
00088 float GeoPosition::bearing(GeoPosition from)
00089 {
00090     return bearingFrom(from);
00091 }
00092 
00093 float GeoPosition::bearingTo(GeoPosition to)
00094 {
00095     double lat1 = radians(_latitude);
00096     double lon1 = radians(_longitude);
00097     double lat2 = radians(to.latitude());
00098     double lon2 = radians(to.longitude());
00099     double dLon = lon2 - lon1;
00100 
00101     double y = sin(dLon) * cos(lat2);
00102     double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
00103 
00104     return degrees(atan2(y, x)); 
00105 }
00106 
00107 /*
00108 JavaScript:    
00109 var y = Math.sin(dLon) * Math.cos(lat2);
00110 var x = Math.cos(lat1)*Math.sin(lat2) -
00111         Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
00112 var brng = Math.atan2(y, x).toDeg();
00113 */
00114 float GeoPosition::bearingFrom(GeoPosition from)
00115 {
00116     double lat1 = radians(from.latitude());
00117     double lon1 = radians(from.longitude());
00118     double lat2 = radians(_latitude);
00119     double lon2 = radians(_longitude);
00120     double dLon = lon2 - lon1;
00121 
00122     double y = sin(dLon) * cos(lat2);
00123     double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
00124 
00125     return degrees(atan2(y, x)); 
00126 }
00127 
00128 /*
00129 float GeoPosition::bearing(LatLong startingPoint)
00130 {
00131 }
00132 
00133 float GeoPosition::bearing(UTM startingPoint)
00134 {
00135 }
00136 */
00137 
00138 float GeoPosition::distance(GeoPosition from)
00139 {
00140     return distanceFrom(from);
00141 }
00142 
00143 float GeoPosition::distanceTo(GeoPosition to)
00144 {
00145     double lat1 = radians(_latitude);
00146     double lon1 = radians(_longitude);
00147     double lat2 = radians(to.latitude());
00148     double lon2 = radians(to.longitude());
00149     double dLat = lat2 - lat1;
00150     double dLon = lon2 - lon1;
00151 
00152     double a = sin(dLat/2.0) * sin(dLat/2.0) + 
00153                cos(lat1) * cos(lat2) *
00154                sin(dLon/2.0) * sin(dLon/2.0);
00155     double c = 2.0 * atan2(sqrt(a), sqrt(1-a));
00156     
00157     return _R * c;
00158 }
00159 
00160 float GeoPosition::distanceFrom(GeoPosition from)
00161 {
00162     double lat1 = radians(from.latitude());
00163     double lon1 = radians(from.longitude());
00164     double lat2 = radians(_latitude);
00165     double lon2 = radians(_longitude);
00166     double dLat = lat2 - lat1;
00167     double dLon = lon2 - lon1;
00168 
00169     double a = sin(dLat/2.0) * sin(dLat/2.0) + 
00170                cos(lat1) * cos(lat2) *
00171                sin(dLon/2.0) * sin(dLon/2.0);
00172     double c = 2.0 * atan2(sqrt(a), sqrt(1-a));
00173     
00174     return _R * c;
00175 }
00176 
00177 /*
00178 float GeoPosition::distance(LatLong startingPoint)
00179 {
00180 }
00181 
00182 float GeoPosition::distance(UTM startingPoint)
00183 {
00184 }
00185 */
00186 
00187 void GeoPosition::setTimestamp(int time) {
00188     _time = time;
00189 }
00190 
00191 int GeoPosition::getTimestamp(void) {
00192     return _time;
00193 }
00194 
00195 
00196