Added triangle intersection test. Seems to work on a few tests - need…

…s some graphics fixes.

geometric_separators.pde

@@ -4,7 +4,7 @@ import java.lang.Boolean;
 import java.util.Random;
 import org.jblas.*;
 import org.apache.commons.math3.linear.*;
-import org.apache.commons.math3.linear.SingularValueDecomposition;
+import org.apache.commons.math3.linear.LUDecomposition;
 
 int reset_x = 140;
 int reset_y = 460;
@@ -70,37 +70,39 @@ void mousePressed() {
 // Get Radon point
 PVector getRadonPoint(ArrayList<PVector> points) {
     PVector radonPoint = null;
-    radonPoint = radonTetrx(points);
+    radonPoint = inTetrahedron(points);
     if (radonPoint == null) {
-        radonPoint = radonIntersect();
+        System.out.println("Intersect");
+        radonPoint = intersectTri(points);
     }
     return radonPoint;
 }
 
 // Tests if a point is within a tetrahedron
-PVector radonTetra(ArrayList<PVector> points) {
+PVector inTetrahedron(ArrayList<PVector> points) {
     PVector radonPoint = null;
     for (int i = 0; i < points.size(); i++) {
         ArrayList<PVector> pointsCopy = new ArrayList<PVector>(points);
         PVector testPoint = pointsCopy.get(i);
         pointsCopy.remove(i);
         // Test if every determinant has the same sign
-        RealMatrix d0Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), pointsCopy.get(2), pointsCopy.get(3);
-        RealMatrix d1Matrix = getDetMatrix(testPoint, pointsCopy.get(1), pointsCopy.get(2), pointsCopy.get(3);
-        RealMatrix d2Matrix = getDetMatrix(pointsCopy.get(0), testPoint, pointsCopy.get(2), pointsCopy.get(3);
-        RealMatrix d3Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), testPoint, pointsCopy.get(3);
-        RealMatrix d4Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), pointsCopy.get(2), testPoint;
+        System.out.println(pointsCopy.size());
+        RealMatrix d0Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), pointsCopy.get(2), pointsCopy.get(3));
+        RealMatrix d1Matrix = getDetMatrix(testPoint, pointsCopy.get(1), pointsCopy.get(2), pointsCopy.get(3));
+        RealMatrix d2Matrix = getDetMatrix(pointsCopy.get(0), testPoint, pointsCopy.get(2), pointsCopy.get(3));
+        RealMatrix d3Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), testPoint, pointsCopy.get(3));
+        RealMatrix d4Matrix = getDetMatrix(pointsCopy.get(0), pointsCopy.get(1), pointsCopy.get(2), testPoint);
         // Compute determinants
-        ArrayList<double> detList = new ArrayList<double>();
-        double d0 = new SingularValueDecomposition(d0Matrix).getDeterminant();
+        ArrayList<Double> detList = new ArrayList<Double>();
+        double d0 = new LUDecomposition(d0Matrix).getDeterminant();
         detList.add(d0);
-        double d1 = new SingularValueDecomposition(d1Matrix).getDeterminant();
+        double d1 = new LUDecomposition(d1Matrix).getDeterminant();
         detList.add(d1);
-        double d2 = new SingularValueDecomposition(d2Matrix).getDeterminant();
+        double d2 = new LUDecomposition(d2Matrix).getDeterminant();
         detList.add(d2);
-        double d3 = new SingularValueDecomposition(d3Matrix).getDeterminant();
+        double d3 = new LUDecomposition(d3Matrix).getDeterminant();
         detList.add(d3);
