Diff: verwerking/rhumb.cpp

Revision:

0:bec310bde899

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/verwerking/rhumb.cpp	Tue Sep 27 19:46:30 2011 +0000
@@ -0,0 +1,54 @@
+#include "rhumb.h"
+#include <math.h>
+
+
+double afstand(pos *cur, pos *tar){
+    double lat_cur_r = cur->lat * deg2rad;
+    double lon_cur_r = cur->lon * deg2rad;
+    double lat_tar_r = tar->lat * deg2rad;
+    double lon_tar_r = tar->lon * deg2rad;
+    
+    double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0));
+
+
+    double dla = lat_tar_r - lat_cur_r;
+    double dlo = lon_tar_r - lon_cur_r;
+
+    double q;
+    
+    if (df == 0)
+        q = cos(lat_cur_r);
+    else
+        q = dla / df;
+    
+        
+    double r = 6371.0;
+    
+
+    double d = sqrt(pow(dla,2) + pow(q,2) * pow(dlo,2)) * r;
+    return d;
+
+
+}
+
+double koers(pos *cur, pos *tar){
+    double lat_cur_r = cur->lat * deg2rad;
+    double lon_cur_r = cur->lon * deg2rad;
+    double lat_tar_r = tar->lat * deg2rad;
+    double lon_tar_r = tar->lon * deg2rad;
+    
+    double df = log(tan(lat_tar_r/2.0 + PI/4.0)/tan(lat_cur_r/2.0 + PI/4.0));
+
+
+    double dla = lat_tar_r - lat_cur_r;
+    double dlo = lon_tar_r - lon_cur_r;
+
+    
+    double k = atan2(dlo,df) * rad2deg;
+    if (k < 0)
+        k += 360;
+
+    return k;
+
+
+}