From 1d8cdd7febc6d8849f7b1b08cb264a993588e9b0 Mon Sep 17 00:00:00 2001 From: Gustl22 Date: Wed, 3 Jan 2018 21:35:38 +0100 Subject: [PATCH] GeometryUtils: add vector utils (#467) + calc bisections + sum, diff + distance + dot product + intersections (line and plane) + normalizations + normal of plane + scale --- .../org/oscim/utils/geom/GeometryUtils.java | 191 ++++++++++++++++++ 1 file changed, 191 insertions(+) diff --git a/vtm/src/org/oscim/utils/geom/GeometryUtils.java b/vtm/src/org/oscim/utils/geom/GeometryUtils.java index 30aa5e4f..c4cf3f99 100644 --- a/vtm/src/org/oscim/utils/geom/GeometryUtils.java +++ b/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 normalizedVectors2D(List points, List outLengths) { + List 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};