-        double d4 = new SingularValueDecomposition(d4Matrix).getDeterminant();
+        double d4 = new LUDecomposition(d4Matrix).getDeterminant();
         detList.add(d4);
         // If the sign test passes
         if (areSameSign(detList)) {
@@ -112,8 +114,103 @@ PVector radonTetra(ArrayList<PVector> points) {
 }
 
 // Tests if a ray intersects a triangle
-void radonIntersect(ArrayList<PVector> points) {
+PVector intersectTri(ArrayList<PVector> points) {
+    PVector point;
+    // Triangle of Points 1, 2, 3
+    point = testIntersect(points.get(0), points.get(1), points.get(2), points.get(3), points.get(4));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 1, 2, 4
+    point = testIntersect(points.get(0), points.get(1), points.get(3), points.get(2), points.get(4));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 1, 2, 5
+    point = testIntersect(points.get(0), points.get(1), points.get(4), points.get(2), points.get(3));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 1, 3, 4
+    point = testIntersect(points.get(0), points.get(2), points.get(3), points.get(1), points.get(4));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 1, 3, 5
+    point = testIntersect(points.get(0), points.get(2), points.get(4), points.get(2), points.get(3));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 1, 4, 5
+    point = testIntersect(points.get(0), points.get(3), points.get(4), points.get(1), points.get(2));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 2, 3, 4
+    point = testIntersect(points.get(1), points.get(2), points.get(3), points.get(0), points.get(4));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 2, 3, 5
+    point = testIntersect(points.get(1), points.get(2), points.get(4), points.get(0), points.get(3));
+    if (point != null) {
+        return point;
+    }
+    // Triangle of Points 2, 4, 5
+    point = testIntersect(points.get(1), points.get(3), points.get(4), points.get(0), points.get(2));
+    if (point != null) {
+        return point;
+    }
+    return null;
+}
 
+// Test ray triangle intersection
+PVector testIntersect(PVector t1, PVector t2, PVector t3, PVector r1, PVector r2) {
+    PVector a = new PVector(t2.x, t2.y, t2.z);
+    a.sub(t1);
+    PVector b = new PVector(t3.x, t3.y, t3.z);
+    b.sub(t1);
+    PVector crossProd = a.cross(b);
+    // Get ray direction
+    PVector rayDir = new PVector(r2.x, r2.y, r2.z);
+    rayDir.sub(r1);
+    PVector w0 = new PVector(r1.x, r1.y, r1.z);
+    w0.sub(t1);
+    float x = -1 * crossProd.dot(w0);
+    float y = crossProd.dot(rayDir);
+    // Get intersection point of the plane
+    float r = x / y;
+    // No intersection
+    if (r < 0) {
+        System.out.println("No intersect.");
+        return null;
+    }
+    rayDir.mult(r);
+    PVector point = new PVector(r1.x, r1.y, r1.z);
+    point.add(rayDir);
+    // Is the point inside of the triangle?
+    float aDot = a.dot(a);
+    float abDot = a.dot(b);
+    float bDot = b.dot(b);
+    PVector w = new PVector(point.x, point.y, point.z);
+    w.sub(t1);
+    float waDot = w.dot(a);
+    float wbDot = w.dot(b);
+    float d = (abDot * abDot) - (aDot * bDot);
+    float s = ((abDot * wbDot) - (bDot * waDot)) / d;
+    float t = ((abDot * waDot) - (aDot * wbDot)) / d;
+    // Check if the point is outside
+    if (s < 0 || s > 1) {
+        System.out.println("Outside.");
+        return null;
+    }
+    else if (t < 0 || (s + t) > 1) {
+        System.out.println("Outside.");
+        return null;
+    }
+    // Otherwise it's inside we can return it
+    System.out.println("Inside");
+    return point;
 }
 
 // Get the determinant matrix for a set of points
@@ -125,13 +222,14 @@ RealMatrix getDetMatrix(PVector p1, PVector p2, PVector p3, PVector p4) {
 }
 
 // Checks if a list of doubles all have the same sign
-boolean areSameSign(ArrayList<double> detList) {
+boolean areSameSign(ArrayList<Double> detList) {
     int negDet = 0;
     for (double det : detList) {
         if (det < 0) {
             negDet++;
         }
     }
+    System.out.println("negDet" + Integer.toString(negDet));
     if (negDet == 0 || negDet == detList.size()) {
         return true;
     }
@@ -141,12 +239,12 @@ boolean areSameSign(ArrayList<double> detList) {
 }
 
 // Convert float array to double array
-float[][] toDoubleArray(float[][] array) {
+double[][] toDoubleArray(float[][] array) {
     int numRows = array.length;
     int numCols = array[0].length;
     double[][] doubleArray = new double[numRows][numCols];
-    for (int i = 0; i < array.length {
-        for (int j = 0; j < array[i].length {
+    for (int i = 0; i < array.length; i++) {
+        for (int j = 0; j < array[i].length; j++) {
             doubleArray[i][j] = (double)array[i][j];
         }
     }
@@ -195,7 +293,7 @@ ArrayList<ArrayList<PVector>> samplePoints(ArrayList<PVector> input) {
         pointList.add(inputCopy.get(index));
         inputCopy.remove(index);
         // Create new set on max size
-        if (pointList.size() == 4) {
+        if (pointList.size() == 5) {
             setList.add(pointList);
             pointList = new ArrayList<PVector>();
         }
@@ -218,6 +316,7 @@ PVector approxCenterpoint(ArrayList<PVector> input) {
 			ArrayList<ArrayList<PVector>> setList = samplePoints(input);
 			ArrayList<PVector> radonList = new ArrayList<PVector>();
 			for (ArrayList<PVector> list : setList) {
+                System.out.println(list.size());
 				PVector radonPoint = getRadonPoint(list);
 				radonList.add(radonPoint);
 			}