Attempt to resolve CaDoodle-Application/issues/200

src/main/java/eu/mihosoft/vrl/v3d/CSG.java

@@ -136,7 +136,7 @@
 
 @SuppressWarnings("restriction")
 public class CSG implements IuserAPI, Serializable {
-	transient private static final double POINTS_CONTACT_DISTANCE =  0.00001;
+	transient private static final double POINTS_CONTACT_DISTANCE = 0.0001;
 	transient private static int MinPolygonsForOffloading = 200;
 	transient private static final long serialVersionUID = 4071874097772427063L;
 	transient private static IDebug3dProvider providerOf3d = null;

src/main/java/eu/mihosoft/vrl/v3d/IPolygonRepairTool.java

@@ -1,5 +1,10 @@
 package eu.mihosoft.vrl.v3d;
 
+import java.util.List;
+
+import javafx.scene.paint.Color;
+
 public interface IPolygonRepairTool {
-	Polygon repairOverlappingEdges(Polygon concave) throws ColinearPointsException;
+	List<Polygon> repairOverlappingEdges(List<Vertex> vertices, PropertyStorage shared,
+			boolean allowDegenerate, Plane p, Color c) throws ColinearPointsException;
 }

src/main/java/eu/mihosoft/vrl/v3d/Node.java

@@ -271,8 +271,7 @@ private static int add(List<Polygon> l, int polygonIndex, int[] polygonStartInde
 			return 1;
 		}
 		try {
-			testAddPolygon(l, orderedPoints, polygonPointX, polygonPointY, polygonPointZ, polygon, polygonBase, size,
-					false);
+			testAddPolygon(l, orderedPoints, polygonPointX, polygonPointY, polygonPointZ, polygon, polygonBase, size);
 			return 1;
 		} catch (Exception ex) {
 			ex.printStackTrace();
@@ -282,7 +281,7 @@ private static int add(List<Polygon> l, int polygonIndex, int[] polygonStartInde
 	}
 
 	private static void testAddPolygon(List<Polygon> l, ArrayList<Vertex> orderedPoints, double[] polygonPointX,
-			double[] polygonPointY, double[] polygonPointZ, Polygon polygon, int polygonBase, int size, boolean test) {
+			double[] polygonPointY, double[] polygonPointZ, Polygon polygon, int polygonBase, int size) {
 		List<Vertex> f = new ArrayList<>();
 		for (int i = polygonBase; i < polygonBase + size; i++) {
 			if (i < orderedPoints.size()) {
@@ -296,7 +295,7 @@ private static void testAddPolygon(List<Polygon> l, ArrayList<Vertex> orderedPoi
 			}
 		}
 
-		add(l, f, polygon, test);
+		add(l, f, polygon);
 	}
 
 	private static boolean addPoint(List<Vertex> f, Vertex v) {
@@ -311,24 +310,23 @@ private static boolean addPoint(List<Vertex> f, Vertex v) {
 		return f.add(v);
 	}
 
-	private static void add(List<Polygon> l, List<Vertex> f, Polygon polygon) {
-		add(l, f, polygon, false);
-	}
+//	private static void add(List<Polygon> l, List<Vertex> f, Polygon polygon) {
+//		add(l, f, polygon, false);
+//	}
 
-	private static void add(List<Polygon> l, List<Vertex> f, Polygon polygon, boolean test) {
+	private static void add(List<Polygon> l, List<Vertex> f, Polygon polygon) {
 		if (f.size() < 3)
 			return;
 		try {
-			if(!Extrude.isCCW(f, polygon.getPlane().getNormal())) {
+			if(polygon.getPlane().checkNormal(f) == NormalState.FLIPPED) {
 				Collections.reverse(f);
 			}
 			Polygon fpoly = new Polygon(f, polygon.getStorage(), true, polygon.getPlane())
 					.setColor(polygon.getColor());
-			if (!test)
-				l.add(fpoly);	
+			l.add(fpoly);	
 		}catch(ColinearPointsException ex) {
 			//ex.printStackTrace();
-			System.err.println(ex.getMessage()+" Pruned Collinear polygon "+f );
+			System.err.println("Pruned Collinear polygon "+f+" "+ex.getMessage() );
 		}
 	}
 	public static String getOsName() {
@@ -934,21 +932,8 @@ private void splitSinglePolygon(Polygon polygon,List<Polygon> coplanarFront, Lis
 		// search for the epsilon values of the incoming plane
 		double posEpsilon = Plane.getEPSILON();
 		int size = polygon.getVertices().size();
-		Vector3d normal = polygon.getPlane().getNormal();
-			for (int i = 0; i < size; i++) {
-				Vector3d pos = polygon.getVertices().get(i).pos;
-				double dot = normal.dot(pos);
-				double t = Math.abs(dot - polygon.getPlane().getDist());
-				if(t>0.01) {
-					throw new RuntimeException("A plane epsilon of "+t+" is impossible");
-				}
-				if (t > posEpsilon) {
-					// com.neuronrobotics.sdk.common.Log.error("Non flat polygon, increasing
-					// positive epsilon "+t);
-					posEpsilon = t;
-				}
+		//Vector3d normal = polygon.getPlane().getNormal();
 
-			}
 		int polygonType = 0;
 		List<Integer> types = new ArrayList<>();
 //			boolean someF =false;
@@ -982,7 +967,7 @@ private void splitSinglePolygon(Polygon polygon,List<Polygon> coplanarFront, Lis
 		// Put the polygon in the correct list, splitting it when necessary.
 		switch (polygonType) {
 		case COPLANAR:
-			double cp = getThisNodePlane().getNormal().dot(normal);
+			double cp = getThisNodePlane().getNormal().dot(polygon.getPlane().getNormal());
 			(cp > 0 ? coplanarFront : coplanarBack).add(polygon);
 			break;
 		case FRONT:
@@ -1042,6 +1027,7 @@ private void splitSinglePolygon(Polygon polygon,List<Polygon> coplanarFront, Lis
 					// therefor the intersection point is halfway between i and j
 					double t = (d / dotMinus);
 					if (!Double.isFinite(t) || t < 0 || t > 1.0) {
+						new RuntimeException("ERROR in interpolation!").printStackTrace();
 					    continue;
 					}
 

src/main/java/eu/mihosoft/vrl/v3d/NonFlatPolygonException.java

@@ -0,0 +1,11 @@
+package eu.mihosoft.vrl.v3d;
+
+public class NonFlatPolygonException extends Exception {
+
+	public NonFlatPolygonException(String string) {
+		super(string);
+	}
+
+	private static final long serialVersionUID = -7968568496236268642L;
+
+}

src/main/java/eu/mihosoft/vrl/v3d/NormalState.java

@@ -0,0 +1,5 @@
+package eu.mihosoft.vrl.v3d;
+
+public enum NormalState {
+SAME,FLIPPED,DIVERGENT;
+}

src/main/java/eu/mihosoft/vrl/v3d/Plane.java

@@ -101,10 +101,15 @@ public Plane(Vector3d normal, double dist) {
 	 * @throws ColinearPointsException 
 	 */
 	public Plane(List<Vertex> vertices, Vector3d testNorm) throws ColinearPointsException {
-		Vector3d a = vertices.get(0).pos;
+
 		Vector3d n = computeNormal(vertices, testNorm);
 		this.setNormal(n);
-		this.setDist(n.dot(a));
+		double distAvg = 0;
+		for(int i=0;i<vertices.size();i++) {
+			Vector3d a = vertices.get(i).pos;
+			distAvg+=n.dot(a);
+		}
+		this.setDist(distAvg/((double)vertices.size()));
 	}
 
 	/**
@@ -116,7 +121,12 @@ public Plane(List<Vertex> vertices, Vector3d testNorm) throws ColinearPointsExce
 	 */
 	public Plane(Vector3d normal, List<Vertex> vertices) {
 		this.setNormal(normal.normalized());
-		this.setDist(normal.dot(vertices.get(0).pos));
+		double distAvg = 0;
+		for(int i=0;i<vertices.size();i++) {
+			Vector3d a = vertices.get(i).pos;
+			distAvg+=normal.dot(a);
+		}
+		this.setDist(distAvg/((double)vertices.size()));
 	}
 	/**
 	 * Constructor. Creates a new plane defined by its normal vector and the
@@ -193,7 +203,7 @@ private boolean isValidNormal(Vector3d normal) {
 	    return false;
 	}
 
-	public boolean checkNormal(ArrayList<Vertex> vertex) {
+	public NormalState checkNormal(List<Vertex> vertex) {
 		Plane p = null;
 		try {
 			p = Plane.createFromPoints(vertex, getNormal());
@@ -203,84 +213,21 @@ public boolean checkNormal(ArrayList<Vertex> vertex) {
 		if (p != null) {
 			Vector3d normal = p.getNormal();
 			Vector3d normal2 = getNormal();
-			double dot = 1-Math.abs(normal.dot(normal2));
+			double dot2 = normal.dot(normal2);
+			double dot = 1-dot2;
+			double dFlipped = 2-dot2;
 			// check for actual misallignment
 			double d = Plane.getEPSILON();
-			if(dot<d)
-				return true;
-			double e3 = (normal.x) - (normal2.x);
-			double e4 = (normal.y) - (normal2.y);
-			double e5 = (normal.z) - (normal2.z);
-			if (e3 > d
-					|| e4 > d
-					|| e5 > d) {
-				double e = Math.abs(normal.x) - Math.abs(normal2.x);
-				double e2 = Math.abs(normal.y) - Math.abs(normal2.y);
-				double f = Math.abs(normal.z) - Math.abs(normal2.z);
-				if (e > d
-						|| e2 > d
-						|| f > d) {
-					return false;
-				}
-				return false;
-			}
+			if(dot>d)
+				if(dFlipped>d)
+					return NormalState.DIVERGENT;
+				else
+					return NormalState.FLIPPED;
+
 		}
-		return true;
+		return NormalState.SAME;
 	}
 
-	public static Vector3d computeNormalCrossProduct(List<Vertex> verts) {
-		int n = verts.size();
-		if (n < 3)
-			return new Vector3d(0, 0, 1);
-
-		// 1. Build all edge vectors
-		List<Vector3d> edges = new ArrayList<>();
-
-		for (int j = 0; j < n; j++) {
-			Vector3d e = verts.get((j+1)%n).pos.minus(verts.get(j).pos);
-			edges.add(e);
-
-		}
-
-
-		// 2. Find pair with smallest |dot| / (|e1||e2|)
-		double bestScore = Double.POSITIVE_INFINITY;
-		Vector3d bestE1 = null, bestE2 = null;
-		for (int i = 0; i < edges.size(); i++) {
-			Vector3d e1 = edges.get(i);
-			double len1 = e1.length();
-			for (int j = i + 1; j < edges.size(); j++) {
-				Vector3d e2 = edges.get(j);
-				double len2 = e2.length();
-				double score = Math.abs(e1.dot(e2)) / (len1 * len2);
-				if (score < bestScore) {
-					bestScore = score;
-					bestE1 = e1;
-					bestE2 = e2;
-				}
-			}
-		}
-
-		// 3. Fallback: use first three vertices if no good pair found
-		if (bestE1 == null) {
-			Vector3d v0 = verts.get(0).pos;
-			bestE1 = verts.get(1).pos.minus(v0);
-			bestE2 = verts.get(2).pos.minus(v0);
-		}
-
-		// 4. Compute normal
-		Vector3d normal = bestE1.cross(bestE2);
-		if (normal.magnitude() < Plane.getEPSILON()) {
-			throw new RuntimeException("Fail! Normal can not be computed");
-		}
-		normal.normalize();
-		Vector3d v0 = verts.get(0).pos;
-		Vector3d std = verts.get(1).pos.minus(v0).cross(verts.get(2).pos.minus(v0)).normalized();
-		if (normal.dot(std) < 0) {
-			normal = normal.negated();
-		}
-		return normal;
-	}