Use dedicated 3-way sort (#339)

* Use dedicated 3-way sort

Arrays.sort carries overhead because it's generalised. But we know
precisely the case here, which is always a 3-element array. Profiling
shows that while assigning ellipsoid IDs most (98% !) of the time is
spent in getSortedRadii(). This dedicated 3-way sort is about 2× faster
than Arrays.sort in a basic performance test.

* Reduce slow methods a bit more

* getSortedRadii() and Math.max() are similar speed

But the result is needed twice (for min and max) so getSortedRadii() is
better here.

* formatting

Modern/ops/src/main/java/org/bonej/ops/ellipsoid/EllipsoidOptimisationStrategy.java

@@ -91,16 +91,14 @@ private static double[][] findContactUnitVectors(final QuickEllipsoid ellipsoid,
 
 		for (int i = 0; i < contactPoints.size(); i++) {
 			final double[] p = contactPoints.get(i);
-			final double px = p[0];
-			final double py = p[1];
-			final double pz = p[2];
-
-			Vector3d distance = new Vector3d(px, py, pz);
-			distance.sub(new Vector3d(cx, cy, cz));
-			final double l = distance.length();
-			final double x = (px - cx) / l;
-			final double y = (py - cy) / l;
-			final double z = (pz - cz) / l;
+			final double px = p[0] - cx;
+			final double py = p[1] - cy;
+			final double pz = p[2] - cz;
+
+			final double l = Math.sqrt(px * px + py * py + pz * pz);
+			final double x = px / l;
+			final double y = py / l;
+			final double z = pz / l;
 			final double[] u = {x, y, z};
 			unitVectors[i] = u;
 		}

Modern/ops/src/main/java/org/bonej/ops/ellipsoid/QuickEllipsoid.java

@@ -202,7 +202,8 @@ public boolean contains(final double x, final double y, final double z) {
 
 		// if length closer than minor semiaxis length
 		// must be inside
-		if (length <= radii[0])
+		final double minRadius = radii[0];
+		if (length <= minRadius)
 			return true;
 
 		final double[][] h = getEllipsoidTensor();
@@ -316,8 +317,29 @@ public void setRotation(final double[][] rotation) {
 	 * @return radii in ascending order
 	 */
 	public double[] getSortedRadii() {
-		final double[] sortedRadii = {ra, rb, rc};
-		Arrays.sort(sortedRadii);
+		double a = this.ra;
+		double b = this.rb;
+		double c = this.rc;
+		double temp = 0;
+
+		if (a > b) {
+			temp = a;
+			a = b;
+			b = temp;
+		}
+		if (b > c) {
+			temp = b;
+			b = c;
+			c = temp;
+		}
+		if (a > b) {
+			temp = a;
+			a = b;
+			b = temp;
+		}
+
+		final double[] sortedRadii = { a, b, c };
+
 		return sortedRadii;
 	}
 
@@ -345,15 +367,15 @@ public double[][] getAxisAlignRandomlyDistributedSurfacePoints(int n) {
 		refreshRandomNumbersIfNeeded();
 
 		final double[] sortedRadii = getSortedRadii();
-		final double muMax = sortedRadii[1]* sortedRadii[2];
+		final double muMax = sortedRadii[1] * sortedRadii[2];
 		final double[][] surfacePoints = new double[n][3];
 		int surfacePointsFound = 0;
 		int attemptCounter = 0;
-		while (surfacePointsFound<n) {
-			final double[] v =  (attemptCounter<numberOfPreallocatedRandomNumbers)? sphereRandomVectors[attemptCounter] : sphereRng.nextVector();
+		while (surfacePointsFound < n) {
+			final double[] v =  (attemptCounter < numberOfPreallocatedRandomNumbers)? sphereRandomVectors[attemptCounter] : sphereRng.nextVector();
 			final double mu = getMu(v);
-			double rn = (attemptCounter<numberOfPreallocatedRandomNumbers) ? uniformRandomNumbers[attemptCounter] : rng.nextDouble();
-			if(rn<=mu/muMax) {
+			double rn = (attemptCounter < numberOfPreallocatedRandomNumbers) ? uniformRandomNumbers[attemptCounter] : rng.nextDouble();
+			if(rn <= mu / muMax) {
 				surfacePoints[surfacePointsFound] = new double[]{v[0], v[1], v[2]};
 				surfacePointsFound++;
 			}
@@ -363,15 +385,15 @@ public double[][] getAxisAlignRandomlyDistributedSurfacePoints(int n) {
 	}
 
 	private void refreshRandomNumbersIfNeeded() {
-		if(sphereRandomVectors==null)
+		if(sphereRandomVectors == null)
 		{
 			sphereRandomVectors = new double[numberOfPreallocatedRandomNumbers][3];
 			uniformRandomNumbers = new double[numberOfPreallocatedRandomNumbers];
 		}
 
-		if((lastRefreshed % randomNumberRefreshmentPeriodicity)==0)
+		if((lastRefreshed % randomNumberRefreshmentPeriodicity) == 0)
 		{
-			for(int i=0;i<numberOfPreallocatedRandomNumbers;i++)
+			for(int i = 0; i < numberOfPreallocatedRandomNumbers; i++)
 			{
 				sphereRandomVectors[i] = sphereRng.nextVector();
 				uniformRandomNumbers[i] = rng.nextDouble();
@@ -381,10 +403,10 @@ private void refreshRandomNumbersIfNeeded() {
 	}
 
 	private double getMu(final double[] v) {
-		final double ra2 = ra*ra;
-		final double rb2 = rb*rb;
-		final double rc2 = rc*rc;
-		final double sqSum = ra2*rc2*v[0]*v[0]+ra2*rb2*v[2]*v[2]+rb2*rc2*v[0]*v[0];
+		final double ra2 = ra * ra;
+		final double rb2 = rb * rb;
+		final double rc2 = rc * rc;
+		final double sqSum = ra2 * rc2 * v[0] * v[0] + ra2 * rb2 * v[2] * v[2] + rb2 * rc2 * v[0] * v[0];
 		return Math.sqrt(sqSum);
 	}
 

Modern/ops/src/test/java/org/bonej/ops/ellipsoid/QuickEllipsoidTest.java

@@ -32,6 +32,9 @@
 
 import static org.junit.Assert.*;
 
+import java.util.Arrays;
+import java.util.Random;
+
 public class QuickEllipsoidTest {
     /**
      * Test for {@link QuickEllipsoid#getSurfacePoints(double[][])}
@@ -99,4 +102,19 @@ public void testContains() {
         assertFalse(e.contains(1,4,1));
         assertTrue(e.contains(1,1,2));
     }
+
+    @Test
+    public void testGetSortedRadii() {
+    	Random rand = new Random();
+    	for (int i = 0; i < 1000; i++) {
+    		final double a = rand.nextDouble();
+    		final double b = rand.nextDouble();
+    		final double c = rand.nextDouble();
+    		final double[] radii = {a, b, c};
+    		final double[] sortedRadii = {a , b, c};
+    		Arrays.sort(sortedRadii);
+    		QuickEllipsoid e = new QuickEllipsoid(radii, new double[]{1,1,1}, new double[][]{{1,0,0},{0,1,0},{0,0,1}});
+    		assertArrayEquals(sortedRadii, e.getSortedRadii(), 0);
+    	}
+    }
 }