From 5974f4a3f041e91cd2872f5474e0fdba1337a0f3 Mon Sep 17 00:00:00 2001 From: Kevin harrington Date: Sun, 10 Nov 2024 09:55:07 -0500 Subject: [PATCH] Attempt robust normal setting --- src/main/java/eu/mihosoft/vrl/v3d/Plane.java | 210 +++++++++++++++---- 1 file changed, 172 insertions(+), 38 deletions(-) diff --git a/src/main/java/eu/mihosoft/vrl/v3d/Plane.java b/src/main/java/eu/mihosoft/vrl/v3d/Plane.java index 9b89659d..672a9a75 100644 --- a/src/main/java/eu/mihosoft/vrl/v3d/Plane.java +++ b/src/main/java/eu/mihosoft/vrl/v3d/Plane.java @@ -102,51 +102,185 @@ public static Plane createFromPoints(List vertices) { Vector3d n = computeNormal(vertices); return new Plane(n, n.dot(a)); } - public static Vector3d computeNormal(List vertices) { - Vector3d normal = new Vector3d(0, 0, 0); - int n = vertices.size(); + if (vertices == null || vertices.size() < 3) { + return new Vector3d(0, 0, 1); // Default normal for degenerate cases + } - for (int i = 0; i < n; i++) { - Vector3d current = vertices.get(i).pos; - Vector3d next = vertices.get((i + 1) % n).pos; + // First attempt: Newell's method + Vector3d normal = new Vector3d(0, 0, 0); + int n = vertices.size(); - normal.x += (current.y - next.y) * (current.z + next.z); - normal.y += (current.z - next.z) * (current.x + next.x); - normal.z += (current.x - next.x) * (current.y + next.y); - } + for (int i = 0; i < n; i++) { + Vector3d current = vertices.get(i).pos; + Vector3d next = vertices.get((i + 1) % n).pos; - Vector3d normalized = normal.normalized(); - // If Newell's method fails, try finding three non-collinear points - double lengthSquared = normal.lengthSquared(); - double d = EPSILON * EPSILON; - if (lengthSquared < d) { // Adjust this epsilon as needed - for (int i = 0; i < n - 2; i++) { - Vector3d a = vertices.get(i).pos; - for (int j = i + 1; j < n - 1; j++) { - Vector3d b = vertices.get(j).pos; - for (int k = j + 1; k < n; k++) { - Vector3d c = vertices.get(k).pos; - normal = b.minus(a).cross(c.minus(a)); - lengthSquared = normal.lengthSquared(); - if (lengthSquared > d) { // Non-zero normal found - return normal.normalized(); - } - } - } - } - } + normal.x += (current.y - next.y) * (current.z + next.z); + normal.y += (current.z - next.z) * (current.x + next.x); + normal.z += (current.x - next.x) * (current.y + next.y); + } - // If all else fails, return a default normal (e.g., in the z direction) - lengthSquared = normal.lengthSquared(); + if (isValidNormal(normal, EPSILON)) { + return normal.normalized(); + } - if (lengthSquared < Double.MIN_VALUE*10) { - throw new NumberFormatException("This set of points is not a valid polygon"); - } - if(normalized.lengthSquared()= epsilon * epsilon; + } + + private static Vector3d findNormalFromNonColinearPoints(List vertices) { + int n = vertices.size(); + Vector3d firstPoint = vertices.get(0).pos; + + // Try to find two vectors that aren't parallel + for (int i = 1; i < n; i++) { + Vector3d v1 = vertices.get(i).pos.minus(firstPoint); + for (int j = i + 1; j < n; j++) { + Vector3d v2 = vertices.get(j).pos.minus(firstPoint); + Vector3d cross = v1.cross(v2); + if (isValidNormal(cross, EPSILON)) { + return cross.normalized(); + } + } + } + return null; + } + + private static Vector3d findPrincipalDirection(List vertices) { + // Find the direction with maximum spread + double maxX = Double.NEGATIVE_INFINITY; + double minX = Double.POSITIVE_INFINITY; + double maxY = Double.NEGATIVE_INFINITY; + double minY = Double.POSITIVE_INFINITY; + double maxZ = Double.NEGATIVE_INFINITY; + double minZ = Double.POSITIVE_INFINITY; + + for (Vertex vertex : vertices) { + Vector3d pos = vertex.pos; + maxX = Math.max(maxX, pos.x); + minX = Math.min(minX, pos.x); + maxY = Math.max(maxY, pos.y); + minY = Math.min(minY, pos.y); + maxZ = Math.max(maxZ, pos.z); + minZ = Math.min(minZ, pos.z); + } + + double rangeX = maxX - minX; + double rangeY = maxY - minY; + double rangeZ = maxZ - minZ; + + // Use the axis with minimum spread as normal direction + if (rangeX <= rangeY && rangeX <= rangeZ) { + return new Vector3d(1, 0, 0); + } else if (rangeY <= rangeX && rangeY <= rangeZ) { + return new Vector3d(0, 1, 0); + } else { + return new Vector3d(0, 0, 1); + } + } + + private static Vector3d determineStatisticalNormal(List vertices) { + // Calculate center of mass + Vector3d center = new Vector3d(0, 0, 0); + for (Vertex vertex : vertices) { + center = center.plus(vertex.pos); + } + center = center.times(1.0 / vertices.size()); + + // Calculate covariance matrix + double[][] covariance = new double[3][3]; + for (Vertex vertex : vertices) { + Vector3d diff = vertex.pos.minus(center); + covariance[0][0] += diff.x * diff.x; + covariance[0][1] += diff.x * diff.y; + covariance[0][2] += diff.x * diff.z; + covariance[1][1] += diff.y * diff.y; + covariance[1][2] += diff.y * diff.z; + covariance[2][2] += diff.z * diff.z; + } + covariance[1][0] = covariance[0][1]; + covariance[2][0] = covariance[0][2]; + covariance[2][1] = covariance[1][2]; + + // Use the eigenvector corresponding to the smallest eigenvalue + // For simplicity, we'll use power iteration to find it + Vector3d normal = new Vector3d(1, 1, 1); + for (int i = 0; i < 10; i++) { + normal = multiplyMatrixVector(covariance, normal); + normal = normal.normalized(); + } + + return normal; + } + + private static Vector3d multiplyMatrixVector(double[][] matrix, Vector3d vector) { + return new Vector3d( + matrix[0][0] * vector.x + matrix[0][1] * vector.y + matrix[0][2] * vector.z, + matrix[1][0] * vector.x + matrix[1][1] * vector.y + matrix[1][2] * vector.z, + matrix[2][0] * vector.x + matrix[2][1] * vector.y + matrix[2][2] * vector.z + ); } +// public static Vector3d computeNormal(List vertices) { +// Vector3d normal = new Vector3d(0, 0, 0); +// int n = vertices.size(); +// +// for (int i = 0; i < n; i++) { +// Vector3d current = vertices.get(i).pos; +// Vector3d next = vertices.get((i + 1) % n).pos; +// +// normal.x += (current.y - next.y) * (current.z + next.z); +// normal.y += (current.z - next.z) * (current.x + next.x); +// normal.z += (current.x - next.x) * (current.y + next.y); +// } +// +// Vector3d normalized = normal.normalized(); +// // If Newell's method fails, try finding three non-collinear points +// double lengthSquared = normal.lengthSquared(); +// double d = EPSILON * EPSILON; +// if (lengthSquared < d) { // Adjust this epsilon as needed +// for (int i = 0; i < n - 2; i++) { +// Vector3d a = vertices.get(i).pos; +// for (int j = i + 1; j < n - 1; j++) { +// Vector3d b = vertices.get(j).pos; +// for (int k = j + 1; k < n; k++) { +// Vector3d c = vertices.get(k).pos; +// normal = b.minus(a).cross(c.minus(a)); +// lengthSquared = normal.lengthSquared(); +// if (lengthSquared > d) { // Non-zero normal found +// return normal.normalized(); +// } +// } +// } +// } +// } +// +// // If all else fails, return a default normal (e.g., in the z direction) +// lengthSquared = normal.lengthSquared(); +// +// if (lengthSquared < Double.MIN_VALUE*10) { +// throw new NumberFormatException("This set of points is not a valid polygon"); +// } +// if(normalized.lengthSquared()