Mapping.cpp

00001 #include "mbed.h" // debugging
00002 #include "Mapping.h"
00003 
00004 /*
00005  * Provide a series of lat/lon coordinates/waypoints, and the object
00006  * automatically generates a bounding rectangle, and determines how to
00007  * translate between lat/lon and cartesian x/y
00008  */
00009 void Mapping::init(int count, GeoPosition *p)
00010 {
00011     double latMin = 360;
00012     double latMax = -360;
00013     double lonMin = 360;
00014     double lonMax = -360;
00015     
00016     // Find minimum and maximum lat/lon
00017     for (int i=0; i < count; i++, p++) {
00018         //fprintf(stdout, "%d (%.6f, %.6f)\n", i, p->latitude(), p->longitude());
00019         if (p->latitude() > latMax)
00020             latMax = p->latitude();
00021         if (p->latitude() < latMin)
00022             latMin = p->latitude();
00023             
00024         if (p->longitude() > lonMax)
00025             lonMax = p->longitude();
00026         if (p->longitude() < lonMin)
00027             lonMin = p->longitude();
00028     }
00029 
00030     // Set the arbitrary cartesian origin to southwest corner
00031     lonZero = lonMin;
00032     latZero = latMin;
00033 
00034     //fprintf(stdout, "min: (%.6f, %.6f)\n", latMin, lonMin);
00035     //fprintf(stdout, "max: (%.6f, %.6f)\n", latMax, lonMax);
00036 
00037     // Three positions required to scale
00038     GeoPosition sw(latMin, lonMin);
00039     GeoPosition nw(latMax, lonMin);
00040     GeoPosition se(latMin, lonMax);
00041     
00042     // Find difference between lat and lon min/max
00043     float dlat = (latMax - latMin);
00044     float dlon = (lonMax - lonMin);
00045     
00046     //fprintf(stdout, "dlat=%.6f dlon=%.6f\n", dlat, dlon);
00047     
00048     // Compute scale based on distances between edges of rectangle
00049     // and difference between min/max lat and min/max lon
00050     lonToX = sw.distanceTo(se) / dlon;
00051     latToY = sw.distanceTo(nw) / dlat;
00052 
00053     fprintf(stdout, "lonToX=%.10f\nlatToY=%.10f\n", lonToX, latToY);
00054 
00055     return;    
00056 }
00057 
00058 void Mapping::geoToCart(GeoPosition pos, CartPosition *cart)
00059 {
00060     cart->set( lonToX * (pos.longitude() - lonZero),
00061                latToY * (pos.latitude() - latZero) );
00062 
00063     return;
00064 }
00065 
00066 void Mapping::cartToGeo(float x, float y, GeoPosition *pos)
00067 {
00068     pos->set( (y / latToY) + latZero, 
00069               (x / lonToX) + lonZero);
00070 
00071     return; 
00072 }
00073 
00074 void Mapping::cartToGeo(CartPosition cart, GeoPosition *pos)
00075 {
00076     cartToGeo(cart._x, cart._y, pos);
00077     
00078     return;
00079 }
00080