MBED Example: How to use geographical position and calculation using latitude and longitude

.from: http://mbed.org/users/shimniok/libraries/GeoPosition/ly4w47

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx GeoPosition.cpp XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx

#include "GeoPosition.h"
#include <math.h>

// Earth's mean radius in meters
#define EARTHRADIUS 6371000.0

GeoPosition::GeoPosition(): _R(EARTHRADIUS), _latitude(0.0), _longitude(0.0)
{
}

GeoPosition::GeoPosition(double latitude, double longitude): _R(EARTHRADIUS), _latitude(latitude), _longitude(longitude)
{
}
   
/*
double GeoPosition::easting()
{
}

double GeoPosition::northing()
{
}
*/

double GeoPosition::latitude()
{
    return _latitude;
}

double GeoPosition::longitude()
{
    return _longitude;
}

void GeoPosition::set(double latitude, double longitude)
{
    _latitude = latitude;
    _longitude = longitude;
}

void GeoPosition::set(GeoPosition pos)
{
    _latitude = pos.latitude();
    _longitude = pos.longitude();
}

/*
void GeoPosition::set(UTM coord)
{
}
*/

void GeoPosition::move(float course, float distance)
{
    double d = distance / _R;
    double c = radians(course);
    double rlat1 = radians(_latitude);
    double rlon1 = radians(_longitude);

    double rlat2 = asin(sin(rlat1)*cos(d) + cos(rlat1)*sin(d)*cos(c));
    double rlon2 = rlon1 + atan2(sin(c)*sin(d)*cos(rlat1), cos(d)-sin(rlat1)*sin(rlat2));

    _latitude  = degrees(rlat2);
    _longitude = degrees(rlon2);

    // bring back within the range -180 to +180
    while (_longitude < -180.0) _longitude += 360.0;
    while (_longitude > 180.0) _longitude -= 360.0;
}

/*
void GeoPosition::move(Direction toWhere)
{
}

Direction GeoPosition::direction(GeoPosition startingPoint)
{
}
*/

float GeoPosition::bearing(GeoPosition from)
{
    double lat1 = radians(from.latitude());
    double lon1 = radians(from.longitude());
    double lat2 = radians(_latitude);
    double lon2 = radians(_longitude);
    double dLon = lon2 - lon1;

    double y = sin(dLon) * cos(lat2);
    double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);

    return degrees(atan2(y, x));
}

/*
float GeoPosition::bearing(LatLong startingPoint)
{
}

float GeoPosition::bearing(UTM startingPoint)
{
}
*/

float GeoPosition::distance(GeoPosition from)
{
    double lat1 = radians(from.latitude());
    double lon1 = radians(from.longitude());
    double lat2 = radians(_latitude);
    double lon2 = radians(_longitude);
    double dLat = lat2 - lat1;
    double dLon = lon2 - lon1;

    double a = sin(dLat/2.0) * sin(dLat/2.0) +
               cos(lat1) * cos(lat2) *
               sin(dLon/2.0) * sin(dLon/2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1-a));
   
    return _R * c;
}

/*
float GeoPosition::distance(LatLong startingPoint)
{
}

float GeoPosition::distance(UTM startingPoint)
{
}
*/

void GeoPosition::setTimestamp(int time) {
    _time = time;
}

int GeoPosition::getTimestamp(void) {
    return _time;
}


   

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx GeoPosition.h XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx

#ifndef __GEOPOSITION_H
#define __GEOPOSITION_H

#ifndef _PI
#define _PI 3.141592653
#endif

#define degrees(x) ((x)*180/_PI)
#define radians(x) ((x)*_PI/180)

/** Geographical position and calculation. Most of this comes from http://www.movable-type.co.uk/scripts/latlong.html
 *
 */
class GeoPosition {
public:

    /** Create a new emtpy position object
     *
     */
    GeoPosition();

    /** Create a new position with the specified latitude and longitude. See set()
     *
     *  @param latitude is the latitude to set
     *  @param longitude is the longitude to set
     */
    GeoPosition(double latitude, double longitude);
   
    /** Get the position's latitude
     *
     *  @returns the position's latitude
     */
    double latitude();
   
    /** Get the position's longitude
     *
     *  @returns the position's longitude
     */
    double longitude();
   
    /** Set the position's location to another position's coordinates
     *
     *  @param pos is another position from which coordinates will be copied
     */
    void set(GeoPosition pos);
   
    /** Set the position's location to the specified coordinates
     *
     *  @param latitude is the new latitude to set
     *  @param longitude is the new longitude to set
     */
    void set(double latitude, double longitude);
   
    /** Move the location of the position by the specified distance and in
     *  the specified direction
     *
     *  @param course is the direction of movement in degrees, absolute not relative
     *  @param distance is the distance of movement along the specified course in meters
     */
    void move(float course, float distance);

    /** Get the bearing from the specified origin position to this position.  To get
     *  relative bearing, subtract the result from your heading.
     *
     *  @param from is the position from which to calculate bearing
     *  @returns the bearing in degrees
     */
    float bearing(GeoPosition from);
   
    /** Get the distance from the specified origin position to this position
     *
     *  @param from is the position from which to calculate distance
     *  @returns the distance in meters
     */
    float distance(GeoPosition from);
   
    /** Set an arbitrary timestamp on the position
     *
     * @param timestamp is an integer timestamp, eg., seconds, milliseconds, or whatever
     */
     void setTimestamp(int time);

    /** Return a previously set timestamp on the position
     *
     * @returns the timestamp (e.g., seconds, milliseconds, etc.)
     */    
     int getTimestamp(void);

private:
    double _R;          /** Earth's mean radius */
    double _latitude;   /** The position's latitude */
    double _longitude;  /** The position's longitude */
    double _northing;   /** The position's UTM northing coordinate */
    double _easting;    /** The position's UTM easting coordinate */
    int _time;          /** Timestamp */
};

#endif

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx EOF XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx