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;
@@ -2126,13 +2126,16 @@ public void run() {
 				System.out.println("ERR polygon " + i + " pruned because of too few points");
 				continue;
 			}
-			Polygon p;
+			List<Polygon> p;
 			try {
-				p = new Polygon(points, polygon.getStorage(), true, pl);
-				newPoly.add(p);
+				p =  Polygon.get(points, polygon.getStorage(), true, pl);
+				newPoly.addAll(p);
 			} catch (ColinearPointsException e) {
 				System.err.println("Pruning " + points);
 				e.printStackTrace();
+			} catch (NonFlatPolygonException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
 			}
 			polygon.getPoints().clear();
 		}

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

@@ -70,8 +70,14 @@ public class Extrude {
 		public CSG points(Vector3d dir, List<Vector3d> points) throws ColinearPointsException {
 
 			List<Vector3d> newList = new ArrayList<>(points);
-			Polygon fromPoints = Polygon.fromPoints(toCCW(newList));
-			return extrude(dir, fromPoints);
+			Polygon fromPoints;
+			try {
+				fromPoints = Polygon.fromVector3d(toCCW(newList)).get(0);
+
+				return extrude(dir, fromPoints);
+			} catch (ColinearPointsException | NonFlatPolygonException e) {
+				throw new RuntimeException(e);
+			}
 		}
 
 		/**
@@ -101,8 +107,14 @@ private CSG monotoneExtrude(Vector3d dir, Polygon polygon1) throws ColinearPoint
 				newPolygons.add(p.transformed(new Transform().move(dir)));
 			}
 			Polygon polygon2 = polygon1.transformed(new Transform().move(dir));
-			List<Polygon> parts = Extrude.monotoneExtrude(polygon2, polygon1);
-			newPolygons.addAll(parts);
+			List<Polygon> parts;
+			try {
+				parts = Extrude.monotoneExtrude(polygon2, polygon1);
+				newPolygons.addAll(parts);
+			} catch (NonFlatPolygonException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
 
 			//ArrayList<Polygon> topPolygons = PolygonUtil.triangulatePolygon(polygon2);
 
@@ -575,9 +587,15 @@ public static CSG sweep(Polygon p, Transform increment, Transform offset, int st
 			running.apply(increment);
 			double unit = ((double) i) / ((double) steps);
 			Polygon step = offsetP.transformed(provider.get(unit, steps)).transformed(running);
-			List<Polygon> parts = monotoneExtrude(prev, step);
-			prev = step;
-			newPolygons.addAll(parts);
+			List<Polygon> parts;
+			try {
+				parts = monotoneExtrude(prev, step);
+				prev = step;
+				newPolygons.addAll(parts);
+			} catch (NonFlatPolygonException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
 		}
 		Polygon polygon2 = prev.clone();
 		List<Polygon> topPolygons = PolygonUtil.triangulatePolygon(polygon2.flipped());
@@ -590,7 +608,7 @@ public static CSG sweep(Polygon p, double angle, double z, double radius, int st
 		return sweep(p, new Transform().rotX(angle).movex(z), new Transform().movey(radius), steps);
 	}
 
-	public static List<Polygon> monotoneExtrude(Polygon polygon2, Polygon polygon1) {
+	public static List<Polygon> monotoneExtrude(Polygon polygon2, Polygon polygon1) throws NonFlatPolygonException {
 		List<Polygon> newPolygons = new ArrayList<>();
 
 		int numvertices = polygon1.getVertices().size();
@@ -608,7 +626,7 @@ public static List<Polygon> monotoneExtrude(Polygon polygon2, Polygon polygon1)
 			if (Math.abs(distance) > Plane.getEPSILON() && Math.abs(z1Dist) > Plane.getEPSILON()) {
 				List<Vector3d> asList = Arrays.asList(bottomV2.clone(), topV1.clone(), bottomV1.clone());
 				try {
-					newPolygons.add(Polygon.fromPoints(asList, polygon1.getStorage()));
+					newPolygons.addAll(Polygon.fromVector3d(asList, polygon1.getStorage()));
 				} catch (ColinearPointsException ex) {
 					System.out.println(ex.getMessage()+" Pruning from extrude");
 				}
@@ -618,7 +636,7 @@ public static List<Polygon> monotoneExtrude(Polygon polygon2, Polygon polygon1)
 			if (Math.abs(distance2) > Plane.getEPSILON() && Math.abs(z1Dist2) > Plane.getEPSILON()) {
 				List<Vector3d> asList2 = Arrays.asList(bottomV2.clone(), topV2.clone(), topV1.clone());
 				try {
-					newPolygons.add(Polygon.fromPoints(asList2, polygon1.getStorage()));
+					newPolygons.addAll(Polygon.fromVector3d(asList2, polygon1.getStorage()));
 				} catch (ColinearPointsException ex) {
 					System.out.println(ex.getMessage()+" Pruning from extrude");
 				}

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,24 @@ 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);	
+			List<Polygon> fpoly =  Polygon.fromVertex(f, polygon.getStorage(), true, polygon.getPlane(),polygon.getColor());
+			l.addAll(fpoly);	
 		}catch(ColinearPointsException ex) {
 			//ex.printStackTrace();
-			System.err.println(ex.getMessage()+" Pruned Collinear polygon "+f );
+			System.err.println("Pruned Collinear polygon "+f+" "+ex.getMessage() );
+		} catch (NonFlatPolygonException e) {
+			e.printStackTrace();
 		}
 	}
 	public static String getOsName() {
@@ -934,21 +933,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 +968,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 +1028,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;
-	}