Diff: ROBOFRIEN_SUB_FUNC/Function.h

Revision:

1:9530746906b6

diff -r 3473b92e991e -r 9530746906b6 ROBOFRIEN_SUB_FUNC/Function.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/Function.h	Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,61 @@
+#define M_PI 3.141592
+
+double deg2rad(double deg) {
+  return deg * (M_PI/180.0);
+}
+
+
+double getDistance(double lat1,double lon1,double lat2,double lon2) {
+  double R = 6371; // Radius of the earth in km
+  double dLat = deg2rad(lat2-lat1);  // deg2rad below
+  double dLon = deg2rad(lon2-lon1); 
+  double a = 
+    sin(dLat/2) * sin(dLat/2) +
+    cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * 
+    sin(dLon/2) * sin(dLon/2)
+    ; 
+  double c = 2 * atan2(sqrt(a), sqrt(1-a)); 
+  double d = R * c; // Distance in km
+  d = d * 1000;     // Distance in meter
+  return d;
+}
+
+double MIN_MAX(double input, double min, double max){
+    double output;
+    if( input < min) output = min;
+    else if(input > max) output = max;
+    else output = input;
+    
+    return output;       
+}
+
+
+float bearing(float lat1,float lon1,float lat2,float lon2){
+
+    float teta1 = (lat1)*M_PI/180.0;
+    float teta2 = (lat2)*M_PI/180.0;;
+//    float delta1 = (lat2-lat1)*M_PI/180.0;;
+    float delta2 = (lon2-lon1)*M_PI/180.0;;
+
+    //==================Heading Formula Calculation================//
+
+    float y = sin(delta2) * cos(teta2);
+    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
+    float brng = atan2(y,x);
+    brng = (brng)*180.0/M_PI;// radians to degrees
+    brng = ( ((int)brng + 360) % 360 ); 
+//    if(brng > 180) brng -= 360;
+    if(brng > 360) brng -= 360;
+    else if(brng < 0) brng += 360;
+    return brng;
+}
+
+float ABSOL(float input){
+    float output;
+    if( input > 0){
+        output = input;
+    }else{
+        output = -input;
+    }
+    return output;
+}