[OC] Add library OpenCoordinate

Conversion latitude/longitude GPS to relative x and y.

owlps-ardrone/oc.c

@@ -0,0 +1,85 @@
+// Bibliotheque OpenCoordinate : fonctions coordonnees geographiques
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#include <stdio.h>
+#include <math.h>
+
+struct point {
+  double lat, lon, alt;
+} point;
+
+struct point_relatif {
+  double x, y, z;
+} point_relatif;
+
+double oc_distance_between(struct point a, struct point b) {
+  // retourne la distance entre les 2 points en metres
+  double R = 6378137; // metres
+  double d_lat = (b.lat-a.lat)/180*M_PI;
+  double d_lon = (b.lon-a.lon)/180*M_PI;
+  double alpha = sin(d_lat/2) * sin(d_lat/2) +
+    cos(a.lat/180*M_PI) * cos(b.lat/180*M_PI) *
+    sin(d_lon/2) * sin(d_lon/2);
+  double c = 2 * atan2(sqrt(alpha), sqrt(1-alpha));
+  return R * c;
+}
+
+struct point_relatif oc_convert(struct point o, struct point a) {
+  //convert transforme des coordonnes WGS84 en metres par rapport au referentiel r
+  struct point proj_x, proj_y;
+  struct point_relatif pr;
+  proj_x.lat=a.lat;
+  proj_x.lon=o.lon;
+  proj_y.lat=o.lat;
+  proj_y.lon=a.lon;
+  pr.x=oc_distance_between(proj_y,o);
+  pr.y=oc_distance_between(proj_x,o);
+  pr.z=0;
+  if (proj_y.lon > o.lon) pr.x=-pr.x;
+  if (proj_x.lat > o.lat) pr.y=-pr.y;
+  return pr;
+}
+
+/*
+  function unconvert(x,y) {
+  if (origine==null) return null;
+  else {
+  var R = 6378137; // metres
+  var lat1 = origine.lat()/180*Math.PI;
+  var lon1 = origine.lng()/180*Math.PI;
+  var d = Math.sqrt(x*x + y*y);
+  var brng = Math.acos( y / d);
+  var lat2 = Math.asin( Math.sin(lat1)*Math.cos(d/R) +
+  Math.cos(lat1)*Math.sin(d/R)*Math.cos(brng) );
+  var lon2 = lon1 +
+  Math.atan2(Math.sin(brng)*Math.sin(d/R)*Math.cos(lat1),
+  Math.cos(d/R)-Math.sin(lat1)*Math.sin(lat2));
+  return new google.maps.LatLng(lat2*180/Math.PI, lon2*180/Math.PI);
+  }
+  }
+*/
+
+double oc_cap(struct point o, struct point a) {
+  struct point_relatif pr=oc_convert(a, o);
+  double alpha;
+  printf("cap x=%f et y=%f\n",pr.x, pr.y);
+  if (pr.x==0)
+    if (pr.y>=0) return 360.0;
+    else return 180.0;
+  else {
+    alpha=atan(pr.y/pr.x) * 180 / M_PI;
+    printf("%f\n",alpha);
+    if (pr.x>0)
+      return ((450 - alpha)/360 - round((450 - alpha)/360))*360;
+    else {
+      //alpha=atan(-pr.y/pr.x) * 180 / PI;
+      //printf("%f\n",alpha);
+      return 270-alpha;
+    }
+  }
+}
+
+