moved angle calc to util

brouter-util/src/main/java/btools/util/CheapAngleMeter.java

@@ -0,0 +1,55 @@
+/**
+ * Calculate the angle defined by 3 points
+ * (and deliver it's cosine on the fly)
+ */
+package btools.util;
+
+public final class CheapAngleMeter
+{
+  private double cosangle;
+
+  public double getCosAngle()
+  {
+    return cosangle;
+  }
+
+  public double calcAngle( int lon0, int lat0,  int lon1, int lat1, int lon2, int lat2 )
+  {
+    double[] lonlat2m = CheapRuler.getLonLatToMeterScales( lat1 );
+    double lon2m = lonlat2m[0];
+    double lat2m = lonlat2m[1];
+    double dx10 = (lon1 - lon0) * lon2m;
+    double dy10 = (lat1 - lat0) * lat2m;
+    double dx21 = (lon2 - lon1) * lon2m;
+    double dy21 = (lat2 - lat1) * lat2m;
+
+    double dd = Math.sqrt( (dx10*dx10 + dy10*dy10)*(dx21*dx21 + dy21*dy21) );
+    if ( dd == 0. ) { cosangle = 1.; return 0.; }
+    double sinp = (dy10*dx21 - dx10*dy21)/dd;
+    double cosp = (dy10*dy21 + dx10*dx21)/dd;
+    cosangle = cosp;
+
+    double offset = 0.;
+    double s2 = sinp*sinp;
+    if ( s2 > 0.5 )
+    {
+      if ( sinp > 0. )
+      {
+        offset = 90.;
+        sinp = -cosp;
+      }
+      else
+      {
+        offset = -90.;
+        sinp = cosp;
+      }
+      s2 = cosp*cosp;
+    }
+    else if ( cosp < 0. )
+    {
+      sinp = -sinp;
+      offset = sinp > 0. ? -180. : 180.;
+    }
+    return offset + sinp * ( 57.4539 + s2 * ( 9.57565 + s2 * ( 4.30904 + s2 * 2.56491 ) ) );
+  }
+}