Diff: Get.cpp

Revision:

34:610d315c1bab

Parent:

32:263d39ea6d3e

Child:

35:009cc4509a90

--- a/Get.cpp	Thu Oct 01 15:24:11 2015 +0000
+++ b/Get.cpp	Fri Oct 02 21:09:02 2015 +0000
@@ -62,6 +62,17 @@
     }
 }
 
+double getDistance_2dest(int task_id_start, int task_id_end) {
+    double dest_Latitude_start = Deg2Rad(Latitude_Path[task_id_start-1]);
+    double dest_Longitude_start = Longitude_Path[task_id_start-1];
+    double dest_Latitude_end = Deg2Rad(Latitude_Path[task_id_end-1]);
+    double dest_Longitude_end = Longitude_Path[task_id_end-1];
+    if(Latitude_Path[task_id_start-1] == 181 or Longitude_Path[task_id_start-1] == 181 or Latitude_Path[task_id_end-1] == 181 or Longitude_Path[task_id_end-1] == 181) {
+        return -1;
+    } else {
+        return acos(sin(dest_Latitude_start)*sin(dest_Latitude_end)+cos(dest_Latitude_start)*cos(dest_Latitude_end)*cos(Deg2Rad(dest_Longitude_end-dest_Longitude_start)))*EARTH_RADIUS;
+    }
+}
 
 double getAngle(int task_id) {
     double cur_Latitude = Deg2Rad(D_GPS_Latitude);
@@ -80,3 +91,16 @@
         return out;
     }
 }
+
+double getCTE(int task_id) {
+    double dis_start = getDistance(task_id);
+    double dis_end = getDistance(task_id+1);
+    double dis_between = getDistance_2dest(task_id,task_id+1);
+    if (dis_start == -1 or dis_end == -1 or dis_between == -1) {
+        return -1;
+    } else {
+        double p = (dis_start+dis_end+dis_between)/2;
+        double area = sqrt(p*(p-dis_start)*(p-dis_end)*(p-dis_between));
+        return 2*area/dis_between;
+    }
+}