Function.h

00001 #define M_PI 3.141592
00002 
00003 double deg2rad(double deg) {
00004   return deg * (M_PI/180.0);
00005 }
00006 
00007 
00008 double getDistance(double lat1,double lon1,double lat2,double lon2) {
00009   double R = 6371; // Radius of the earth in km
00010   double dLat = deg2rad(lat2-lat1);  // deg2rad below
00011   double dLon = deg2rad(lon2-lon1); 
00012   double a = 
00013     sin(dLat/2) * sin(dLat/2) +
00014     cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * 
00015     sin(dLon/2) * sin(dLon/2)
00016     ; 
00017   double c = 2 * atan2(sqrt(a), sqrt(1-a)); 
00018   double d = R * c; // Distance in km
00019   d = d * 1000;     // Distance in meter
00020   return d;
00021 }
00022 
00023 double MIN_MAX(double input, double min, double max){
00024     double output;
00025     if( input < min) output = min;
00026     else if(input > max) output = max;
00027     else output = input;
00028     
00029     return output;       
00030 }
00031 
00032 
00033 float bearing(float lat1,float lon1,float lat2,float lon2){
00034 
00035     float teta1 = (lat1)*M_PI/180.0;
00036     float teta2 = (lat2)*M_PI/180.0;;
00037 //    float delta1 = (lat2-lat1)*M_PI/180.0;;
00038     float delta2 = (lon2-lon1)*M_PI/180.0;;
00039 
00040     //==================Heading Formula Calculation================//
00041 
00042     float y = sin(delta2) * cos(teta2);
00043     float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
00044     float brng = atan2(y,x);
00045     brng = (brng)*180.0/M_PI;// radians to degrees
00046     brng = ( ((int)brng + 360) % 360 ); 
00047 //    if(brng > 180) brng -= 360;
00048     if(brng > 360) brng -= 360;
00049     else if(brng < 0) brng += 360;
00050     return brng;
00051 }
00052 
00053 float ABSOL(float input){
00054     float output;
00055     if( input > 0){
00056         output = input;
00057     }else{
00058         output = -input;
00059     }
00060     return output;
00061 }