GeometryUtils: add vector utils (#467)
+ calc bisections + sum, diff + distance + dot product + intersections (line and plane) + normalizations + normal of plane + scale
This commit is contained in:
Gustl22
Emux
parent
baa924c697
commit
1d8cdd7feb
@ -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};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user