GeometryUtils: add vector utils (#467)

+ calc bisections
+ sum, diff
+ distance
+ dot product
+ intersections (line and plane)
+ normalizations
+ normal of plane
+ scale

vtm/src/org/oscim/utils/geom/GeometryUtils.java

@@ -1,5 +1,6 @@
 /*
  * Copyright 2012, 2013 Hannes Janetzek
+ * Copyright 2018 Gustl22
  *
  * This file is part of the OpenScienceMap project (http://www.opensciencemap.org).
  *
@@ -16,6 +17,9 @@
  */
 package org.oscim.utils.geom;
 
+import java.util.ArrayList;
+import java.util.List;
+
 public final class GeometryUtils {
 
     private GeometryUtils() {
@@ -77,6 +81,57 @@ public final class GeometryUtils {
         return (area < 0 ? -area : area) * 0.5f;
     }
 
+    /**
+     * @param v1 the first normalized vector
+     * @param v2 the second normalized vector
+     * @return the bisection of the to vectors
+     */
+    public static float[] bisectionNorm2D(float[] v1, float[] v2) {
+        // Normalize vectors
+        /*float absBA = (float) Math.sqrt(v1[0] * v1[0] + v1[1] * v1[1]);
+        float absBC = (float) Math.sqrt(v2[0] * v2[0] + v2[1] * v2[1]);
+        v1[0] /= absBA;
+        v1[1] /= absBA;
+        v2[0] /= absBC;
+        v2[1] /= absBC;*/
+
+        float[] bisection = new float[2];
+        bisection[0] = v1[0] + v2[0];
+        bisection[1] = v1[1] + v2[1];
+        if (bisection[0] == 0 && bisection[1] == 0) {
+            // 90 degree to v1
+            bisection[0] = v1[1];
+            bisection[1] = v1[0];
+        }
+        return bisection;
+    }
+
+    /**
+     * @param a first vector
+     * @param b second vector (same length as a)
+     * @return a - b
+     */
+    public static float[] diffVec(float[] a, float[] b) {
+        float[] diff = new float[a.length];
+        for (int i = 0; i < a.length; i++) {
+            diff[i] = a[i] - b[i];
+        }
+        return diff;
+    }
+
+    /**
+     * @param a first vector
+     * @param b second vector (same length as a)
+     * @return a + b
+     */
+    public static float[] sumVec(float[] a, float[] b) {
+        float[] add = new float[a.length];
+        for (int i = 0; i < a.length; i++) {
+            add[i] = b[i] + a[i];
+        }
+        return add;
+    }
+
     public static float squaredDistance(float[] p, int a, int b) {
         return (p[a] - p[b]) * (p[a] - p[b]) + (p[a + 1] - p[b + 1]) * (p[a + 1] - p[b + 1]);
     }
@@ -116,6 +171,17 @@ public final class GeometryUtils {
         return Math.sqrt(dx * dx + dy * dy);
     }
 
+    /**
+     * @param a point a[x,y]
+     * @param b point b[x,y]
+     * @return the distance between a and b.
+     */
+    public static double distance2D(float[] a, float[] b) {
+        float dx = a[0] - b[0];
+        float dy = a[1] - b[1];
+        return Math.sqrt(dx * dx + dy * dy);
+    }
+
     public static double dotProduct(float[] p, int a, int b, int c) {
 
         double ux = (p[b] - p[a]);
@@ -140,6 +206,131 @@ public final class GeometryUtils {
         return dotp;
     }
 
+    /**
+     * @param a first vector
+     * @param b second vector
+     * @return the dot product
+     */
+    public static float dotProduct(float[] a, float[] b) {
+        float dp = 0;
+        for (int i = 0; i < a.length; i++) {
+            dp += a[i] * b[i];
+        }
+        return dp;
+    }
+
+    /**
+     * @param pA position vector of A
+     * @param vA direction vector of A
+     * @param pB position vector of B
+     * @param vB direction vector of B
+     * @return the intersection point
+     */
+    public static float[] intersectionLines2D(float[] pA, float[] vA, float[] pB, float[] vB) {
+        // pA + ldA * vA == pB + ldB * vB;
+
+        float det = vB[0] * vA[1] - vB[1] * vA[0];
+        if (det == 0) {
+            // log.debug(vA.toString() + "and" + vB.toString() + "do not intersect");
+            return null;
+        }
+        float lambA = ((pB[1] - pA[1]) * vB[0] - (pB[0] - pA[0]) * vB[1]) / det;
+
+        float[] intersection = new float[2];
+        intersection[0] = pA[0] + lambA * vA[0];
+        intersection[1] = pA[1] + lambA * vA[1];
+
+        return intersection;
+    }
+
+    /**
+     * Calculate intersection of a plane with a line
+     *
+     * @param pL position vector of line
+     * @param vL direction vector of line
+     * @param pP position vector of plane
+     * @param vP normal vector of plane
+     * @return the intersection point
+     */
+    public static float[] intersectionLinePlane(float[] pL, float[] vL, float[] pP, float[] vP) {
+        float det = dotProduct(vL, vP);
+        if (det == 0) return null;
+        float phi = dotProduct(diffVec(pP, pL), vP) / det;
+        return sumVec(scale(vL, phi), pL);
+    }
+
+    /**
+     * @return more than 0 if points of triangle are clockwise, 0 if triangle is a line,
+     * else less than 0.
+     */
+    public static float isTrisClockwise(float[] pA, float[] pB, float[] pC) {
+        float v = (pB[0] - pA[0]) * (pB[1] + pA[1]);
+        v += (pC[0] - pB[0]) * (pC[1] + pB[1]);
+        v += (pA[0] - pC[0]) * (pA[1] + pC[1]);
+        return v;
+    }
+
+    /**
+     * Calculate the normalized direction vectors of point list (polygon)
+     *
+     * @param points     the list of 2D points
+     * @param outLengths the optional list to store lengths of vectors
+     * @return the normalized direction vectors
+     */
+    public static List<float[]> normalizedVectors2D(List<float[]> points, List<Float> outLengths) {
+        List<float[]> normVectors = new ArrayList<>();
+
+        for (int i = 0; i < points.size(); i++) {
+            float[] pA = points.get(i);
+            float[] pB = points.get((i + 1) % points.size());
+
+            float[] vBA = diffVec(pB, pA);
+
+            // Get length of AB
+            float length = (float) Math.sqrt(vBA[0] * vBA[0] + vBA[1] * vBA[1]);
+            if (outLengths != null)
+                outLengths.add(length);
+
+            vBA[0] /= length; // Normalize vector
+            vBA[1] /= length;
+
+            normVectors.add(vBA);
+        }
+        return normVectors;
+    }
+
+    /**
+     * @param pA first point of plane
+     * @param pB second point of plane
+     * @param pC third point of plane
+     * @return the normal of plane
+     */
+    public static float[] normalOfPlane(float[] pA, float[] pB, float[] pC) {
+        // Calculate normal for color gradient
+        float[] BA = diffVec(pB, pA);
+        float[] BC = diffVec(pC, pA);
+
+        // Vector product (c is at right angle to a and b)
+        float[] normal = new float[3];
+        normal[0] = BA[1] * BC[2] - BA[2] * BC[1];
+        normal[1] = BA[2] * BC[0] - BA[0] * BC[2];
+        normal[2] = BA[0] * BC[1] - BA[1] * BC[0];
+        return normal;
+    }
+
+    /**
+     * @param v     the vector
+     * @param scale the scale
+     * @return the scaled vector
+     */
+    public static float[] scale(float[] v, float scale) {
+        float[] scaled = new float[v.length];
+        for (int i = 0; i < v.length; i++) {
+            scaled[i] = v[i] * scale;
+        }
+        return scaled;
+    }
+
     public static void main(String[] args) {
         float[] p = {-1, 0, 0, 0, 0, 0};