/**
 *  Content:   Algorithms.java
 *  Author:    Jakob Svensson (c) 2006
 */

package algorithmrepository;

import jafama.FastMath;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.LinkedHashSet;

public final class Algorithms {

   private static final double h = 1.0e-6;

    public static interface IFunction {
        double eval(double x);
    }


    /**
     * Calculates derivative of a single valued function using
     * forward differences.
     *
     * @param f0 The function value at x
     * @param f1 The function value at x+dx.
     * @return The forward difference derivative of the function at x.
     */
    public static final double derf(double f0, double f1, double dx) {
        return (f1 - f0) / dx;
    }


    /**
     * Calculates derivative of a single valued function using
     * a central difference formula.
     *
     * @param f0 The function value at x-dx
     * @param f1 The function value at x+dx.
     * @param dx The step size.
     * @return The central difference derivative of the function at x.
     */
    public static final double der(double f0, double f1, double dx) {
        return (f1 - f0) / (2 * dx);
    }


    /**
     * Calculates 2nd derivative of a single valued function using
     * a central difference formula.
     *
     * @param f0 The function value at x-dx
     * @param f1 The function value at x
     * @param f2 The function value at x+dx
     * @param dx The step size
     * @return The central difference derivative of the function at x.
     */
    public static final double der2(double f0, double f1, double f2, double dx) {

        return (f2 - 2 * f1 + f0) / (dx * dx);
    }


    /**
     * Calculates 2nd mixed derivative of a single valued function using
     * central differences (d2fdxdy).
     *
     * @param fll The function value at x-dx, y-dx
     * @param fhl The function value at x+dx, y-dx
     * @param flh The function value at x-dx, y+dx
     * @param fhh The function value at x+dx, y+dx
     * @param dx x increment
     * @param dy y increment
     * @return The 2nd mixed derivative of the function at x.
     */
    public static final double der2m(double fll, double fhl, double flh, double fhh, double dx, double dy) {
        return (fhh - flh - fhl + fll) / (4 * dx * dy);
    }


    /**
     * Calculates 2nd mixed derivative of a single valued function using
     * forward differences (d2fdxdy).
     *
     * @param fll The function value at x, y
     * @param fhl The function value at x+dx, y
     * @param fhh The function value at x+dx, y+dy
     * @param flh The function value at x, y+dy
     * @param dx x increment
     * @param dy y increment
     * @return The 2nd mixed derivative of the function at x.
     */
    public static final double der2mf(double fll, double fhl, double fhh, double flh, double dx, double dy) {
        return (fhh - flh - fhl + fll) / (dx * dy);
    }


    /**
     * Returns the difference between consecutive elements of f.
     * 
     * @param f The values.
     * @return The difference between consecutive values.
     */
    public static final double[] diff(double[] f) {
        double[] ret = new double[f.length - 1];

        for(int i = 0;i < ret.length; ++i) {
            ret[i] = f[i + 1] - f[i];
        }

        return ret;
    }


    /**
     * Calculates the derivative of the function described by a discrete set of points.
     * Uses forward differences at the ends and central differences in the central region.
     * 
     * @param x The points at which the function values are given.
     * @param f The function values
     * @return The derivative at the given points.
     */
    public static final double[] der(double[] x, double[] f) {
        double[] ret = new double[x.length];

        ret[0] = derf(f[0], f[1], x[1] - x[0]);
        ret[ret.length - 1] = derf(f[ret.length - 2], f[ret.length - 1], x[ret.length - 1] - x[ret.length - 2]);

        if (ret.length == 2)
            return ret;

        for(int i = 1;i < ret.length - 1; ++i) {
            ret[i] = der(f[i - 1], f[i + 1], x[i + 1] - x[i]);
        }

        return ret;
    }


    /**
     * Calculates the dot product between two 3D vectors.
     *
     * @param a First vector
     * @param b Second vector.
     * @return The dot product between a and b.
     */
    public static final double dot(double[] a, double[] b) {

        return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
    }


    /**
     * Calculates the length of a N-D vector.
     * 
     * @param a The vector.
     * @return The length of the given vector.
     */
    public static final double veclength(double[] a) {
        double sum2 = 0;
        for(int i = 0;i < a.length; ++i) {
            sum2 += a[i] * a[i];
        }

        return Math.sqrt(sum2);
    }


    /**
     * Calculates the cross product between two 3D vectors.
     *
     * @param a First vector
     * @param b Second vector.
     * @return The cross product between a and b.
     */
    public static final double[] cross(double[] a, double[] b) {

    	return new double[] { a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0] };
    }

    public static final double[] reNorm(double a[]){
        double l = FastMath.sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
        a[0] /= l; a[1] /= l; a[2] /= l;
        return a;
    }
       

    /**
     * Calculates the shortest distance between a line and a point.
     *
     * @param a First vector on line
     * @param b Second vector on line.
     * @param c Point for which distance to line is sought.
     * @return The shortest distance between c and the line described by a and b.
     */
    public static final double distLinePoint(double[] a, double[] b, double[] c) {
        double[] ab = new double[] { b[0] - a[0], b[1] - a[1], b[2] - a[2] };
        double[] ac = new double[] { c[0] - a[0], c[1] - a[1], c[2] - a[2] };
        double[] abxac = cross(ab, ac);

        return Math.sqrt(dot(abxac, abxac) / dot(ab, ab));
    }
       
    
    /**
     * Returns the shortest distance from the point c to the line a->b.
     * If the point is off the ends, the point returned will be the nearest of a/b.
     * 
     * @param ax x-coordinate for point a
     * @param ay y-coordinate for point a
     * @param bx x-coordinate for point b
     * @param by y-coordinate for point b
     * @param cx x-coordinate for point c
     * @param cy y-coordinate for point c
     * 
     * @return The signed shortest distance from the point c to the line a->b.
     */
    public static double distLinePoint2DSigned(double ax, double ay, double bx, double by, double cx, double cy){
        
        double magAB2 = (bx-ax)*(bx-ax) + (by-ay)*(by-ay);
        double ABdotAC = (bx-ax)*(cx-ax) + (by-ay)*(cy-ay);
        double ABxAC = ((bx-ax)*(ay-cy) - (by-ay)*(ax-cx) );
        
        double fracDist = ABdotAC / magAB2; //frcation dist along AB of closest point to C
        
        if(fracDist < 0) //off front, just +/-|C-A|
            return ((ABxAC > 0) ? 1 : -1) * Math.sqrt((cx-ax)*(cx-ax) + (cy-ay)*(cy-ay));
        else if(fracDist > 1) //off end, just +/-|C-B|
            return ((ABxAC > 0) ? 1 : -1) * Math.sqrt((cx-bx)*(cx-bx) + (cy-by)*(cy-by));
        else //in the middle, return closest dist: |AB ^ AC| / |AB|
            return  ABxAC / Math.sqrt(magAB2);
        
         // d = ((bx-ax)(ay-cy) - (ax-cx)(by-ay)) / sqrt((bx-ax)^2 + (by-ay)^2 );
    }

    /**
     * Returns the nearest distance between a piecewise linear curve and a point.
     * 
     * @param x Curve x-coordinates
     * @param y Curve y-coordinates 
     * @param cx Point x
     * @param cy Point y
     * @return The shortest distance to the 
     */
    public static double distCurvePoint2D(double[] x, double[] y, double cx, double cy) {
        double minDistance = Double.MAX_VALUE;
        for(int i=0;i<x.length-1;++i) {
            double dist = Math.abs(distLinePoint2DSigned(x[i], y[i], x[i+1], y[i+1], cx, cy));
            if (dist < minDistance) minDistance = dist;
        }
        
        return minDistance;
    }

    /**
     * Returns the intersection point of the two line segments. If the lines are parallell or coincidential,
     * null is returned, otherwise the distance from p1 resp p3 to
     * p2 resp p4 is returned, normalised so that a distance < 0 or > 1 indicates intersection is outside
     * the given ends of the line segments.
     * 
     * The intersection point will be
     *  
     *      x = p1[0] + ret[0]*(p2[0]-p1[0])
     *      y = p1[1] + ret[0]*(p2[1]-p1[1])
     * 
     * Taken from http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
     * 
     * @param p1 First point of first line
     * @param p2 Second point of first line
     * @param p3 First point of second line
     * @param p4 Second point of second line
     * @return null if the lines are parallell or coincidential, otherwise the distance from p1 resp p3 to
     * p2 resp p4 is returned, normalised so that a distance < 0 or > 1 indicates intersection is outside
     * the given ends of the line segments.
     */
    public static final double[] intersectionTwoLineSegments2D(double[] p1, double[] p2, double[] p3, double[] p4) {
        double[] ret = new double[2];

        double d = (p4[1] - p3[1]) * (p2[0] - p1[0]) - (p4[0] - p3[0]) * (p2[1] - p1[1]);
        if (d == 0)
            return null;

        double n = (p4[0] - p3[0]) * (p1[1] - p3[1]) - (p4[1] - p3[1]) * (p1[0] - p3[0]);
        ret[0] = n / d;
        n = (p2[0] - p1[0]) * (p1[1] - p3[1]) - (p2[1] - p1[1]) * (p1[0] - p3[0]);
        ret[1] = n / d;

        return ret;
    }


    /** intersectionTwoLineSegments2D - different calling.
     * Test if the line (x1,y1)-(x2,y2) intersects the line (x3,y3)-(x4,y4) */
    public static final boolean intersectionTwoLineSegments2D(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double hitCoord[]) {
        double ua, ub;

        ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / (((y4 - y3) * (x2 - x1)) - ((x4 - x3) * (y2 - y1)));
        ub = ((x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1)) / (((y2 - y1) * (x4 - x3)) - ((x2 - x1) * (y4 - y3)));

        if (hitCoord != null) {
            hitCoord[0] = x1 + ua * (x2 - x1);
            hitCoord[1] = y1 + ua * (y2 - y1);
        }

        return (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1);
    }
    
    
    /**
     * Returns all intersection points between a given line segment, defined by (x1,y1)-(x2,y2) and a polygon.
     * The order of the intersection points returned is along the given boundary.
     * 
     * @param bx x-coordinates for the polygon boundary
     * @param by y-coordinates for the polygon boundary
     * @param x1 Start x-coordinate of line segment
     * @param y1 Start y-coordinate of line segment
     * @param x2 End x-coordinate of line segment
     * @param y2 End y-coordinate of line segment.
     * @return ret[0][i] is the x-coordinate of the i:th intersection point, ret[1][i] the corresponding y-coordinate
     */
    public static final double[][] intersectionLineSegmentPolygon2D(double[] bx, double[] by, double x1, double y1, double x2, double y2) {
        DynamicDoubleArray xIntersect = new DynamicDoubleArray();
        DynamicDoubleArray yIntersect = new DynamicDoubleArray();
        double[] hitCoord = new double[2];
        for(int i=0;i<bx.length-1;++i) {
            if (intersectionTwoLineSegments2D(x1, y1, x2, y2, bx[i], by[i], bx[i+1], by[i+1], hitCoord)) {
                xIntersect.add(hitCoord[0]);
                yIntersect.add(hitCoord[1]);
            }
        }
        
        return new double[][] { xIntersect.getTrimmedArray(), yIntersect.getTrimmedArray() };
    }
    
    /**
     * Returns all intersection points between a given line segment, defined by (x1,y1)-(x2,y2) and a polygon.
     * The returned intersection points will be sorted so that the intersection points nearest to the starting point
     * of the line segment will be returned first.
     * 
     * @param bx x-coordinates for the polygon boundary
     * @param by y-coordinates for the polygon boundary
     * @param x1 Start x-coordinate of line segment
     * @param y1 Start y-coordinate of line segment
     * @param x2 End x-coordinate of line segment
     * @param y2 End y-coordinate of line segment.
     * @return ret[0][i] is the x-coordinate for the i:th intersection point, ret[1][i] te y-coordinate for the i:th intersection point
     */
    public static final double[][] intersectionLineSegmentPolygon2DOrderedAlongLineSegment(double[] bx, double[] by, double x1, double y1, double x2, double y2) {
        double[][] points = intersectionLineSegmentPolygon2D(bx, by, x1, y1, x2, y2);       
        int numIntersections = points[0].length;
        
        double[] dist2 = new double[numIntersections];
        for(int i=0;i<numIntersections;++i) {
            dist2[i] = (points[0][i]-x1)*(points[0][i]-x1)+(points[1][i]-y1)*(points[1][i]-y1);
        }
        
        int[] index = new int[numIntersections];
        quicksort(dist2, index);
        
        double[][] pointsSorted = new double[2][numIntersections];
        for(int i=0;i<numIntersections;++i) {
            pointsSorted[0][i] = points[0][index[i]];
            pointsSorted[1][i] = points[1][index[i]];
        }
  
        return pointsSorted;
    }

    
    public static final double GSPROPORTION = 0.38196601125011;


    /**
     * Updates the bracketing triple of a minimum of a function f
     * using golden section search given
     * an initial bracketing triple xt[0] < xt[1] < xt[2] and
     * corresponding function values ft[0], ft[1], ft[2]
     * such that ft[1] < ft[0] and ft[1] < ft[2].
     *
     * On return, best estimate of min(f(x)) will be at x=xt[1].
     * The corresponding function values will be updated by the method.
     *
     * The method should simply be called repeatedly until desired convergence
     * has been reached.
     *
     * See http://www.algorithmrepository.org
     *
     * @param xt Bracketing triple of minimum of function f
     * @param ft Function values at the corresponding values of the triple.
     * @param f  The function to be minimized.
     */
    public static final void gssearch(double[] xt, double[] ft, IFunction f) {
        double a = xt[0], b = xt[1], c = xt[2], x, fx;
        if (b - a > c - b) {
            x = b - GSPROPORTION * (b - a);
            fx = f.eval(x);
            if (fx < ft[1]) {
                xt[2] = xt[1];
                ft[2] = ft[1];
                xt[1] = x;
                ft[1] = fx;
                return;
            }
            else {
                xt[0] = x;
                ft[0] = fx;
                return;
            }
        }
        else {
            x = b + GSPROPORTION * (c - b);
            fx = f.eval(x);
            if (fx < ft[1]) {
                xt[0] = xt[1];
                ft[0] = ft[1];
                xt[1] = x;
                ft[1] = fx;
                return;
            }
            else {
                xt[2] = x;
                ft[2] = fx;
                return;
            }
        }
    }


    /**
     *
     * Updates the bracketing triple of a minimum of a function f using
     * parabolic interpolation given
     * an initial bracketing triple xt[0] < xt[1] < xt[2] and
     * corresponding function values ft[0], ft[1], ft[2]
     * such that ft[1] < ft[0] and ft[1] < ft[2].
     *
     * The method is not guaranteed to find a minimum point. In case the
     * point found is not lower than the previous best estimate (in x[1]), no
     * change will be made to the triplet and the method will return
     * <code>false</code>
     *
     * On return, best estimate of min(f(x)) will be at x=xt[1].
     * The corresponding function values will be updated by the method.
     *
     * See http://www.algorithmrepository.org
     *
     * @param xt Bracketing triple of minimum of function f
     * @param ft Function values at the corresponding values of the triple.
     * @param f  The function to be minimized.
     * @return <code>true</code> if a lower point was found,
     * <code>false</code> if the search failed.
     */
    public static final boolean parabolicsearch(double[] xt, double[] ft, IFunction f) {
        double a = xt[0], b = xt[1], c = xt[2];
        double fbc = ft[1] - ft[2], fba = ft[1] - ft[0];
        double denom = (b - a) * fbc - (b - c) * fba;

        if (denom == 0)
            return false;

        double x = b - 0.5 * ((b - a) * (b - a) * fbc - (b - c) * (b - c) * fba) / denom;
        double fx = f.eval(x);

        if (fx < ft[1]) {
            xt[1] = x;
            ft[1] = fx;
            return true;
        }
        else {
            return false;
        }
    }



    /**
     * Calculates the definite integral of a function using the trapezoidal rule.
     *
     * See http://www.algorithmrepository.org
     *
     * @param f Array of equidistant function values with f[0]= f(a) and f[n-1] = f(b)
     * @param dx Distance between consecutive function evaluations
     *
     * @return The integral of the given function using the trapezoidal rule.
     */
    public static final double trapz(double[] f, double dx) {
        int n = f.length;
        double sum = 0.5 * (f[0] + f[n - 1]);
        for(int i = 1;i < n - 1; ++i) {
            sum += f[i];
        }

        return sum * dx;
    }
    
    
    /**
     * Calculates the definite integral of a function using the trapezoidal rule.
     *
     * See http://www.algorithmrepository.org
     *
     * @param f Array of function values with f[0]= f(a) and f[n-1] = f(b)
     * @param x Positions at which the function values are evaluated, need not be uniformly distributed.
     *
     * @return The integral of the given function using the trapezoidal rule.
     */
    public static final double trapz(double[] f, double[] x) {
        int n = f.length;
        double sum = 0;
        for(int i = 0;i < n - 1; ++i) {
            sum += 0.5*Math.abs((x[i+1] - x[i]))*(f[i+1] + f[i]);
        }
 
        return sum;
    }

    /**
     * Based on trapz and adapted for non equidistant function values
     * @param x Array of not equidistant function domain values 
     * @param f Array of not equidistant function values
     *
     * @return The integral of the given function using the trapezoidal rule.
     */
    public static final double trapzNotEquidist(double[] x, double[] f) {
        int n = f.length;
        double[] dist = new double[n-1];
        for (int i=0; i<dist.length; i++) dist[i] = x[i+1] - x[i];

        double sum = 0;

        for(int i = 1;i < n - 1; ++i) sum += f[i] * 0.5*(dist[i-1]+dist[i]);
        
        return sum + f[0]*0.5*dist[0] + f[n-1]*0.5*dist[n-2];
    }

    
    /**
     * Calculates the poles (positions at which the function should be evaluated) and weights (the weight of
     * each function value) for Gauss-Legendre integrations of the form:
     * 
     *      Integral(f, from, to) = sum(weight_i*f(pole_i))
     *  
     * where the integration limits are [-1,1]. The poles and weights can be transformed to arbitrary limits 
     * using the transformGaussLegendrePolesAndWeights() method. The reason there is a split between two method
     * calls is that the calculation of the poles and weights is computationally intensive, but not the transformation
     * of the limits. The poles and weights for the [-1,1] limits can be calculated once and stored away, only
     * calling the transformGaussLegendrePolesAndWeights() when the limits are not [-1,1].
     * 
     * The weights and poles for Gauss-Legendre integratino are chosen in such a way as to make the approximation
     * *exact* for any polynomial of a degree < = 2n-1, where n is the size of the weight
     * and poles vectors (the order). 
     * 
     * Based on a C++ code by Andreas Werner.
     * 
     * @param order The number of terms in the Gauss-Legendre integral approximation.
     * @return ret[0][i] is the i:th pole, ret[1][i] is the i:th weight.
     */
    public static final double[][] createGaussLegendrePolesAndWeights(int order) {
        double[] poles = new double[order];
        double[] weights = new double[order];

        double[][] ja = new double[order][];
        double[][] jv = new double[order][];
        for(int i = 0;i < order;i++ )
            ja[i] = new double[i + 1];
        for(int k = 0;k < order;k++ )
            jv[k] = new double[order];

        for(int k = 0;k < order - 1;k++ ) {
            poles[k] = (k + 1) / Math.sqrt(4.0 * (k + 1) * (k + 1) - 1.0);
        }
        for(int k = 0;k < order;k++ ) {
            for(int i = 0;i <= k;i++ ) {
                if (i == k - 1)
                    ja[k][i] = poles[i];
                else
                    ja[k][i] = 0.0;
            }
        }

        double PRECISION = 15;
        double delta = Math.pow(10.0, ( -PRECISION)); // static in C++ version
        double eps = delta; // static in C++ version

        for(int i = 0;i < order;i++ ) {
            jv[i][i] = 1;
            for(int j = 0;j < order;j++ )
                if (i != j)
                    jv[i][j] = 0;
        }

        int k = 0;
        double residuum = Double.MAX_VALUE;
        double sum = 0;

        do {
            for(int p = 0;p < order - 1;p++ ) {
                for(int q = p + 1;q < order;q++ ) {
                    if (Math.abs(ja[q][p]) > eps) {
                        double teta = (ja[q][q] - ja[p][p]) / (2.0 * ja[q][p]);
                        double t = 1.0;

                        if (Math.abs(teta) > delta) {
                            if (teta > 0.0)
                                t = 1.0 / (teta + Math.sqrt(teta * teta + 1.0));
                            else
                                t = 1.0 / (teta - Math.sqrt(teta * teta + 1.0));
                        }
                        double c = 1.0 / Math.sqrt(1.0 + t * t);
                        double s = c * t;
                        double r = s / (1.0 + c);

                        ja[p][p] -= t * ja[q][p];
                        ja[q][q] += t * ja[q][p];
                        ja[q][p] = 0;
                        for(int j = 0;j < p;j++ ) {
                            double g = ja[q][j] + r * ja[p][j];
                            double h = ja[p][j] - r * ja[q][j];

                            ja[p][j] -= s * g;
                            ja[q][j] += s * h;
                        }
                        for(int i = p + 1;i < q;i++ ) {
                            double g = ja[q][i] + r * ja[i][p];
                            double h = ja[i][p] - r * ja[q][i];

                            ja[i][p] -= s * g;
                            ja[q][i] += s * h;
                        }
                        for(int i = q + 1;i < order;i++ ) {
                            double g = ja[i][q] + r * ja[i][p];
                            double h = ja[i][p] - r * ja[i][q];

                            ja[i][p] -= s * g;
                            ja[i][q] += s * h;
                        }
                        for(int i = 0;i < order;i++ ) {
                            double g = jv[i][q] + r * jv[i][p];
                            double h = jv[i][p] - r * jv[i][q];

                            jv[i][p] -= s * g;
                            jv[i][q] += s * h;
                        }
                    }
                }
            }
            for(int i = 1;i < order;i++ ) {
                for(int j = 0;j < i;j++ ) {
                    sum += ja[i][j] * ja[i][j];
                }
            }
            if (Math.abs(residuum - (2.0 * sum - eps * eps)) < eps)
                k++ ;
            residuum = 2.0 * sum - eps * eps;
        } while ((2.0 * sum) > (eps * eps) && k < 5);

        for(int j = 0;j < order;j++ ) {
            weights[j] = 2.0 * jv[0][j] * jv[0][j];
            poles[j] = ja[j][j];
        }

        return new double[][] { poles, weights };

    }


    /**
     * Transforms Gauss-Legendre poles and weights that have been created for the limits [-1,1] to other limits.
     * The Gauss-Legendre poles and weights for the [-1,1] limits can be created by the function
     * createGaussLegendrePolesAndWeights().
     *   
     * @param pw pw[0] should contain the poles and pw[1] the weights created for the limits [-1,1].
     * @param lowerLimit The new lower limit for the integration.
     * @param upperLimit The new upper limit for the integration.
     * @return ret[0][i] is the i:th pole, ret[1][i] is the i:th weight.
     */
    public static final double[][] transformGaussLegendrePolesAndWeights(double[][] pw, double lowerLimit, double upperLimit) {
        int n = pw[0].length;
        double[] poles = pw[0];
        double[] weights = pw[1];
        double d = (upperLimit - lowerLimit) / 2.0;

        double[] x = new double[n];
        double[] w = new double[n];

        for(int i = 0;i < n;i++ ) {
            x[i] = d * (poles[i] + 1.0) + lowerLimit;
            w[i] = d * weights[i];
        }

        return new double[][] { x, w };
    }



    /**
     * Does one bisection division to narrow down
     * the interval where the root of a function is situated.
     *
     * The new bracket and corresponding function values are returned in
     * the given arrays so that the function can be called repeatedly until desired
     * convergence. The best estimate of the position of the root will be returned in
     * xv[1] (see below).
     *
     * @param xv A double[3] array containing left, middle and right positions. Root should be between left and right bounds.
     * @param fv A double[3] array containing the corresponding function values.
     * @param f The function whose root is sought.
     */
    public static final void bisect(double[] xv, double[] fv, IFunction f) {
        if (fv[0] * fv[1] < 0) {
            xv[2] = xv[1];
            fv[2] = fv[1];
        }
        else {
            xv[0] = xv[1];
            fv[0] = fv[1];
        }
        xv[1] = (xv[0] + xv[2]) / 2.0;
        fv[1] = f.eval(xv[1]);
    }

    /**
     * The right hand side of dx/dt=f(x,t), where x and f are vectors. 
     * 
     */
    public static interface ODEFunction {
        /**
         * Defines the right hand side of the ODE system dx/dt=f(x,t),
         * where x and f are vectors.
         * 
         * @param x The current value of the x-vector.
         * @param t The current time.
         * @param f The value of f(x,t) should be returned in this
         * array that will be allocated on call.
         */
        void value(double[] x, double t, double[] f);
    }


    /**
     * Takes one step in solving dx/dt=f(x,t) where x and f are vectors, using
     * the Runge-Kutta (RK4) method.
     * 
     * On return, the current value of x(t) can be found in the x-array, and the current
     * value of t as first element in the t-array.
     * 
     * Reusing arrays in this way makes it easy to call the method iteratively,
     * possibly changing the step size dynamically outside the internal
     * Runge-Kutta updating.
     * 
     * Example: Solves dx/dt=x (x0=1,t=0 gives x(t)=exp(t))
     * 
     *  double h = 0.001;
     *  int numPoints = 1000;
     *  double[] xcur = new double[] { 1.0 };
     *  double[] tcur = new double[] { 0.0 };
     *  double[][] storage = new double[5][1];
     *  double[] t = new double[numPoints];
     *  double[] x = new double[numPoints];
     *    
     *  ODEFunction f = new ODEFunction() {          
     *       public void value(double[] x, double t, double[] f) {
     *           f[0] = x[0];
     *       }
     *  };
     *   
     *  t[0] = tcur[0]; x[0] = xcur[0];
     *  for(int i=1;i<numPoints;++i) {
     *        // step size h could be changed dynamically here              
     *        tcur[0] = h*(i-1); // Alternatively, to reduce cumulative round-off errors on t.
     *        rungeKuttaStep(f,h,xcur,tcur,storage);             
     *        
     *        // Save results
     *        t[i] = tcur[0]; x[i] = xcur[0];
     *  }
     * 
     * @param xcur Contains the current value of x(t), and on return contains the
     * updated position (at t+h) after one Runge-Kutta iteration. Should contain the
     * starting x on the first call.
     * @param tcur An array of length 1 that contains the current time. Should contain
     * the starting time on first call.
     * @param h the step size for the next iteration.
     * @param storage A double vector of dimension storage[5][dim] that is used internally for storage of temporary
     * variables. Does not need to be preserved between function calls, but must be allocated to [5][dim] where
     * dim is the number of equations in the equation system.
     */
    public static final void rungeKuttaStep(ODEFunction f, double h, double[] xcur, double[] tcur, double[][] storage) {
        int dim = xcur.length;
        for(int i = 0;i < dim; ++i) {
            storage[0][i] = xcur[i];
        }
        double t = tcur[0];

        // c1
        f.value(xcur, t, storage[1]);

        // c2
        for(int i = 0;i < dim; ++i) {
            xcur[i] = storage[0][i] + h / 2.0 * storage[1][i];
        }
        f.value(xcur, t + h / 2.0, storage[2]);

        // c3
        for(int i = 0;i < dim; ++i) {
            xcur[i] = storage[0][i] + h / 2.0 * storage[2][i];
        }
        f.value(xcur, t + h / 2.0, storage[3]);

        // c4
        for(int i = 0;i < dim; ++i) {
            xcur[i] = storage[0][i] + h * storage[3][i];
        }
        f.value(xcur, t + h, storage[4]);

        for(int i = 0;i < dim; ++i) {
            xcur[i] = storage[0][i] + h / 6.0 * (storage[1][i] + 2 * storage[2][i] + 2 * storage[3][i] + storage[4][i]);
        }

        tcur[0] = t + h;
    }


    /**
     * Returns the solution to the ODE system defined by the supplied
     * ODEFunction interface.
     * 
     * Example:
     * 
     * ODEFunction f = new ODEFunction() {          
     *       public void value(double[] x, double t, double[] f) {
     *           f[0] = x[0];
     *       }
     *  };
     *  double[][] ret = rungeKutta(f, new double[] { 1.0 }, 0.0, 0.001, 1000);
     *
     * @param f Defines the right hand side of dx/dt=f(x,t). See ODEFunction.
     * @param xinit The initial value of x.
     * @param tinit The initial value of t.
     * @param h The step size.
     * @param numPoints The number of points.
     * @return ret[0][i] is the i:th time value of x(t). ret[j][i] is xj(t), the value of element
     * j of the vector x(t) at the i:th time step.
     */
    public static final double[][] rungeKutta(ODEFunction f, double[] xinit, double tinit, double h, int numPoints) {
        int dim = xinit.length;
        double[][] ret = new double[dim + 1][numPoints];

        double[][] storage = new double[5][dim];
        double[] xcur = new double[dim];
        double[] tcur = new double[1];
        ret[0][0] = tinit;
        tcur[0] = tinit;
        for(int j = 0;j < dim; ++j) {
            ret[j + 1][0] = xinit[j];
            xcur[j] = xinit[j];
        }

        for(int i = 1;i < numPoints; ++i) {
            tcur[0] = h * (i - 1);
            rungeKuttaStep(f, h, xcur, tcur, storage);
            ret[0][i] = tcur[0];
            for(int j = 0;j < dim; ++j) {
                ret[j + 1][i] = xcur[j];
            }
        }


        return ret;
    }


    /**
     * Returns the rotation matrix corresponding to a rotation
     * theta radians around an axis defined by the vector v.
     * v must be normalised.
     *
     * See http://www.algorithmrepository.org
     *
     * @param v The normalised direction of the axis of rotation.
     * @param theta The angle (in radians) of rotation around
     * the axis.
     * @return The rotation matrix for this transformation. A
     * multiplication of the rotation matrix with a column
     * vector will give a new vector rotated an angle theta
     * around the given axis.
     */
    public static final double[][] rotationMatrix(double[] v, double theta) {
        double[][] ret = new double[3][3];
        double vx = v[0], vy = v[1], vz = v[2];
        double c = Math.cos(theta);
        double s = Math.sin(theta);
        double ic = 1 - c;

        ret[0][0] = c + ic * vx * vx;
        ret[0][1] = ic * vx * vy - s * vz;
        ret[0][2] = ic * vx * vz + s * vy;
        ret[1][0] = ic * vy * vx + s * vz;
        ret[1][1] = c + ic * vy * vy;
        ret[1][2] = ic * vy * vz - s * vx;
        ret[2][0] = ic * vz * vx - s * vy;
        ret[2][1] = ic * vz * vy + s * vx;
        ret[2][2] = c + ic * vz * vz;

        return ret;
    }


    /**
     * Creates a rotation matrix from three rotation angles. The rotation matrix
     * is created by first rotating an angle thetay arond the y-axis, then
     * an angle thetax around the x-axis, then an angle thetaz around the z-axis.
     * A rotation is positive if it turns a vector in the anti-clockwise
     * direction when looking down the rotation axis from the top (this
     * is the most common convention for the angle in a cylindrical coordinate
     * system).
     *
     * See http://www.algorithmrepository.org
     *
     * @param thetax Angle (in radians) to rotate around x-axis (see
     * description above for rotation order)
     * @param thetay Angle (in radians) to rotate around y-axis (see
     * description above for rotation order)
     * @param thetaz Angle (in radians) to rotate around z-axis (see
     * description above for rotation order)
     * @return The rotation matrix corresponding to the above specification of
     * the rotation.
     */
    public static final double[][] rotationMatrix(double thetax, double thetay, double thetaz) {
        double[][] ret = new double[3][3];
        double cx = Math.cos(thetax), cy = Math.cos(thetay), cz = Math.cos(thetaz);
        double sx = -Math.sin(thetax), sy = -Math.sin(thetay), sz = -Math.sin(thetaz);

        ret[0][0] = cz * cy + sz * sx * sy;
        ret[0][1] = sz * cx;
        ret[0][2] = -cz * sy + sz * sx * cy;
        ret[1][0] = -sz * cy + cz * sx * sy;
        ret[1][1] = cz * cx;
        ret[1][2] = sz * sy + cz * sx * cy;
        ret[2][0] = cx * sy;
        ret[2][1] = -sx;
        ret[2][2] = cx * cy;

        return ret;
    }


    /**
     * Calculates the rotation matrix corresponding to the rotation of the
     * given coordinate system. On multiplication of the returned rotation
     * matrix with a vector, the vector will be rotated as the coordinate
     * system is rotated in relation to the global system. It does not
     * do a coordinate transformation (the vectors components will still
     * be components in the global system, just rotated).
     *
     * See http://www.algorithmrepository.org
     *
     * @param axes The normalised orthogonal coordinate axes of the local system. The format is
     *              axes[0][0-2] x-axis
     *              axes[1][0-2] y-axis
     *              axes[2][0-2] z-axis
     * @return The rotation matrix for this coordinate transformation.
     */
    public static final double[][] rotationMatrix(double[][] axes) {
        double thetax = Math.asin(axes[1][2]);
        double thetay = -Math.atan2(axes[0][2], axes[2][2]);
        double thetaz = -Math.atan2(axes[1][0], axes[1][1]);

        return rotationMatrix(thetax, thetay, thetaz);
    }


    /**
     * Rotates the given vector in accordance with a previously
     * calculated rotation matrix by multiplying the rotation
     * matrix with the given vector.
     *
     * See http://www.algorithmrepository.org
     *
     * @param rot A previously calculated rotation matrix.
     * @param r The vector to be rotated.
     * @return The given vector rotated by the given rotation matrix.
     */
    public static final double[] rotateVector(double[][] rot, double[] r) {
        double[] ret = new double[3];
        ret[0] = rot[0][0] * r[0] + rot[0][1] * r[1] + rot[0][2] * r[2];
        ret[1] = rot[1][0] * r[0] + rot[1][1] * r[1] + rot[1][2] * r[2];
        ret[2] = rot[2][0] * r[0] + rot[2][1] * r[1] + rot[2][2] * r[2];

        return ret;
    }


    /**
     * Calculates the transformation matrix for a transformation of vectors
     * expressed in a global cartesian system to vectors expressed in a
     * system defined by three new orthonormal axes (here referred to as
     * the 'local' system).
     *
     * A vector rg expressed in the global coordinate system is transformed to a vector rl
     * in the local system through:
     *
     *      rl = trans*(rg-ol)
     *
     * where trans is the transformation matrix and ol the origo of the local system.
     *
     * See http://www.algorithmrepository.org
     *
     * (This is an *extreme* convenience method, since it only return a copy of
     * the original axes matrix - which is the transformation matrix!).
     *
     * @param axes The normalised orthogonal coordinate axes of the local system. The format is
     *              axes[0][0-2] x-axis
     *              axes[1][0-2] y-axis
     *              axes[2][0-2] z-axis
     * @return The transformation matrix for this coordinate transformation. See above for usage.
     */
    public static final double[][] transformationMatrix(double[][] axes) {
        double[][] ret = new double[3][];
        ret[0] = axes[0].clone();
        ret[1] = axes[1].clone();
        ret[2] = axes[2].clone();

        return ret;
    }


    /**
     * Transforms a vector r expressed in a global cartesian system, to its
     * representation in a local orthonormal system with specified origo and
     * rotation matrix:
     *
     *     rl = trans * (rg - origo)
     *
     * See http://www.algorithmrepository.org
     *
     * @param trans Transformation matrix for coordinate transformation
     * @param origo Origo of local system
     * @param rg Position vector represented in global systems components.
     * @return Corresponding position vector expressed in local systems components.
     */
    public static final double[] coordinateTransform(double[][] trans, double[] origo, double[] rg) {
        double[] rt = new double[] { rg[0] - origo[0], rg[1] - origo[1], rg[2] - origo[2] };
        return rotateVector(trans, rt);
    }


    /**
     * Transforms a vector rl expressed in a local cartesian system to second
     * (global) system. The transformation matrix given is the inverse of the
     * transformation matrix for transformation from global system to local
     * system. The origo is the origo of the local system expressed as coordinates
     * in the global system.
     *
     *     rg = trans^-1 * rl + origo
     *
     * See http://www.algorithmrepository.org
     *
     * @param transinv Inverse of transformation matrix for coordinate transformation from
     * the global system to the local system.
     * @param origo Origo of local system
     * @param rl Position vector represented in local systems components.
     * @return Corresponding position vector expressed in global systems components.
     */
    public static final double[] coordinateTransformInverse(double[][] transinv, double[] origo, double[] rl) {
        double[] ret = rotateVector(transinv, rl);
        ret[0] += origo[0];
        ret[1] += origo[1];
        ret[2] += origo[2];

        return ret;
    }

    /**
     * Multiplies two 3x3 matricies.
     *
     * @param m The 3x3 matrix to inverse.
     * @return The inverse of the given 3x3 matrix.
     */
    public static double[][] mul3x3(double[][] A, double[][] B) {
        
        return new double[][]{
                { A[0][0]*B[0][0] + A[0][1]*B[1][0] + A[0][2]*B[2][0],
                    A[0][0]*B[0][1] + A[0][1]*B[1][1] + A[0][2]*B[2][1],
                        A[0][0]*B[0][2] + A[0][1]*B[1][2] + A[0][2]*B[2][2], },
                { A[1][0]*B[0][0] + A[1][1]*B[1][0] + A[1][2]*B[2][0],
                    A[1][0]*B[0][1] + A[1][1]*B[1][1] + A[1][2]*B[2][1],
                        A[1][0]*B[0][2] + A[1][1]*B[1][2] + A[1][2]*B[2][2], },
                { A[2][0]*B[0][0] + A[2][1]*B[1][0] + A[2][2]*B[2][0],
                    A[2][0]*B[0][1] + A[2][1]*B[1][1] + A[2][2]*B[2][1],
                        A[2][0]*B[0][2] + A[2][1]*B[1][2] + A[2][2]*B[2][2], },
        };
    
    }
    
    /**
     * Calculates the inverse of a 3x3 matrix.
     *
     * See http://www.algorithmrepository.org
     *
     * @param m The 3x3 matrix to inverse.
     * @return The inverse of the given 3x3 matrix.
     */
    public static final double[][] inv3x3(double[][] m) {
        double[][] ret = new double[3][3];
        double deti = 1.0 / det3x3(m);

        ret[0][0] = deti * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
        ret[0][1] = deti * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);
        ret[0][2] = deti * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);

        ret[1][0] = deti * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);
        ret[1][1] = deti * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);
        ret[1][2] = deti * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);

        ret[2][0] = deti * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
        ret[2][1] = deti * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);
        ret[2][2] = deti * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);

        return ret;
    }


    /**
     * Calculates the determinant of a 3x3 matrix.
     *
     * See http://www.algorithmrepository.org
     *
     * @param m The 3x3 matrix whose determinant is sought.
     * @return The determinant of the given 3x3 matrix.
     */
    public static final double det3x3(double[][] m) {
        return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) - m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]) + m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]);
    }


    /**
     * Calculates the determinant of a 2x2 matrix.
     *
     * See http://www.algorithmrepository.org
     *
     * @param m The 2x2 matrix whose determinant is sought.
     * @return The determinant of the given 2x2 matrix.
     */
    public static final double det2x2(double[][] m) {
        return m[0][0] * m[1][1] - m[0][1] * m[1][0];
    }



    /**
     * Calculates the inverse of a 2x2 matrix.
     *
     * See http://www.algorithmrepository.org
     *
     * @param m The 2x2 matrix to inverse.
     * @return The inverse of the given 2x2 matrix.
     */
    public static double[][] inv2x2(double[][] m) {
        double[][] ret = new double[2][2];
        double deti = 1.0 / det2x2(m);

        ret[0][0] = deti * m[1][1];
        ret[0][1] = -deti * m[0][1];

        ret[1][0] = -deti * m[1][0];
        ret[1][1] = deti * m[0][0];

        return ret;
    }


    /**
     * Calculates the area of a truncated cone.
     * 
     * @return
     */
    public static double truncatedLateralConeArea(double R, double r, double height) {
        return Math.PI*(R+r)*Math.sqrt(height*height+(R-r)*(R-r));
    }

    /**
     * Implements hyperbolic arc sine.
     *
     * @param x The argument.
     * @return asinh(x)
     */
    public static final double asinh(double x) {
        return Math.log(x + Math.sqrt(1 + x * x));
    }
    
    /**
     * Determines if the path defined by points in x,y are describing a clockwise or
     * anti-clockwise movement. Only works for convex paths.
     * 
     * @param xy xy[0] are the x-coordinates of points along path, 
     *           xy[1] are the y-coordinates of points along path
     * @return true if the points describe a clockwise path, false if anti-clockwise.
     */
    public static boolean clockwisePath(double[][] xy) {
        double[] x = xy[0], y = xy[1];
        double x1 = x[1] - x[0];
        double y1 = y[1] - y[0];
        double EPS = 1e-12;
        
        // Find first non-parallell to first vector
        for(int i=1;i<x.length-1;++i) {
            double x2 = x[i+1] - x[i];
            double y2 = y[i+1] - y[i];       
            
            double cross = x1*y2 - y1*x2;
            if (Math.abs(cross) > EPS) { 
               return cross > 0 ? false : true;
            }
        }
        
        throw new RuntimeException("Could not determine direction in clockwisePath()");        
    }
    
    

    /**
     * Reduces resolution of a matrix (image) by averaging over a rectangular region 
     * binSizeX, binSizeY.
     * 
     * @param a Input matrix
     * @param binSizeColumns Bin size in the x-direction
     * @param binSizeRows Bin size in y-direction
     * @return The reduced mat
     */
    public static double[][] bin2D(double[][] a, int binSizeRows, int binSizeColumns) {
        return bin2D(a, binSizeRows, binSizeColumns, false);
    }

        
    /**
     * Reduces resolution of a matrix (image) by averaging over a rectangular region 
     * binSizeX, binSizeY.
     * 
     * @param a Input matrix
     * @param binSizeColumns Bin size in the x-direction
     * @param binSizeRows Bin size in y-direction
     * @param excludeNaN if true elements that are Double.NaN will not be part of the averaging.
     * @return The reduced mat
     */
    public static double[][] bin2D(double[][] a, int binSizeRows, int binSizeColumns, boolean excludeNaN) {
        if (binSizeRows == 1 && binSizeColumns == 1) return a;
        int binnedImageSizeX = a[0].length / binSizeColumns;
        int binnedImageSizeY = a.length / binSizeRows;
        double[][] ret = new double[binnedImageSizeY][binnedImageSizeX];
        for(int i=0;i<ret[0].length;++i) {
            for(int j=0;j<ret.length;++j) {
                double sum = 0;
                int numElementsInSum = 0;
                for(int k=0;k<binSizeRows;++k) {
                    for(int l=0;l<binSizeColumns;++l) {
                        double elem = a[i*binSizeRows+k][j*binSizeColumns+l];
                        if (!(excludeNaN && Double.isNaN(elem))) { 
                            sum += elem;
                            ++numElementsInSum;
                        }
                    }
                }
                ret[i][j] = numElementsInSum == 0 ? Double.NaN : sum / (double)(numElementsInSum);
            }
        }
        
        return ret;
    }
    
    
    /**
     * Find intersection point of a line and a plane,
     * see http://www.ipp.mpg.de/~cak/intersectionLinePlane/intersectionLinePlane.pdf
     *
     * @param PL  Position of a point on the line
     * @param VL  Vector along line, need not be normalised
     * @param PP  Position of a point on the plane
     * @param VP1 First vector on the plane
     * @param VP2 Second vector on the plane (not parallel to VP1)
     * @return Position of the intersection
     */
    public static final double[] intersectionLinePlane(double PL[], double VL[], double PP[], double VP1[], double VP2[]) {

        double A = VL[1]*VP1[0] - VL[0]*VP1[1]; double B = VL[1]*VP1[2] - VP1[1]*VL[2];
        double t2_up = A*(VL[2]*(PP[1]-PL[1]) - VL[1]*(PP[2]-PL[2])) - B*(VL[0]*(PP[1]-PL[1]) - VL[1]*(PP[0]-PL[0]));
        double t2_down = B*(VP2[1]*VL[0]-VP2[0]*VL[1]) - A*(VL[2]*VP2[1]-VP2[2]*VL[1]);
        double t2 = t2_up/t2_down;
        double t1 = (VL[2]*(PP[1]+t2*VP2[1]-PL[1]) - VL[1]*(PP[2]+t2*VP2[2]-PL[2]))/B;
        double t = (PP[0] + t1*VP1[0] + t2*VP2[0] - PL[0])/VL[0];

        return new double[] { PL[0] + t*VL[0], PL[1] + t*VL[1], PL[2] + t*VL[2]};
    }    
    


    /**
     * Returns 3D coordinates of grid points on a circle with given middle point, two vectors along the circle and the radius
     *
     * @param middlePoint Middle Point of the circle
     * @param orthVector1 First orthogonal vector to the normal of the circle
     * @param orthVector2 Second orthogonal vector to the normal of the circle and to orthVector1
     * @param radius Radius of the circle
     * @param nGridPointsPerDimension Number of grid points for each both dimensions. If 1, the circle middle point is returned.
     *
     * @return Grid points, ret[i][j] is the jth grid point for dimension i.
     */
    public static final double[][] gridPointsOnCircle(double middlePoint[], double[] normalVector, double radius, int nGridPointsPerDimension) {

        // If only one point per dimension, return the circle middle point
        if (nGridPointsPerDimension == 1) return new double[][]{new double[]{middlePoint[0]}, new double[]{middlePoint[1]}, new double[]{middlePoint[2]}};
        // a and b are coordinates along both orthogonal vectors, they are both zero in the middle point of the circle
        double da = 2 * radius / nGridPointsPerDimension;
        double db = da;

        double[][] perpVectors = perpVectors(normalVector, true);

        double[] perpVector1 = perpVectors[0];
        double[] perpVector2 = perpVectors[1];

        // Calculate the start point (lower left corner of the square with negative a and b)
        double xStart = middlePoint[0] - radius*perpVector1[0] - radius*perpVector2[0];
        double yStart = middlePoint[1] - radius*perpVector1[1] - radius*perpVector2[1];
        double zStart = middlePoint[2] - radius*perpVector1[2] - radius*perpVector2[2];

        // 3D points to return
        DynamicDoubleArray xValues = new DynamicDoubleArray(), yValues = new DynamicDoubleArray(), zValues = new DynamicDoubleArray();

        // Now go up with a and b from -r to +r
        for (int i=0; i<nGridPointsPerDimension; i++)
            for (int j=0; j<nGridPointsPerDimension; j++) {
                double x = xStart + (0.5+i)*da*perpVector1[0] + (0.5+j)*db*perpVector2[0];
                double y = yStart + (0.5+i)*da*perpVector1[1] + (0.5+j)*db*perpVector2[1];
                double z = zStart + (0.5+i)*da*perpVector1[2] + (0.5+j)*db*perpVector2[2];

                // Add this point only if it's closer to the middle point than the radius
                if (Algorithms.veclength(new double[]{x-middlePoint[0], y-middlePoint[1], z-middlePoint[2]}) <= radius) {
                    xValues.add(x);
                    yValues.add(y);
                    zValues.add(z);
                }
            }
        xValues.trim();
        yValues.trim();
        zValues.trim();
        return new double[][] {xValues.getArray(), yValues.getArray(), zValues.getArray()};
    }






    /**
     * Normalizes a vector of arbitrary dimension to the given length
     * @param vector Vector of arbitrary dimension to be normalized
     * @param length Desired length of the vector
     * @return Vector with the given length
     */
    public static final double[] normaliseVector(double[] vector, double length) {
        double[] normalisedVec = new double[vector.length];
        double vectorLength = 0;
        for (int i=0; i<vector.length; i++)
            vectorLength += vector[i]*vector[i];
        vectorLength = Math.sqrt(vectorLength);
        for (int i=0; i<vector.length; i++)
            normalisedVec[i] = vector[i] / vectorLength * length;
        return normalisedVec;
    }

    /**
     * Normalise vector to unit length.
     * 
     * @param vector
     * @return
     */
    public static final double[] normaliseVector(double[] vector) {
        return normaliseVector(vector, 1);
    }


    /**
     * Calculates two perpendicular vectors to a given vector.
     * @param vector
     * @return
     */
    @Deprecated /** Use perpVectorsOriented instead */
    public static double[][] perpVectors(double[] vector, boolean normaliseToOne) {

        double perpVectors[][] = new double[2][];

        // Try with an arbitrary vector (1,1,1)
        perpVectors[0] = Algorithms.cross(vector, new double[]{1e-3,1e-3,1e-3});

        // If vector and {1,1,1} are parallel, perpVectors[0] will be zero.
        // Try with adding 100 to the x coordinate, it must not be zero now
        if (Algorithms.veclength(perpVectors[0]) == 0) perpVectors[0] = Algorithms.cross(vector, new double[]{101e-3,1e-3,1e-3});

        perpVectors[1] = Algorithms.cross(vector, perpVectors[0]);

        if (normaliseToOne) {
            perpVectors[0] = normaliseVector(perpVectors[0], 1);
            perpVectors[1] = normaliseVector(perpVectors[1], 1);
        }

        return perpVectors;
    }

    
    /**
     * Calculates two perpendicular vectors to a given vector. The first one has no z component (on the (x,y) plane).
     * @param vector
     * @return [0][] Vector perpendicular to the given vector and lying on the (x,y) plane.
     * [1][] Vector perpedicular to the given vector and oriented as much as possible upwords (maximal z component)
     */
    public static double[][] perpVectorsOriented(double[] vector, boolean normaliseToOne) {

        double perpVectors[][] = new double[2][];

        // First vector is perpendicular to the given vector (dot product = 0) and z component = 0. Take x=1 for this vector (arbitrary choice).
        perpVectors[0] = new double[]{1, -vector[0]/vector[1], 0};

        // The second one is perpendicular to the both previous vectors
        perpVectors[1] = Algorithms.cross(vector, perpVectors[0]);
        
        // Check if the second vector points upwords (positive z), if not, redirect it
        if (perpVectors[1][2] < 0) {perpVectors[1][0] *= -1; perpVectors[1][1] *= -1; perpVectors[1][2] *= -1;}

        if (normaliseToOne) {
            perpVectors[0] = normaliseVector(perpVectors[0], 1);
            perpVectors[1] = normaliseVector(perpVectors[1], 1);
        }

        return perpVectors;
    }

    
    /**
     * Adjust phase jumps by a multiple of 2*pi if phase changes more than PI.
     * 
     * @param phi The phase angle.
     * @return The continous, without jumps, phase changes.
     */
    public static final double[] unwrap(double[] phi) {
        double[] ret = new double[phi.length];

        ret[0] = phi[0];
        for(int i = 0;i < phi.length - 1; ++i) {
            double dphi = phi[i + 1] - phi[i];
            while (dphi > Math.PI) {
                dphi -= 2 * Math.PI;
            }
            while (dphi < -Math.PI) {
                dphi += 2 * Math.PI;
            }
            ret[i + 1] = ret[i] + dphi;
        }

        return ret;
    }


    /**
     * Adds up the given elements, element by element.
     * 
     * @param v The given values to add.
     * @return The cumulative sum.
     */
    public static final double[] cumsum(double[] v) {
        double[] ret = new double[v.length];

        ret[0] = v[0];
        for(int i = 1;i < ret.length; ++i) {
            ret[i] = ret[i - 1] + v[i];
        }

        return ret;
    }


    /**
     * Modified Bessel function, second kind, with
     * integer argument (order), corresponding to
     * nu = n+1/2 for the general modified Bessel function.
     * 
     * @param x Function argument
     * @param n Order, nu=n+1/2.
     * @return Bessel function of second kind of the argument
     */
    // public static double besselkn(double x, int n) {

    // }

    /**
     * Does a binary search (bisection) to find the index of the element in the
     * array x for which x[index] <= x < x[index+1], where x is monotonically
     * increasing.
     *
     * Adapted from numerical recipes ch. 3.4.
     *
     * @param x An array of monotonically increasing values.
     * @param xv The value whose nearest lower index is sought in the array.
     * @return The index that corresponds to x[index] <= x < x[index+1] or
     * -1 if x < x[0], x.length if x > x[x.length-1]
     */
    public static final int binarySearch(double[] x, double xv) {
        int num = x.length;
        if (xv > x[num - 1])
            return x.length;
        if (xv < x[0])
            return -1;

        int ju = num, jm, jl = -1;

        while (ju - jl > 1) {
            jm = (ju + jl) >> 1;
            if (xv >= x[jm]) {
                jl = jm;
            }
            else {
                ju = jm;
            }
        }
        if (xv > x[num - 1])
            return num;
        if (xv == x[0])
            return 0;
        if (xv == x[num - 1])
            return num - 2;

        return jl;
    }



    /**
     * Returns the contours for the given contour levels using a
     * marching squares algorithm.
     *  
     * @param xp An array of montonically increasing x-values for the grid points 
     * @param yp An array of montonically increasing y-values for the grid points
     * @param fp The function values at the grid points above
     * fp[i][j] is the function value at xp[i],yp[j].
     * @param cval The contour level value 
     * @return ret[lineSegNum][0][0], ret[lineSegNum][0][1] is (x1,y1)
     * and ret[lineSegNum][1][0], ret[lineSegNum][1][1] is (x2,y2)
     * for the line segment number lineSegNum.
     */
    public static final double[][][] contour(double[] xp, double[] yp, double[][] fp, double cval) {
        ArrayList<double[][]> lineSegs = new ArrayList<double[][]>();
        double[][][] ret;

        for(int i = 0;i < xp.length - 1; ++i) {
            for(int j = 0;j < yp.length - 1; ++j) {
                ret = contourSegment(xp[i], yp[j], xp[i + 1] - xp[i], yp[j + 1] - yp[j], fp[i][j], fp[i + 1][j], fp[i + 1][j + 1], fp[i][j + 1], cval, null);
                if (ret != null) {
                    for(int k = 0;k < ret.length; ++k) {
                        lineSegs.add(ret[k]);
                    }
                }
            }
        }

        return lineSegs.toArray(new double[lineSegs.size()][][]);
    }



    /**
     * Traces out a contour from a starting point, following the contour
     * line rather than finding all contour values on the grid.
     * 
     * The algorithm works in the following way (Moore Neighbourhood): 
     * First an attempt is made to go to the next expected box. 
     * If the contour goes through this cell, stay there,
     * otherwise go around that cell in the anticlockwise direction and
     * visit each such neighbourhood cell (there are eight of them) until
     * a cell is found that contains the contour. If no cell is found or
     * the cell found is a cell already traced out, the algorithm stops, 
     * returning the polygon so traced out.
     * 
     * If the polygon so traced out is not closed, an attempt is made
     * to redo the process from the starting point, trying to trace out
     * in the opposite direction. If this is possible, the two polygonal
     * segments are merged.
     * 
     * See http://www.algorithmrepository.org
     * 
     * @param xp An array of montonically increasing x-values for the grid points 
     * @param yp An array of montonically increasing y-values for the grid points
     * @param fp The function values at the grid points above
     * fp[i][j] is the function value at xp[i],yp[j].
     * @param ixstart The x-index of the lower left corner of the cell where contouring should start 
     * @param iystart The x-index of the lower left corner of the cell where contouring should start
     * @param cval The contour level value 
     * 
     * @return A polygon of the contour from the given starting point.
     * ret[0][i] is the ith x coordinate, ret[1][i] is the ith y-coordinate
     */
    public static final double[][] contourTrace(double[] xp, double[] yp, double[][] fp, int ixstart, int iystart, double cval) {
        double[][][] firstLineSegs = contourSegment(xp[ixstart], yp[iystart], xp[ixstart + 1] - xp[ixstart], yp[iystart + 1] - yp[iystart], fp[ixstart][iystart], fp[ixstart + 1][iystart], fp[ixstart + 1][iystart + 1], fp[ixstart][iystart + 1], cval, null);
        if (firstLineSegs == null)
            return null; // Contour does not go through starting cell
        DynamicDoubleArray x0 = new DynamicDoubleArray(100);
        DynamicDoubleArray y0 = new DynamicDoubleArray(100);
        DynamicDoubleArray x1 = new DynamicDoubleArray(100);
        DynamicDoubleArray y1 = new DynamicDoubleArray(100);
        boolean[][] visited = new boolean[fp.length][fp[0].length];
        double tol = 1e-12; // Points with coordinate values within tol are
                            // regarded as connected
        double[] x = null;
        double[] y = null;

        x0.add(firstLineSegs[0][0][0]);
        y0.add(firstLineSegs[0][0][1]);
        visited[ixstart][iystart] = true;
        singleSidedContourTrace(xp, yp, fp, ixstart, iystart, visited, cval, x0, y0);

        // Closed
        if (Math.abs(x0.getArray()[x0.size() - 1] - firstLineSegs[0][1][0]) < tol && Math.abs(y0.getArray()[y0.size() - 1] - firstLineSegs[0][1][1]) < tol) {
            x0.add(firstLineSegs[0][0][0]);
            y0.add(firstLineSegs[0][0][1]);
            x = x0.getTrimmedArray();
            y = y0.getTrimmedArray();
        }
        else {
            x1.add(firstLineSegs[0][1][0]);
            y1.add(firstLineSegs[0][1][1]);
            singleSidedContourTrace(xp, yp, fp, ixstart, iystart, visited, cval, x1, y1);

            double[] xp0 = x0.getTrimmedArray();
            double[] yp0 = y0.getTrimmedArray();
            double[] xp1 = x1.getTrimmedArray();
            double[] yp1 = y1.getTrimmedArray();

            x = new double[xp0.length + xp1.length];
            y = new double[xp0.length + xp1.length];
            for(int i = 0;i < xp0.length; ++i) {
                x[i] = xp0[xp0.length - i - 1];
                y[i] = yp0[yp0.length - i - 1];
            }
            for(int i = 0;i < xp1.length; ++i) {
                x[i + xp0.length] = xp1[i];
                y[i + yp0.length] = yp1[i];
            }
        }

        return new double[][] { x, y };
    }


    private static final void singleSidedContourTrace(double[] xp, double[] yp, double[][] fp, int ix, int iy, boolean[][] visited, double cval, DynamicDoubleArray x, DynamicDoubleArray y) {
        int n[] = new int[1];
        int[] ixv = new int[1], iyv = new int[1];
        double[][][] lineSegs;
        int iprevx = -1, iprevy = -1;
        double cx = x.getArray()[0];
        double cy = y.getArray()[0];
        double tol = 1e-12; // Points with coordinate values within tol are
                            // regarded as connected

        for(;;) {
            ixv[0] = ix;
            iyv[0] = iy;
            lineSegs = findSurrounding(xp, yp, fp, visited, cval, ixv, iyv, n, cx, cy);
            if (lineSegs == null) {
                break;
            }
            iprevx = ix;
            iprevy = iy;
            ix = ixv[0];
            iy = iyv[0];
            if (Math.abs(cx - lineSegs[0][0][0]) < tol && Math.abs(cy - lineSegs[0][0][1]) < tol) {
                cx = lineSegs[0][1][0];
                cy = lineSegs[0][1][1];
                x.add(cx);
                y.add(cy);
                visited[ix][iy] = true;
            }
            else if (Math.abs(cx - lineSegs[0][1][0]) < tol && Math.abs(cy - lineSegs[0][1][1]) < tol) {
                cx = lineSegs[0][0][0];
                cy = lineSegs[0][0][1];
                x.add(cx);
                y.add(cy);
                visited[ix][iy] = true;
            }
            else {
                break;
            }
        }
    }


    /**
     * Traces out a contour from a starting point, following the contour
     * line rather than finding all contour values on the grid.
     * 
     * The algorithm works in the following way (Moore Neighbourhood): 
     * First an attempt is made to go to the next expected box. 
     * If the contour goes through this cell, stay there,
     * otherwise go around that cell in the anticlockwise direction and
     * visit each such neighbourhood cell (there are eight of them) until
     * a cell is found that contains the contour. If no cell is found or
     * the cell found is a cell already traced out, the algorithm stops, 
     * returning the polygon so traced out.
     * 
     * If the polygon so traced out is not closed, an attempt is made
     * to redo the process from the starting point, trying to trace out
     * in the opposite direction. If this is possible, the two polygonal
     * segments are merged.
     * 
     * See http://www.algorithmrepository.org
     * 
     * @param xp An array of montonically increasing x-values for the grid points 
     * @param yp An array of montonically increasing y-values for the grid points
     * @param fp The function values at the grid points above
     * fp[i][j] is the function value at xp[i],yp[j].
     * @param ixstart The x-index of the lower left corner of the cell where contouring should start 
     * @param iystart The x-index of the lower left corner of the cell where contouring should start
     * @param cval The contour level value 
     * 
     * @return A polygon of the contour from the given starting point.
     * ret[0][i] is the ith x coordinate, ret[1][i] is the ith y-coordinate
     */
    /*
     * public static double[][] contourTrace(double[] xp, double[] yp,
     * double[][] fp, int ixstart, int iystart, double cval) {
     * DynamicDoubleArray x, y; DynamicDoubleArray x0 = new
     * DynamicDoubleArray(100); DynamicDoubleArray y0 = new
     * DynamicDoubleArray(100); DynamicDoubleArray x1 = new
     * DynamicDoubleArray(100); DynamicDoubleArray y1 = new
     * DynamicDoubleArray(100);
     * 
     * double[][][] lineSegs; boolean[][] visited = new
     * boolean[fp.length][fp[0].length]; int ix = ixstart, iy=iystart; int
     * iprevx = -1, iprevy = -1; int[] ixv = new int[1], iyv = new int[1]; int[]
     * n = new int[1]; double[] xa, ya; int numPoints; double tol = 1e-12; //
     * Points with coordinate values within tol are regarded as connected
     * 
     * lineSegs = contourSegment(xp[ix], yp[iy], xp[ix+1]-xp[ix],
     * yp[iy+1]-yp[iy], fp[ix][iy], fp[ix+1][iy], fp[ix+1][iy+1], fp[ix][iy+1],
     * cval,n); if (lineSegs == null) return null; // Contour does not go
     * through starting cell
     * 
     * double[][][] firstLineSegs = lineSegs; visited[ix][iy] = true;
     * System.out.println(n[0]);
     * 
     * x = x0; y = y0;
     * 
     * x.add(firstLineSegs[0][0][0]); y.add(firstLineSegs[0][0][1]);
     * 
     * // Do first in one direction that start again from starting point // in
     * opposite direction for(int direction=0;direction<=1;++direction) {
     * for(;;) { xa = x.getArray(); ya = y.getArray(); numPoints = x.size();
     * 
     * // Find candidate square switch (n[0]) { case 1: case 14: // Left-Bottom
     * if (iprevx == ix && iprevy == iy-1 && ix > 0) { iprevx = ix; ix -= 1; }
     * else if (iprevx == ix-1 && iprevy == iy && iy > 0) { iprevy = iy; iy -=
     * 1; } break; case 2: case 13: // Left-Top break; case 4: case 11: //
     * Top-Right break; case 7: case 8: // Bottom-Right break; case 3: case 12:
     * // Bottom-Top break; case 6: case 9: // Left-Right break; case 5: //
     * Left-Bottom, Top-Right ridge break; case 10: // Left-Top, Bottom-Right
     * ridge break; default: }
     * 
     * if (visited[ix][iy]) { lineSegs = null; } else { lineSegs =
     * contourSegment(xp[ix], yp[iy], xp[ix+1]-xp[ix], yp[iy+1]-yp[iy],
     * fp[ix][iy], fp[ix+1][iy], fp[ix+1][iy+1], fp[ix][iy+1], cval,n); }
     * 
     * if (lineSegs == null) { // Guess did not work, find surrounding cell
     * ixv[0] = ix; iyv[0] = iy; lineSegs = findSurrounding(xp, yp, fp, visited,
     * cval, ixv, iyv, n, xa[numPoints-1], ya[numPoints-1]); if (lineSegs ==
     * null) { // End of contour line if (direction == 0) { // Start again from
     * first point ix = ixstart; iy=iystart; iprevx = -1; iprevy = -1; } break;
     * } iprevx = ix; iprevy = iy; ix = ixv[0]; iy = iyv[0]; }
     * 
     * if (Math.abs(xa[numPoints-1]-lineSegs[0][0][0]) < tol &&
     * Math.abs(ya[numPoints-1]-lineSegs[0][0][1]) < tol) {
     * x.add(lineSegs[0][1][0]); y.add(lineSegs[0][1][1]); visited[ix][iy] =
     * true; } else if (Math.abs(xa[numPoints-1]-lineSegs[0][1][0]) < tol &&
     * Math.abs(ya[numPoints-1]-lineSegs[0][1][1]) < tol) {
     * x.add(lineSegs[0][0][0]); y.add(lineSegs[0][0][1]); visited[ix][iy] =
     * true; } else { // Not closed if (direction == 0) { // Start again from
     * first point ix = ixstart; iy=iystart; iprevx = -1; iprevy = -1; } break;
     * } } }
     * 
     * double[][] ret = new double[][] { x.getTrimmedArray(),
     * y.getTrimmedArray() }; return ret; }
     */


    /**
     *
     * Finds a cell surrounding a given cell: Going in the 
     * anti-clockwise direction, pick the first cell that contains
     * the contour value and is not previously visited. If no such
     * cell can be found the contour has ended. The new indicies are
     * returned if a cell is found, otherwise null is returned.  
     * 
     * @param xp 
     * @param yp 
     * @param fp
     * @param visited 
     * @param cval 
     * @param icx
     * @param icy
     * @return
     */
    public static final double[][][] findSurrounding(double[] xp, double[] yp, double[][] fp, boolean[][] visited, double cval, int icx[], int[] icy, int[] n, double prevx, double prevy) {
        int ix, iy;
        int numx = xp.length;
        int numy = yp.length;
        double[][][] lineSegs;
        double tol = 1e-12; // Points with coordinate values within tol are
                            // regarded as connected

        for(int i = 0;i < 8; ++i) {
            ix = icx[0] + corners[i][0];
            iy = icy[0] + corners[i][1];

            if (ix >= 0 && ix < numx - 1 && iy >= 0 && iy < numy - 1 && !visited[ix][iy]) {
                lineSegs = contourSegment(xp[ix], yp[iy], xp[ix + 1] - xp[ix], yp[iy + 1] - yp[iy], fp[ix][iy], fp[ix + 1][iy], fp[ix + 1][iy + 1], fp[ix][iy + 1], cval, n);
                if (n[0] == 0 || n[0] == 15) { // Contour does not go through
                                               // cell
                    visited[ix][iy] = true;
                    continue;
                }
                if (Math.abs(lineSegs[0][0][0] - prevx) < tol && Math.abs(lineSegs[0][0][1] - prevy) < tol) {
                    icx[0] = ix;
                    icy[0] = iy;
                    return lineSegs;

                }
                if (Math.abs(lineSegs[0][1][0] - prevx) < tol && Math.abs(lineSegs[0][1][1] - prevy) < tol) {
                    icx[0] = ix;
                    icy[0] = iy;
                    return lineSegs;
                }

            }

        }

        return null;
    }

    private static final int[][] corners = new int[][] { { 0, -1 }, { -1, -1 }, { -1, 0 }, { -1, 1 }, { 0, 1 }, { 1, 1 }, { 1, 0 }, { 1, -1 } };


    /**
     * A marching squares algorithm for a single grid cell.
     * 
     * Returns the line segments through the given grid cell that describes the 
     * contour through that cell. 
     * 
     * Based on Edward Angels 'Interactive computer graphics', 4th edition. 
     * 
     * See http://www.algorithmrepository.org
     * 
     * @param x00 x-coordinate of the lower left (0,0) corner of the cell 
     * @param y00 y-coordinate of the lower left (0,0) corner of the cell
     * @param dx  The extent of the cell in the x-direction
     * @param dy  The extent of the cell in the y-direction 
     * @param f00 Function value at (0,0)
     * @param f10 Function value at (1,0)
     * @param f11 Function value at (1,1)
     * @param f01 Function value at (0,1)
     * @param cval The contour value
     * @param nv If allocated to int[1] on call, will contain the internal
     * parameter indicating which of the 16 possible cases for contouring through
     * the cell. See source code for understanding of this parameter.
     * @return The line segments found, null if contour outside cell.
     * lineSeg[i][0][0], lineSeg[i][0][1] is (x1,y1) for line segment i
     * lineSeg[i][1][0], lineSeg[i][1][1] is (x2,y2) for line segment i 
     * The number of returned line segments for a cell is either 0, 1 or 2.
     */
    public static final double[][][] contourSegment(double x00, double y00, double dx, double dy, double f00, double f10, double f11, double f01, double cval, int[] nv) {
        double[][][] ret = null;

        int n = 0;
        if (f00 > cval)
            n += 1;
        if (f10 > cval)
            n += 8;
        if (f11 > cval)
            n += 4;
        if (f01 > cval)
            n += 2;

        switch (n) {
        case 1:
        case 14: // Left-Bottom
            ret = new double[1][2][2];
            ret[0][0][0] = x00;
            ret[0][0][1] = y00 + dy * (cval - f00) / (f01 - f00);
            ret[0][1][0] = x00 + dx * (cval - f00) / (f10 - f00);
            ret[0][1][1] = y00;
            break;
        case 2:
        case 13: // Left-Top
            ret = new double[1][2][2];
            ret[0][0][0] = x00;
            ret[0][0][1] = y00 + dy * (cval - f00) / (f01 - f00);
            ret[0][1][0] = x00 + dx * (cval - f01) / (f11 - f01);
            ret[0][1][1] = y00 + dy;
            break;
        case 4:
        case 11:
            ret = new double[1][2][2];
            ret[0][0][0] = x00 + dx * (cval - f01) / (f11 - f01);
            ret[0][0][1] = y00 + dy;
            ret[0][1][0] = x00 + dx;
            ret[0][1][1] = y00 + dy * (cval - f10) / (f11 - f10);
            break;
        case 7:
        case 8:
            ret = new double[1][2][2];
            ret[0][0][0] = x00 + dx * (cval - f00) / (f10 - f00);
            ret[0][0][1] = y00;
            ret[0][1][0] = x00 + dx;
            ret[0][1][1] = y00 + dy * (cval - f10) / (f11 - f10);
            break;
        case 3:
        case 12:
            ret = new double[1][2][2];
            ret[0][0][0] = x00 + dx * (cval - f00) / (f10 - f00);
            ret[0][0][1] = y00;
            ret[0][1][0] = x00 + dx * (cval - f01) / (f11 - f01);
            ret[0][1][1] = y00 + dy;
            break;
        case 6:
        case 9:
            ret = new double[1][2][2];
            ret[0][0][0] = x00;
            ret[0][0][1] = y00 + dy * (cval - f00) / (f01 - f00);
            ret[0][1][0] = x00 + dx;
            ret[0][1][1] = y00 + dy * (cval - f10) / (f11 - f10);
            break;
        case 5:
            ret = new double[2][2][2];
            ret[0][0][0] = x00;
            ret[0][0][1] = y00 + dy * (cval - f00) / (f01 - f00);
            ret[0][1][0] = x00 + dx * (cval - f00) / (f10 - f00);
            ret[0][1][1] = y00;
            ret[1][0][0] = x00 + dx * (cval - f01) / (f11 - f01);
            ret[1][0][1] = y00 + dy;
            ret[1][1][0] = x00 + dx;
            ret[1][1][1] = y00 + dy * (cval - f10) / (f11 - f10);
            break;
        case 10:
            ret = new double[2][2][2];
            ret[0][0][0] = x00;
            ret[0][0][1] = y00 + dy * (cval - f00) / (f01 - f00);
            ret[0][1][0] = x00 + dx * (cval - f01) / (f11 - f01);
            ret[0][1][1] = y00 + dy;
            ret[1][0][0] = x00 + dy * (cval - f00) / (f10 - f00);
            ret[1][0][1] = y00;
            ret[1][1][0] = x00 + dx;
            ret[1][1][1] = y00 + dy * (cval - f10) / (f11 - f10);
            break;
        default:
        }

        if (nv != null)
            nv[0] = n;

        return ret;
    }

    
    /**
     * Quicksort for a arbitrary objects.
     * 
     * Based on R. Sedgewick 'Algorithms in Java Part 1-4'
     * 
     * Example: quicksort(a,null);
     * Sorts the whole array a in ascending direction
     * 
     * @param a     An array of doubles that should be sorted.
     * @param ia    If allocated on call to ia[a.length], 
     * will on return contain the shuffled indices.
     * 
     */
    public static final void quicksortComparable(Comparable[] a, int[] ia) {
        if (ia != null) {
            for(int i = 0;i < a.length; ++i) {
                ia[i] = i;
            }
        }

        quicksortpartComparable(a, 0, a.length - 1, ia);
    }
    
    private static final void quicksortpartComparable(Comparable[] a, int left, int right, int[] ia) {
        if (right <= left)
            return;
        int i = partitionObjectComparable(a, left, right, ia);
        quicksortpartComparable(a, left, i - 1, ia);
        quicksortpartComparable(a, i + 1, right, ia);
    }
    
    private static final int partitionObjectComparable(Comparable[] a, int left, int right, int[] ia) {
        int i = left - 1;
        int j = right;
        
        while (true) {
            while (a[ ++i].compareTo(a[right]) < 0)
                ;
            while (a[right].compareTo(a[ --j]) < 0)
                if (j == left)
                    break;
            if (i >= j)
                break;
            Comparable swap = a[i];
            a[i] = a[j];
            a[j] = swap;
            if (ia != null) {
                int iswap = ia[i];
                ia[i] = ia[j];
                ia[j] = iswap;
            }

        }
        Comparable swap = a[i];
        a[i] = a[right];
        a[right] = swap;
        if (ia != null) {
            int iswap = ia[i];
            ia[i] = ia[right];
            ia[right] = iswap;
        }

        return i;
    }





    /**
     * Quicksort for a double array.
     * 
     * Based on R. Sedgewick 'Algorithms in Java Part 1-4'
     * 
     * Example: quicksort(a,null);
     * Sorts the whole array a in ascending direction
     * 
     * @param a     An array of doubles that should be sorted.
     * @param ia    If allocated on call to ia[a.length], 
     * will on return contain the shuffled indices.
     * 
     */
    public static final void quicksort(double[] a, int[] ia) {
        if (ia != null) {
            for(int i = 0;i < a.length; ++i) {
                ia[i] = i;
            }
        }

        quicksortpart(a, 0, a.length - 1, ia);
    }


    /**
     * Quicksort for a int array. (see quicksort())
     * 
     * @param a     An array of ints that should be sorted.
     * @param ia    If allocated on call to ia[a.length], will on return contain the shuffled indices.
     * 
     */
    public static final void quicksortInt(int[] a, int[] ia) {
        if (ia != null) {
            for(int i = 0;i < a.length; ++i) {
                ia[i] = i;
            }
        }

        quicksortpartInt(a, 0, a.length - 1, ia);
    }

    /**
     * Orders the given array according to the indices stored
     * in another array. Can be used for sorting an array
     * in the same order as an array previously sorted with
     * for example quicksort().
     * 
     * @param a The array to be ordered.
     * @param ia The indices determining the new ordering of the
     * array elements.
     */
    public static final void order(double[] a, int[] ia) {
        double[] ac = a.clone();
        for(int i = 0;i < a.length; ++i) {
            a[i] = ac[ia[i]];
        }
    }
    
    /**
     * Orders the given array according to the indices stored
     * in another array. Can be used for sorting an array
     * in the same order as an array previously sorted with
     * for example quicksort().
     * 
     * @param a The array to be ordered.
     * @param ia The indices determining the new ordering of the
     * array elements.
     */
    public static final void order(Object[] a, int[] ia) {
        Object[] ac = a.clone();
        for(int i = 0;i < a.length; ++i) {
            a[i] = ac[ia[i]];
        }
    }


    private static final void quicksortpart(double[] a, int left, int right, int[] ia) {
        if (right <= left)
            return;
        int i = partition(a, left, right, ia);
        quicksortpart(a, left, i - 1, ia);
        quicksortpart(a, i + 1, right, ia);
    }

    private static final void quicksortpartInt(int[] a, int left, int right, int[] ia) {
        if (right <= left)
            return;
        int i = partitionInt(a, left, right, ia);
        quicksortpartInt(a, left, i - 1, ia);
        quicksortpartInt(a, i + 1, right, ia);
    }

    private static final int partition(double[] a, int left, int right, int[] ia) {
        int i = left - 1;
        int j = right;
        while (true) {
            while (a[ ++i] < a[right])
                ;
            while (a[right] < a[ --j])
                if (j == left)
                    break;
            if (i >= j)
                break;
            double swap = a[i];
            a[i] = a[j];
            a[j] = swap;
            if (ia != null) {
                int iswap = ia[i];
                ia[i] = ia[j];
                ia[j] = iswap;
            }

        }
        double swap = a[i];
        a[i] = a[right];
        a[right] = swap;
        if (ia != null) {
            int iswap = ia[i];
            ia[i] = ia[right];
            ia[right] = iswap;
        }

        return i;
    }

    private static final int partitionInt(int[] a, int left, int right, int[] ia) {
        int i = left - 1;
        int j = right;
        while (true) {
            while (a[ ++i] < a[right])
                ;
            while (a[right] < a[ --j])
                if (j == left)
                    break;
            if (i >= j)
                break;
            int swap = a[i];
            a[i] = a[j];
            a[j] = swap;
            if (ia != null) {
                int iswap = ia[i];
                ia[i] = ia[j];
                ia[j] = iswap;
            }

        }
        int swap = a[i];
        a[i] = a[right];
        a[right] = swap;
        if (ia != null) {
            int iswap = ia[i];
            ia[i] = ia[right];
            ia[right] = iswap;
        }

        return i;
    }
    
    
    /**
     * 
     * Calculates the intersection points between a 3D line and a volume defined by polygonal boundary rotated 2*pi around the origin.
     * Actually, the routine is quite clumsily written, since it steps along the path, to make it easy to pick up all intersection points
     * on all sides. It could proably all be done analytically.
     *  
     * x-values of intersection points are returned in ret[i][0]
     * y-values of intersection points are returned in ret[i][1]
     * z-values of intersection points are returned in ret[i][2]
     * 
     * @param startPositions Start positions for straight lines: startPosistions[0][i] is x for start position i.
     * @param directions Directions for straight lines: directions[0][i] is x-coordinate of direction vector/
     * @param boundaryR R-values of cylindrical boundary
     * @param boundaryZ Z-values of cylindrical boundary
     * @param stepLength Step length when numerically walking along line to check for intersection points
     * @param maxLength Max length to step through along line.
     * @param maxNumIntersectionsReturned Max number of intersections that will be returned
     * @return ret[i][0][j] is the x-value of the j:th intersection point for line/direction i, ret[i][1][j] the y-values and ret[i][2][j] the z-values 
     * 
     */
    public static double[][][] intersectionLineWithRotatedPolygonVolumeBoundary(double[][] startPositions, double[][] directions, double[] boundaryR, double[] boundaryZ, double stepLength, double maxLength, int maxNumIntersectionsReturned) {
        double[][][] ret = new double[startPositions[0].length][3][];
        
        for(int i=0;i<startPositions[0].length;++i) {
            double length = stepLength;
            double[] startPos = new double[] { startPositions[0][i], startPositions[1][i], startPositions[2][i] };
            double[] currentPos = startPos.clone();
            double[] nextPos = new double[3];
            double[] direction = Algorithms.normaliseVector(new double[] { directions[0][i], directions[1][i], directions[2][i] });
            boolean currentPosInsideBoundary = Algorithms.isWithinPolygon(Math.sqrt(currentPos[0]*currentPos[0] + currentPos[1]*currentPos[1]), currentPos[2], boundaryR, boundaryZ);
            int numIntersections = 0;
            ArrayList<double[]> intersectionPoints = new ArrayList<double[]>();         
            while(length <= maxLength) {
                nextPos[0] = startPos[0] + direction[0]*length;
                nextPos[1] = startPos[1] + direction[1]*length;
                nextPos[2] = startPos[2] + direction[2]*length;
                
                double r2 = Math.sqrt(nextPos[0]*nextPos[0] + nextPos[1]*nextPos[1]);
                double z2 = nextPos[2];
                
                boolean isWithinPolygon = Algorithms.isWithinPolygon(r2, z2, boundaryR, boundaryZ); 
                if (isWithinPolygon && !currentPosInsideBoundary) {
                    double r1 = Math.sqrt(currentPos[0]*currentPos[0] + currentPos[1]*currentPos[1]);
                    double z1 = currentPos[2];
                    double[][] intersections = Algorithms.intersectionLineSegmentPolygon2DOrderedAlongLineSegment(boundaryR, boundaryZ, r1, z1, r2, z2);
                    double dist = Double.NaN;
                    if (direction[2] != 0) {
                        double zIntersect = intersections[1][0];
                        dist = (zIntersect - z1)/direction[2]; // Z-component of dist*direction + currentPos = intersectionPoint, gives the distance from currentPos to intersection point
                    } else {
                        double rIntersect = intersections[0][0];
                        double directionR = Math.sqrt(direction[0]*direction[0] + direction[1]*direction[1]);
                        dist = (rIntersect - r1)/directionR; // R-component of dist*direction + currentPos = intersectionPoint, gives the distance from currentPos to intersection point
                    } 
                    double[] intersectionPoint = new double[] { currentPos[0]+dist*direction[0],  currentPos[1]+dist*direction[1], currentPos[2]+dist*direction[2]};
                    intersectionPoints.add(intersectionPoint);
                    ++numIntersections;
                    currentPosInsideBoundary = true;
                } else if (!isWithinPolygon && currentPosInsideBoundary) {
                    double r1 = Math.sqrt(currentPos[0]*currentPos[0] + currentPos[1]*currentPos[1]);
                    double z1 = currentPos[2];
                    double[][] intersections = Algorithms.intersectionLineSegmentPolygon2DOrderedAlongLineSegment(boundaryR, boundaryZ, r1, z1, r2, z2);
                    double dist = Double.NaN;
                    if (direction[2] != 0) {
                        double zIntersect = intersections[1][0];
                        dist = (zIntersect - z1)/direction[2]; // Z-component of dist*direction + currentPos = intersectionPoint, gives the distance from currentPos to intersection point
                    } else {
                        double rIntersect = intersections[0][0];
                        double directionR = Math.sqrt(direction[0]*direction[0] + direction[1]*direction[1]);
                        dist = (rIntersect - r1)/directionR; // R-component of dist*direction + currentPos = intersectionPoint, gives the distance from currentPos to intersection point
                    } 
                    double[] intersectionPoint = new double[] { currentPos[0]+dist*direction[0],  currentPos[1]+dist*direction[1], currentPos[2]+dist*direction[2]};
                    intersectionPoints.add(intersectionPoint);
                   ++numIntersections;
                    currentPosInsideBoundary = false;
                }
                
                if (numIntersections >= maxNumIntersectionsReturned) break;
                                
                currentPos[0] = nextPos[0]; currentPos[1] = nextPos[1]; currentPos[2] = nextPos[2];
                length += stepLength;
            }
            if (numIntersections > 0) {
                ret[i][0] = new double[intersectionPoints.size()];
                ret[i][1] = new double[intersectionPoints.size()];
                ret[i][2] = new double[intersectionPoints.size()];
                for(int j=0;j<ret[i][0].length;++j) {
                    double[] point = intersectionPoints.get(j);
                    ret[i][0][j] = point[0];
                    ret[i][1][j] = point[1];
                    ret[i][2][j] = point[2];
                }
            } else {
                ret[i] = null;
            }
        }
    
        return ret;
    }
    
    
    /**
     * Finds the unique elements in a and returns them in the order found.
     * 
     * @param a The array.
     * @return An array with the unique values in a
     */
    public static double[] unique(double[] a) {
        LinkedHashSet<Double> set = new LinkedHashSet<Double>();
        
        for(int i=0;i<a.length;++i) {
            set.add(a[i]);
        }
        
        double[] ret = new double[set.size()]; 
        int index = 0;
        for(Double d : set) {
            ret[index++] = d;
        }
        
        return ret;        
    }
    
    /**
     * Reverses the order of the rows in a matrix.
     * The whole matrix will be copied.
     * 
     * @param a A matrix.
     * @return The matrix with the rows in reverse order.
     */
    public static double[][] flipud(double[][] a) {
        double[][] ret = new double[a.length][a[0].length];
        for(int i=0;i<a.length;++i) {
            System.arraycopy(a[i], 0, ret[a.length-1-i], 0, a[i].length);
        }
        
        return ret;
    }
    
    /**
     * Reverses the order of the array elements.
     * 
     * @param a An array.
     * @return An array with order reversed.
     */
    public static double[] fliplr(double[] a) {
        double[] ret = new double[a.length];
        
        for(int i=0;i<ret.length;++i) {
            ret[i] = a[a.length-1-i];
        }
        
        return ret;
    }
    

    /**
     * Returns the maximum value in the given array.
     * If the index argument is allocated by the
     * caller (to index[1]), the index of the maximum
     * value is returned in index[0].
     * 
     * @param v A vector with values.
     * @param index If allocated by the caller to int[1],
     * will return the index of the maximum value.
     * 
     * @return The maximum value in the given array.
     */
    public static final double max(double[] v, int[] index) {
        // Must init from -MAX_VALUE rather than first element, since first element can be NaN
        double maxv = -Double.MAX_VALUE;
        int iv = -1;
        for(int i = 0;i < v.length; ++i) {
            if (v[i] > maxv) {
                maxv = v[i];
                iv = i;
            }
        }
        if (index != null) {
            index[0] = iv;
        }
        
        return maxv;
    }
    
    /**
     * Returns the maximum value in the given array.
     * 
     * @param v A vector with values.
     * 
     * @return The maximum value in the given array.
     */    
    public static final double max(double[] v) {
           return max(v, null);
    }
    
    /**
     * Returns the index of the maximum value in the given array.
     * @param v A vector with values
     * 
     * @return The index of the array element with the maximum value.
     */
    public static final int maxi(double[] v) {
        int[] index = new int[1];
        max(v, index);
        return index[0];
    }

    /**
     * Returns the minimum value in the given array.
     * If the index argument is allocated by the
     * caller (to index[1]), the index of the minimum
     * value is returned in index[0].
     * 
     * @param v A vector with values.
     * @param index If allocated by the caller to int[1],
     * will return the index of the minimum value.
     * 
     * @return The minimum value in the given array.
     */
    public static final double min(double[] v, int[] index) {
        // Must init from MAX_VALUE rather than first element, since first element can be NaN
        double minv = Double.MAX_VALUE; 
        int iv = -1;
        for(int i = 0;i < v.length; ++i) {
            if (v[i] < minv) {
                minv = v[i];
                iv = i;
            }
        }
        if (index != null) {
            index[0] = iv;
        }
        
        return minv;
    }
    
    /**
     * Returns the minimum value in the given array.
     * 
     * @param v A vector with values.
     * 
     * @return The minimum value in the given array.
     */    
    public static final double min(double[] v) {
        return min(v, null);
    }
    
    /**
     * Returns the index of the minimum value in the given array.
     * @param v A vector with values
     * 
     * @return The index of the array element with the minimum value.
     */
    public static final int mini(double[] v) {
        int[] index = new int[1];
        min(v, index);
        return index[0];
    }

        
    
    /**
     * Returns the maximum value in the given matrix.
     * If the index argument is allocated by the
     * caller (to index[2]), the index of the maximum
     * value is returned in row=index[0], col=index[1].
     * 
     * @param v A matrix with values.
     * @param index If allocated by the caller to int[2], will 
     * return the index row=index[0],col=index[1] of the maximum value.
     * 
     * @return The maximum value in the given matrix.
     */
    public static final double max(double[][] v, int[] index) {
        // Must init from MAX_VALUE rather than first element, since first element can be NaN        
        double maxv = -Double.MAX_VALUE;
        int iv = -1, jv = -1;
        for(int i=0;i<v.length;++i) {
            for(int j=0;j<v[0].length;++j) {
                if (v[i][j] > maxv) {
                    maxv = v[i][j];
                    iv = i;
                    jv = j;
                }
            }
        }
        if (index != null) {
            index[0] = iv;
            index[1] = jv;
        }
        
        return maxv;
    }
    

    
    /**
     * Returns the maximum value in the given matrix.
     * 
     * @param v A matrix with values.
     * 
     * @return The maximum value in the given matrix.
     */    
    public static final double max(double[][] v) {
           return max(v, null);
    }

    /**
     * Returns the minimum value in the given matrix.
     * If the index argument is allocated by the
     * caller (to index[2]), the index of the minimum
     * value is returned in row=index[0], col=index[1].
     * 
     * @param v A matrix with values.
     * @param index If allocated by the caller to int[2],
     * will return the index of the minimum value in
     * row=index[0], col=index[1].
     * 
     * @return The minimum value in the given matrix.
     */
    public static final double min(double[][] v, int[] index) {
        // Must init from MAX_VALUE rather than first element, since first element can be NaN
        double minv = Double.MAX_VALUE;
        int iv = -1, jv = -1;
        for(int i=0;i<v.length;++i) {
            for(int j=0;j<v[0].length;++j) {
                if (v[i][j] < minv) {
                    minv = v[i][j];
                    iv = i;
                    jv = j;
                }
            }
        }
        if (index != null) {
            index[0] = iv;
            index[1] = jv;
        }
        
        return minv;
    }
    
    /**
     * Returns the minimum value in the given matrix.
     * 
     * @param v A matrix with values.
     * 
     * @return The minimum value in the given matrix.
     */    
    public static final double min(double[][] v) {
        return min(v, null);
    }
    
    /**
     * Returns the maximum value in the given integer array.
     * If the index argument is allocated by the
     * caller (to index[1]), the index of the maximum
     * value is returned in index[0].
     * 
     * @param v A vector with values.
     * @param index If allocated by the caller to int[1],
     * will return the index of the maximum value.
     * 
     * @return The maximum value in the given integer array.
     */
    public static final int max(int[] v, int[] index) {
        int maxv = v[0];
        int iv = 0;
        for(int i = 1;i < v.length; ++i) {
            if (v[i] > maxv) {
                maxv = v[i];
                iv = i;
            }
        }
        if (index != null) {
            index[0] = iv;
        }
        
        return maxv;
    }
    
    /**
     * Returns the maximum value in the given integer array.
     * 
     * @param v A vector with values.
     * 
     * @return The maximum value in the given integer array.
     */    
    public static final int max(int[] v) {
           return max(v, null);
    }

    /**
     * Returns the minimum value in the given integer array.
     * If the index argument is allocated by the
     * caller (to index[1]), the index of the minimum
     * value is returned in index[0].
     * 
     * @param v A vector with values.
     * @param index If allocated by the caller to int[1],
     * will return the index of the minimum value.
     * 
     * @return The minimum value in the given integer array.
     */
    public static final int min(int[] v, int[] index) {
        int minv = v[0];
        int iv = 0;
        for(int i = 1;i < v.length; ++i) {
            if (v[i] < minv) {
                minv = v[i];
                iv = i;
            }
        }
        if (index != null) {
            index[0] = iv;
        }
        
        return minv;
    }
    
    /**
     * Returns the minimum value in the given integer array.
     * 
     * @param v A vector with values.
     * 
     * @return The minimum value in the given integer array.
     */    
    public static final int min(int[] v) {
        return min(v, null);
    }
     
    
    /**
     * Returns the mean value of the elements in an array.
     * 
     * @param v A vector with values.
     * 
     * @return The mean value of the elements in the given array.
     */
    public static final double mean(double[] v) {
        double ret = 0;
        for(int i=0;i<v.length;++i) {
            ret += v[i];
        }
        
        return ret/(double)v.length;
    }
    
    
    /**
     * Returns the mean value of the elements in an array, where
     * elements with Double.NaN are ignored.
     * 
     * @param v A vector with values.
     * 
     * @return The mean value of the elements in the given array.
     */
    public static final double nanmean(double[] v) {
        double ret = 0;
        int numNaN = 0;
        for(int i=0;i<v.length;++i) {
            if (Double.isNaN(v[i])) {
                ++numNaN;
            } else {
                ret += v[i];
            }            
        }
        
        return numNaN == v.length ? Double.NaN : ret/(double)(v.length-numNaN);
    }

    
    /**
     * Returns the variance of the samples in the
     * vector x.
     * 
     * @param x
     * @return
     */
    public static final double var(double[] x) {
        double m = 0;
        double m2 = 0;
        for(int i=0;i<x.length;++i) {
            m += x[i];
            m2 += x[i]*x[i];
        }
        
        double n = (double)x.length;
        return 1.0/n*(m2-m*m/n);
    }
    
    
    /**
     * Returns the standard deviation of the samples in the
     * vector x.
     * 
     * @param x
     * @return
     */    
    public static final double std(double[] x) {
        return Math.sqrt(var(x));
    }
    
    /**
     * Calculates the Root Mean Squared Error (RMSE) between
     * two signals. Usually one is the real signal and the other
     * an estimate of it.
     * The RMSE is an estimate of the standard deviation 
     * of the predictor.
     * 
     * @param s1 Signal 1
     * @param s2 Signal 2
     * @return The RMSE of s1 as predictor of s2 (or the other way around, order
     * of the arrays are irrelevant).
     */
    public static double rmse(double[] s1, double[] s2) {
        double sum2 = 0;
        for(int i=0;i<s1.length;++i) {
            sum2 += (s1[i] - s2[i])*(s1[i] - s2[i]);
        }
        
        return Math.sqrt(sum2/(double)s1.length);
    }
    
    /**
     * Normalises the values in an array, so that
     *  dNew = (d-mean)/std
     * where mean is the mean of d and std the standard deviation of d.
     * After this operation, the new vector will have mean 0 and standard deviation 1.
     *  
     * @param d The data vector.
     * @return The normalised data vector.
     */
    public static double[] normalise(double[] d) {
        double mean = Algorithms.mean(d);
        double std = Algorithms.std(d);
        double[] ret = new double[d.length];
        for(int i=0;i<ret.length;++i) {
            ret[i] = (d[i] - mean)/std;
        }
        
        return ret;
    }
    
    
    /**
     * Histogram, should be equivalent to the corresponding matlab function.
     * 
     * @param d The values to be binned
     * @param numBins Number of bins.
     * @return ret[0] are the bin centres, ret[1] the number of counts for that bin.
     */
    public static final double[][] hist(double[] d, int numBins) {
        double dmin = algorithmrepository.Algorithms.min(d, null);
        double dmax = algorithmrepository.Algorithms.max(d, null);
        
        double[] x = new double[numBins];
        double dx = (dmax - dmin)/(double)(numBins - 1.0);
        for(int i=0;i<numBins;++i) x[i] = dmin + i*dx;
        double[] f = new double[numBins];
        for(int i=0;i<d.length;++i) {
            int left = (int) Math.floor((numBins-1)*(d[i]-dmin)/(dmax-dmin));
            f[left] += 1;
        }

        return new double[][] { x, f };
    }
    
    
    /**
     * Finds the bias (a) and slope (b) of the line y=a+b*x in the least squares sense from the
     * given points.
     * @param x x-values
     * @param y observations at x
     * @return ret[0] is a, ret[1] is b
     */
    public static final double[] lineFit(double[] x, double[] y) {
        double b = cov(x, y) / var(x);
        double a = mean(y) - b * mean(x);

        return new double[] { a, b };
    }


    /**
     * Returns the correlation coefficient between
     * the samples in two vectors.
     * 
     * @param x First vector with samples.
     * @param y Second vector with samples.
     * @return The correlation coefficient between x
     * and y.
     */
    public static final double corr(double[] x, double[] y) {
        double mx = 0;
        double my = 0;
        double mx2 = 0;
        double my2 = 0;
        double mxy = 0;
        for(int i = 0;i < x.length; ++i) {
            mx += x[i];
            my += y[i];
            mx2 += x[i] * x[i];
            my2 += y[i] * y[i];
            mxy += x[i] * y[i];
        }

        return (mxy - mx * my) / Math.sqrt(x.length * (mx2 - mx * mx + my2 - my * my));
    }
    
    /**
     * Returns the correlation coefficient between
     * the samples in two vectors. This on is numerically
     * more stable than corr(), but takes longer time.
     * 
     * @param x First vector with samples.
     * @param y Second vector with samples.
     * @return The correlation coefficient between x
     * and y.
     */    
    public static double corr2(double[] x, double[] y) {
        double cov = cov(x, y);
        return cov/std(x)/std(y);
    }


    /**
     * Returns the covariance between
     * the samples in two vectors.
     * 
     * @param x First vector with samples.
     * @param y Second vector with samples.
     * @return The correlation coefficient between x
     * and y.
     */
    public static final double cov(double[] x, double[] y) {
        double mx = 0;
        double my = 0;
        double mxy = 0;
        for(int i = 0;i < x.length; ++i) {
            mx += x[i];
            my += y[i];
            mxy += x[i] * y[i];
        }

        return mxy / x.length - mx * my / (x.length * x.length);
    }


    /**
     * Returns two matrices corresponding to the x-coordinate and y-coordinates
     * on the grid specified by the vectors x and y. From these matrices a z-value
     * on the grid could be specified by z(ij)=f(xgrid(ij),ygrid(ij)) for each element ij 
     * in the returned matrices xgrid,ygrid.
     * @param x The Nx x-coordinates of the grid
     * @param y The Ny y-coordinates of the grid
     * @return ret[0] = xgrid, ret[1] = ygrid, where xgrid and ygrid are Ny*Nx (OBS! - imitates matlab)
     * matrices containing the values of the coordinates at every grid position.
     */
    public static final double[][][] meshgrid(double[] x, double[] y) {
        double[][] xgrid = new double[y.length][x.length];
        double[][] ygrid = new double[y.length][x.length];

        for(int i = 0;i < y.length; ++i) {
            for(int j = 0;j < x.length; ++j) {
                xgrid[i][j] = x[j];
                ygrid[i][j] = y[i];
            }
        }

        return new double[][][] { xgrid, ygrid };
    }
    

    /**
     * Returns three matrices corresponding to the x-coordinate, y-coordinates and z-coordinates
     * on the grid specified by the vectors x, y and z. 
     * @param x The Nx x-coordinates of the grid
     * @param y The Ny y-coordinates of the grid
     * @param z The Nz z-coordinates of the grid
     * @return ret[0] = xgrid, ret[1] = ygrid, ret[2] = zgrid where xgrid, ygrid, zgrid are Nz*Ny*Nx
     * matrices containing the values of the coordinates at every grid position.
     */
    public static final double[][][][] meshgrid(double[] x, double[] y, double[] z) {
        double[][][] xgrid = new double[z.length][y.length][x.length];
        double[][][] ygrid = new double[z.length][y.length][x.length];
        double[][][] zgrid = new double[z.length][y.length][x.length];

        for(int i = 0;i < z.length; ++i) {
            for(int j = 0;j < y.length; ++j) {
                for(int k = 0;k < x.length; ++k) {
                    xgrid[i][j][k] = x[k];
                    ygrid[i][j][k] = y[j];
                    zgrid[i][j][k] = z[i];
                    
                }
            }            
        }

        return new double[][][][] { xgrid, ygrid, zgrid };
    }


    /**
     * Determining if point inside 2D closed polygon.
     * First and last point in bx and by are assumed to be the same.
     * 
     * A Java translation from 
     * http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/
     * 
     * See http://www.algorithmrepository.org
     * 
     * @param x x-coordinate of point 
     * @param y y-coordinate of point 
     * @param bx x-coordinates of boundary
     * @param by y-coordinates of boundary
     * 
     * @return true if point inside polygon, false otherwise.
     */
    public static final boolean isWithinPolygon(double x, double y, double[] bx, double[] by) {
        boolean c = false;
        int i, j;
        int npol = bx.length - 1;

        for(i = 0, j = npol - 1;i < npol;j = i++ ) {
            if ((((by[i] <= y) && (y < by[j])) || ((by[j] <= y) && (y < by[i]))) && (x < (bx[j] - bx[i]) * (y - by[i]) / (by[j] - by[i]) + bx[i]))
                c = !c;
        }

        return c;
    }


    /**
     * Returns the area of a polygon. 
     * Based on http://local.wasp.uwa.edu.au/~pbourke/geometry/polyarea/ for description.
     *  
     * @param x An array of the x-coordinates of the corner points of the
     * polygon. 
     * @param y An array of the y-coordinates of the corner points of the
     * polygon. 
     * @return The area of the polygon.
     */
    public static final double polygonArea(double[] x, double[] y) {
        int n, i, j;
        double area = 0;

        if (x[0] == x[x.length - 1] && y[0] == y[y.length - 1]) {
            n = x.length - 1;
        }
        else {
            n = x.length;
        }

        for(i = 0;i < n;i++ ) {
            j = (i + 1) % n;
            area += x[i] * y[j];
            area -= y[i] * x[j];
        }

        area /= 2;
        return (area < 0 ? -area : area);
    }


    /**
     * Returns the length of the polygon circumference.
     * 
     * @param x An array of the x-coordinates of the corner points of the
     * polygon. 
     * @param y An array of the y-coordinates of the corner points of the
     * polygon. 
     * @return The circumference of the polygon.
     */
    public static final double polygonLength(double[] x, double[] y) {
        int n, i, j;
        double length = 0;

        if (x[0] == x[x.length - 1] && y[0] == y[y.length - 1]) {
            n = x.length - 1;
        }
        else {
            n = x.length;
        }

        for(i = 0;i < n - 1;i++ ) {
            length += Math.sqrt((x[i + 1] - x[i]) * (x[i + 1] - x[i]) + (y[i + 1] - y[i]) * (y[i + 1] - y[i]));
        }
        length += Math.sqrt((x[n - 1] - x[0]) * (x[n - 1] - x[0]) + (y[n - 1] - y[0]) * (y[n - 1] - y[0]));

        return length;
    }


    /**
     * Returns the angle [0, 2*PI], of a point x, y.
     * 
     * @param x x-coord of point
     * @param y y-coord of point
     * @return The angle [0, 2*PI] the point makes with the x-axis, in the interval [0, 2*PI]
     */
    public static final double angle(double x, double y) {
        double angle = FastMath.atan2(y, x);

        return angle < 0 ? 2 * Math.PI + angle : angle;
    }

    /**
     * Returns the angle [0, 2*PI], of a point x, y.
     * 
     * @param x x-coords of points
     * @param y y-coords of points
     * @return The angles [0, 2*PI] the point makes with the x-axis, in the interval [0, 2*PI]
     */
    public static final double[] angle(double[] x, double[] y) {
        double[] ret = new double[x.length];
        for(int i=0;i<ret.length;++i) {
            ret[i] = angle(x[i], y[i]);
        }
        
        return ret;
    }
    

    /**
     * Calculates the centroid of a polygon. Also calculates and returns the
     * polygon area, which is an intermediate result of the centroid calculation.
     * 
     * Based on http://local.wasp.uwa.edu.au/~pbourke/geometry/polyarea/ for description.
     * 
     * The centroid is defined: Cx = 1/Area*Integral(x*dA), Cy = 1/Area*Integral(y*dA)
     * 
     * @param x An array of the x-coordinates of the corner points of the
     * polygon. 
     * @param y An array of the y-coordinates of the corner points of the
     * polygon. 
     * @return ret[0] is the x-coordinate of the centroid (Cx), ret[1] the y-coordinate
     * of the centroid (Cy). ret[2] returns the polygon area.
     */
    public static final double[] polygonCentroid(double[] x, double[] y) {
        int n;

        if (x[0] == x[x.length - 1] && y[0] == y[y.length - 1]) {
            n = x.length - 1;
        }
        else {
            n = x.length;
        }

        double cx = 0, cy = 0;
        double area = 0;
        int i, j;
        double factor = 0;
        for(i = 0;i < n;i++ ) {
            j = (i + 1) % n;
            factor = (x[i] * y[j] - x[j] * y[i]);
            cx += (x[i] + x[j]) * factor;
            cy += (y[i] + y[j]) * factor;

            area += x[i] * y[j];
            area -= y[i] * x[j];
        }
        area /= 2.0;

        factor = 1 / (6 * area);
        cx *= factor;
        cy *= factor;

        return new double[] { cx, cy, Math.abs(area) };
    }
    
    
    /**
     * Expands a polygon a distance in all directions by putting new polygon points outward
     * from each point on the original polygon in a direction given by the average direction of the
     * perpendiculars to the line segments surrounding the original point.
     * 
     * @param polygon boundary[0] are x-coordinates, boundary[1] are y-coordinates
     * @param distance The distance of the new polygon outward from the given polygon.
     * @return ret[0] are the x-coordinates, ret[1] the y-coordinates.
     */
    public static final double[][] stretchPolygon(double[][] polygon, double distance) {
        double[][] ret = new double[2][polygon[0].length];
        double[] centre = Algorithms.polygonCentroid(polygon[0], polygon[1]);
        
        double direction = Algorithms.cross(new double[] { polygon[0][1] - polygon[0][0], polygon[1][1] - polygon[1][0], 0}, new double[] {centre[0]-polygon[0][0], centre[1]-polygon[1][0], 0 })[2];
        boolean clockwise = direction > 0 ? true : false;
        
        int numPoints = polygon[0].length;     
        int numUniquePoints;
        if (polygon[0][0] == polygon[0][numPoints - 1] && polygon[1][0] == polygon[1][numPoints - 1]) {
            numUniquePoints = numPoints - 1;
        }
        else {
            numUniquePoints = numPoints;
        }

        int p1, p2, p3;     
        for(int i=0;i<numUniquePoints;++i) {
            if (i == 0) {
                p1 = numUniquePoints-1; p2 = 0; p3 = 1;
            } else if (i == numUniquePoints-1) {
                p1 = i-1; p2 = i; p3 = 0;
            } else {
                p1 = i-1; p2 = i; p3 = i+1;
            }           
            
            double theta1 = Algorithms.angle(polygon[1][p2] - polygon[1][p1], polygon[0][p1] - polygon[0][p2]); // Angle of perp to line between p1 & p2
            double theta2 = Algorithms.angle(polygon[1][p3] - polygon[1][p2], polygon[0][p2] - polygon[0][p3]); // Angle of perp to line between p2 & p3
            double theta = 0.5*(theta1+theta2)+(clockwise ? 0 : Math.PI);
            if (theta1 >= 1.5*Math.PI && theta2 <= Math.PI || theta2 >= 1.5*Math.PI && theta1 <= Math.PI) theta += Math.PI;
            
            ret[0][i] = polygon[0][i]+distance*Math.cos(theta);
            ret[1][i] = polygon[1][i]+distance*Math.sin(theta);                 
        }
        
        if (numPoints != numUniquePoints) {
            ret[0][numPoints-1]= ret[0][0];
            ret[1][numPoints-1]= ret[1][0];
        }
        
        return ret;
    }
    
    
    /**
     * Returns one of the two vectors perpendicular to a give vector. 
     * If clockwise=true the perpendicular vector is pointing in the direction
     * that correspond to a clockwise rotation 90 degres of the orginal vector.
     * If clockwise=false, the returned vector correponds to an anti-clockwise
     * rotation 90 degres of the original vector.
     *  
     * @param x x-coordinate of vector
     * @param y y-coordinate of vector
     * @param clockwise true if vector should be rotated clockwise 90 degrees, false
     * if rotated anti-clockwise 9 degrees.
     * @return ret[0] is the x-coordinate of the perpendicular vector, ret[1] the y-coordinate.
     */
    public static double[] perpendicularVector2D(double x, double y, boolean clockwise) {
        return new double[] { (clockwise ? 1 : -1)*y, (clockwise ? -1 : 1)*x };
    }

    
    /** Java doesn't have the error function, which is annoying.
     * This was copied from somewhere. I can't remember where, but it works.
     * 
     * Fractional error in math formula less than 1.2 * 10 ^ -7.
     * although subject to catastrophic cancellation when z in very close to 0 */
     public final static double erf(double z) {
         double t = 1.0 / (1.0 + 0.5 * Math.abs(z));
    
         // use Horner's method
         double ans = 1 - t * FastMath.exp( -z*z   -   1.26551223 +
                                             t * ( 1.00002368 +
                                             t * ( 0.37409196 + 
                                             t * ( 0.09678418 + 
                                             t * (-0.18628806 + 
                                             t * ( 0.27886807 + 
                                             t * (-1.13520398 + 
                                             t * ( 1.48851587 + 
                                             t * (-0.82215223 + 
                                             t * ( 0.17087277))))))))));
         if (z >= 0) return  ans;
         else        return -ans;
     }
    
     
    /**
     * The cumulative distribution function (Integral [-INF, x]) for a normal distribution.
     * 
     * @param mean The mean of the distribution
     * @param sigma The standard deviation of the distribution
     * @param x The random variable.
     */
    public static double cdfNormal(double mean, double sigma, double x) {
        return 0.5 * (1 + erf( (x - mean) / (Math.sqrt(2) * sigma)) );
    }


    /**
     * 
     * Does one Newton iteration to find a root of the function f.
     * 
     * @param f Function whos root is sought.
     * @param xcur The current (initial) position
     * @param fcur The value of f at xcur[0]
     * @param delta The step size for the numerical derivative (forward differences are used).
     */
    public static final void newton1D(IFunction f, double[] xcur, double[] fcur, double delta) {
        double f1 = f.eval(xcur[0] + delta);
        double dfdx = der(fcur[0], f1, delta);

        xcur[0] -= fcur[0] / dfdx;
        fcur[0] = f.eval(xcur[0]);
    }


    /**
     * Does on iteration towards finding an extremum of the 1D function f 
     * using Newtons method and numerical derivatives.
     * 
     * @param f The function whose extremum is sought.
     * @param xcur The current position, on return the next position
     * @param fcur The function value at the current position, on return the next function value
     * @param delta The step for the numerical derivatives.
     */
    public static final void optNewton1D(IFunction f, double[] xcur, double[] fcur, double delta) {
        double f0 = f.eval(xcur[0] - delta);
        double f1 = fcur[0];
        double f2 = f.eval(xcur[0] + delta);

        double dfdx = der(f0, f2, delta);
        double d2fdx2 = der2(f0, f1, f2, delta);

        xcur[0] -= dfdx / d2fdx2;
        fcur[0] = f.eval(xcur[0]);
    }


    /**
     * Does one iteration towards finding a minimum of the 1D function f
     * using gradient descent and numerical derivatives.
     * 
     * @param f The function to minimize
     * @param xcur The current position, on return the next position
     * @param fcur The function value at the current position, on return the next function value
     * @param delta The step used for calculating numerical forward differences of the function f.
     * @param step The step taken in the gradient direction
     */
    public static final void optGradientDescent1D(IFunction f, double[] xcur, double[] fcur, double delta, double step) {
        double f0 = fcur[0];
        double f1 = f.eval(xcur[0] + delta);
        double dfdx = derf(f0, f1, delta);

        xcur[0] += -step * dfdx;
        fcur[0] = f.eval(xcur[0]);
    }


    // Constants for calculating elliptical functions
    private static final double aE[] = { 1.0, 0.44325141463, 0.06260601220, 0.04757383546, 0.01736506451 };

    private static final double bE[] = { 0.0, 0.24988368310, 0.09200180037, 0.04069697536, 0.00526449639 };

    /* Elliptic K(k^2) */

    private static final double aK[] = { 1.38629436112, 0.09666344259, 0.03590092383, 0.03742563713, 0.01451196212 };

    private static final double bK[] = { 0.5, 0.12498593597, 0.06880248576, 0.03328355346, 0.00441787012 };


    /**
     * Elliptic function from M. Abramowitz, "Handbook of Mathematical Functions", 
     * formula 17.3.36
     * http://www.math.sfu.ca/~cbm/aands/page_592.htm
     * 
     * @param k2
     * @return
     */
    public static final double ellipticE(double k2) {
        return (ellipticA(k2, aE, bE));
    }


    /**
     * See M. Abramowitz, "Handbook of Mathematical Functions", formula 17.3.34
     * http://www.math.sfu.ca/~cbm/aands/page_591.htm
     * 
     * @param k2
     * @return
     */
    public static final double ellipticK(double k2) {
        return (ellipticA(k2, aK, bK));
    }


    /**
     * See M. Abramowitz, "Handbook of Mathematical Functions"
     * formula 17.3.34, 17.3.36
     * http://www.math.sfu.ca/~cbm/aands/page_592.htm
     * 
     * @param m
     * @param a
     * @param b
     * @return
     */
    public static final double ellipticA(double m, double[] a, double b[]) {
        int i;
        double m1 = 1 - m;
        double sum_a = 0;
        double sum_b = 0;
        double m1potenz = 1;

        for(i = 0;i < 5;i++ ) {
            sum_a += a[i] * m1potenz;
            sum_b += b[i] * m1potenz;
            m1potenz *= m1;
        }

        return (sum_a + sum_b * Math.log(1 / m1));
    }


    /**
     * A mod(a,b) that works also when a is negative.
     * 
     * @param a
     * @param b
     * @return The modulus
     */
    public static final double mod(double a, double b) {
        double ret = a % b;
        return ret < 0 ? ret + b : ret;
    }
    
    /**
     * Returns coordinates along a line at given distances from the starting point.
     * 
     * @param p1 start point of the line (x,y,z).
     * @param p2 end point of the line (x,y,z).
     * @param distances Distances along the line from p1 to p2.
     * @return The coordinates for the given distances from the starting point.
     * ret[0] are the x-coordinates, ret[1] the y-coordinates, ret[2] the z-coordinates.
     */
    public static double[][] pointsAlongLine(double[] p1, double[] p2, double[] distances) {               
        double[][] ret = new double[3][distances.length];
        double[] n = new double[] { p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]  };        
        double length = Math.sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
        for(int i=0;i<n.length;++i) n[i] /= length;
        for(int i=0;i<distances.length;++i) {
            ret[0][i] = p1[0]+distances[i]*n[0];
            ret[1][i] = p1[1]+distances[i]*n[1];
            ret[2][i] = p1[2]+distances[i]*n[2];
        }
        
        return ret;
    } 

    /**
     * Returns coordinates along a line beteen start and end points, expressed
     * as the ratio [0,1] of the distnace between the start and end point.
     * 
     * @param p1 start point of the line (x,y,z).
     * @param p2 end point of the line (x,y,z).
     * @param relDistances Relative distances along the line from p1 to p2.
     * @return The coordinates for the given distances from the starting point.
     * ret[0] are the x-coordinates, ret[1] the y-coordinates, ret[2] the z-coordinates.
     */
    public static double[][] pointsAlongLineRelative(double[] p1, double[] p2, double[] relDistances) {
        double length = veclength(new double[] { p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2] });
        double[] distances = new double[relDistances.length];
        for(int i=0;i<distances.length;++i) {
            distances[i] = length*relDistances[i];
        }
        return pointsAlongLine(p1, p2, distances);
    }
    
    
    /**
     * Distributes points evenly along a path.
     * 
     * @param x x-coordinates of knots
     * @param y y-coordinates of knots
     * @param numPoints Number of points along the path.
     * @return ret[0][i] is the x-coordinate of the i:t point,
     * ret[1][i] is the y-coordinate of the i:th point.
     */
    public static double[][] pointsAlongPath(double[] x, double[] y, int numPoints) {
        double[][] ret = new double[2][numPoints];
        double[] lengths = new double[x.length-1];
        double totalLength = 0;
        for(int i=0;i<x.length-1;++i) {
            lengths[i]= Math.sqrt((x[i+1]-x[i])*(x[i+1]-x[i])+(y[i+1]-y[i])*(y[i+1]-y[i]));
            totalLength += lengths[i];
        }
        
        double[] pointDistances = new double[numPoints]; 
        if(numPoints == 1) {
            pointDistances[0] = totalLength/2; 
        } else {
            double dx = totalLength/(double)(numPoints - 1.0);
            for(int i=0;i<numPoints;++i) pointDistances[i] = i*dx;
        }
        
        int pathIndex = 0;
        double currentLength = lengths[pathIndex];
        double currentDistance = 0;
        for(int i=0;i<numPoints;++i) {
            while (pointDistances[i] > currentDistance+currentLength) {
                currentDistance += lengths[pathIndex];
                ++pathIndex;
                currentLength = lengths[pathIndex];               
            }
            ret[0][i] = x[pathIndex]+(pointDistances[i]-currentDistance)/currentLength*(x[pathIndex+1]-x[pathIndex]);
            ret[1][i] = y[pathIndex]+(pointDistances[i]-currentDistance)/currentLength*(y[pathIndex+1]-y[pathIndex]);
        }
        
        return ret;
    }
    
    
    /**
     * Distributes points evenly along a path.
     * 
     * @param x x-coordinates of knots
     * @param y y-coordinates of knots
     * @param z z-coordinates of knots
     * @param numPoints Number of points along the path.
     * @return ret[0][i] is the x-coordinate of the i:t point,
     * ret[1][i] the y-coordinate, and ret[2][i] the z-coordinate.
     */
    public static double[][] pointsAlongPath3D(double[] x, double[] y, double[] z, int numPoints) {
        double[][] ret = new double[3][numPoints];
        double[] lengths = new double[x.length-1];
        double totalLength = 0;
        for(int i=0;i<x.length-1;++i) {
            lengths[i]= Math.sqrt((x[i+1]-x[i])*(x[i+1]-x[i])+(y[i+1]-y[i])*(y[i+1]-y[i])+(z[i+1]-z[i])*(z[i+1]-z[i]));
            totalLength += lengths[i];
        }
        
        double TOL = totalLength*1e-15;
        
        double[] pointDistances = new double[numPoints]; 
        if(numPoints == 1) {
            pointDistances[0] = totalLength/2; 
        } else {
            double dx = totalLength/(double)(numPoints - 1.0);
            for(int i=0;i<numPoints;++i) pointDistances[i] = i*dx;
        }
        
        int pathIndex = 0;
        double currentLength = lengths[pathIndex];
        double currentDistance = 0;
        for(int i=0;i<numPoints;++i) {
            while (pointDistances[i] > currentDistance+currentLength+TOL) {
                currentDistance += lengths[pathIndex];
                ++pathIndex;
                currentLength = lengths[pathIndex];               
            }
            ret[0][i] = x[pathIndex]+(pointDistances[i]-currentDistance)/currentLength*(x[pathIndex+1]-x[pathIndex]);
            ret[1][i] = y[pathIndex]+(pointDistances[i]-currentDistance)/currentLength*(y[pathIndex+1]-y[pathIndex]);
            ret[2][i] = z[pathIndex]+(pointDistances[i]-currentDistance)/currentLength*(z[pathIndex+1]-z[pathIndex]);
        }
        
        return ret;
    }



    /** Find intersection points of a line and a double-cone
     * Courtesy of http://www.geometrictools.com
     * Specifically: http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
     * @param P  Position of a point on the line
     * @param D  Vector along line
     * @param V  Position of cone vertex (axis crossing)
     * @param A  Vector along axis of cone
     * @param theta  Cone angle
     * @return zero or two values of t, where intersection points are P + t*D, i.e. dist t along line  
     */
    public static final double[] coneLineIntersection(double P[], double D[], double V[], double A[], double theta) {


        double gamma2 = Math.cos(theta);
        gamma2 = gamma2 * gamma2;

        double M[][] = new double[][] { { A[0] * A[0] - gamma2, A[0] * A[1], A[0] * A[2] }, { A[1] * A[0], A[1] * A[1] - gamma2, A[1] * A[2] }, { A[2] * A[0], A[2] * A[1], A[2] * A[2] - gamma2 } };


        double Delta[] = new double[] { P[0] - V[0], P[1] - V[1], P[2] - V[2] };

        // M Delta =
        // (M[0][0] * Delta[0] + M[0][1] * Delta[1] + M[0][2] * Delta[2]) +
        // (M[1][0] * Delta[0] + M[1][1] * Delta[1] + M[1][2] * Delta[2]) +
        // (M[2][0] * Delta[0] + M[2][1] * Delta[1] + M[2][2] * Delta[2]);

        double c0 = // DeltaT M Delta
        Delta[0] * (M[0][0] * Delta[0] + M[0][1] * Delta[1] + M[0][2] * Delta[2]) + Delta[1] * (M[1][0] * Delta[0] + M[1][1] * Delta[1] + M[1][2] * Delta[2]) + Delta[2] * (M[2][0] * Delta[0] + M[2][1] * Delta[1] + M[2][2] * Delta[2]);

        double c1 = // DT M Delta
        D[0] * (M[0][0] * Delta[0] + M[0][1] * Delta[1] + M[0][2] * Delta[2]) + D[1] * (M[1][0] * Delta[0] + M[1][1] * Delta[1] + M[1][2] * Delta[2]) + D[2] * (M[2][0] * Delta[0] + M[2][1] * Delta[1] + M[2][2] * Delta[2]);

        double c2 = // DT M D
        D[0] * (M[0][0] * D[0] + M[0][1] * D[1] + M[0][2] * D[2]) + D[1] * (M[1][0] * D[0] + M[1][1] * D[1] + M[1][2] * D[2]) + D[2] * (M[2][0] * D[0] + M[2][1] * D[1] + M[2][2] * D[2]);

        // dist t*D along line, solve by quadratic formula

        double a = c2;
        double b = 2 * c1;
        double c = c0;

        double b24ac = b * b - 4 * a * c;

        if (b24ac < 0) // no real roots, no intersection
            return new double[0];

        double rb24ac = Math.sqrt(b24ac);
        // otherwise, at least one root
        return new double[] { ( -b + rb24ac) / (2 * a), ( -b - rb24ac) / (2 * a) };

    }

    
    /** Find intersection points on a line of a cylinder
     * 
     * @param L  Position of a point on the line
     * @param V  Vector along line
     * @param C  Position on axis of cylinder
     * @param U  Vector along axis of cylinder
     * @param r2  Cylinder radius squared
     * @return zero or two values of t, where intersection points are L + t*V, i.e. dist t along line  
     */
    public static final double[] cylinderLineIntersection(double L[], double V[], double C[], double U[], double r2) {
        
        double UxV[] = new double[]{     U[1]*V[2] - U[2]*V[1],
                                        -U[0]*V[2] + U[2]*V[0], 
                                         U[0]*V[1] - U[1]*V[0],        };
        
        
        
        double UxCL[] = new double[]{     U[1]*(L[2]-C[2]) - U[2]*(L[1]-C[1]),
                                         -U[0]*(L[2]-C[2]) + U[2]*(L[0]-C[0]), 
                                          U[0]*(L[1]-C[1]) - U[1]*(L[0]-C[0]),        };

        double a = UxV[0]*UxV[0] + UxV[1]*UxV[1] + UxV[2]*UxV[2];
        
        double b = 2 * (UxV[0]*UxCL[0] + UxV[1]*UxCL[1] + UxV[2]*UxCL[2]);
        
        double c = UxCL[0]*UxCL[0] + UxCL[1]*UxCL[1] + UxCL[2]*UxCL[2] - r2;
        
        double b24ac = b * b - 4 * a * c;

        if (b24ac <= 0) // no real roots, no intersection, also covers u // v case
            return new double[0];

        double rb24ac = Math.sqrt(b24ac);
        
        // otherwise, at least one root
        return new double[] { ( -b + rb24ac) / (2 * a), ( -b - rb24ac) / (2 * a) };
        
    }

    /** Returns value of s along the line  X = A + s.u that is closest to the line X' = B + t.v
     * 
     * Get this by writing d^2 = (X - Y).(X - Y), expanding the dot product and finding dd^2/ds = 0
     * 
     * @param A Start of line 1 
     * @param u Unit vector of line 1
     * @param B Start of line 2
     * @param v Unit vector of line 2
     * @return Distance along line 1 from A in +ve u, that is closest to any point on line 2.
     */
    public static final double pointOnLineNearestAnotherLine(double A[], double u[], double B[], double v[]){

        double C[] = new double[]{ A[0]-B[0], A[1]-B[1], A[2]-B[2] };
        
        double uv = u[0]*v[0] + u[1]*v[1] + u[2]*v[2];
        double Cv = C[0]*v[0] + C[1]*v[1] + C[2]*v[2];
        double Cu = C[0]*u[0] + C[1]*u[1] + C[2]*u[2];
        
        return (Cv*uv - Cu) / (1 - uv*uv);
        
    }
    
    public static final double[] vec3dMinus(double a[], double b[]){ 
        return new double[]{ a[0]-b[0], a[1]-b[1], a[2]-b[2] };
    }
    
    /**  returns square distance, fills p with point on circle */
   /*Doesn't seem to work.
    *  public static final double pointOnCircleNearestPoint3D(double p[], double cc[], double cn[], double cr){
    
        // Signed distance from point to plane of circle.
        double diff0[] = vec3dMinus(p, cc);
        double dist = dot(diff0, cn);

        // Projection of P-C onto plane is Q-C = P-C - (fDist)*N.
        double diff1[] = new double[]{ 
            diff0[0] - dist*cn[0], 
            diff0[1] - dist*cn[1], 
            diff0[2] - dist*cn[2] };
        double sqrLen = dot(diff1, diff1);
       
        //if (sqrLen >= 1e-10){
            double len = FastMath.sqrt(sqrLen);
            double mClosestPoint1[] = new double[]{
                cc[0] + (cr / len)*diff1[0],
                cc[1] + (cr / len)*diff1[1],
                cc[2] + (cr / len)*diff1[2]
            };
            
            double diff2[] = vec3dMinus(p, mClosestPoint1);
            
            p[0] = mClosestPoint1[0];
            p[1] = mClosestPoint1[1];
            p[2] = mClosestPoint1[2];
            
            return dot(diff2, diff2);
            
       /* } else { //intersect??
            double mClosestPoint1[] = new double[]{ 
                Double.POSITIVE_INFINITY,
                Double.POSITIVE_INFINITY,
                Double.POSITIVE_INFINITY,
            };
            sqrDistance = cr*cr + dist*dist;
        }

        double mClosestPoint0[] = p;*/
    /*
    }
    */
 /*Not finished
  *    public static final double[] pointOnCircleNearestLine3D(double l0[], double l[], double cc[], double cn[], double cr){
 
        double diff[] = vec3dMinus(l0, cc);
        double diffSqrLen = dot(diff, diff);
        double MdM = dot(l, l);
        double DdM = dot(diff, l);
        double NdM = dot(cn, l);
        double DdN = dot(diff, cn);

        double a0 = DdM;
        double a1 = MdM;
        double b0 = DdM - NdM*DdN;
        double b1 = MdM - NdM*NdM;
        double c0 = diffSqrLen - DdN*DdN;
        double c1 = b0;
        double c2 = b1;
        double rsqr = cr*cr;

        double a0sqr = a0*a0;
        double a1sqr = a1*a1;
        double twoA0A1 = 2.0*a0*a1;
        double b0sqr = b0*b0;
        double b1Sqr = b1*b1;
        double twoB0B1 = 2.0*b0*b1;
        double twoC1 = 2.0*c1;

        // The minimum point B+t*M occurs when t is a root of the quartic
        // equation whose coefficients are defined below.
        Polynomial1<Real> poly(4);
        poly[0] = a0sqr*c0 - b0sqr*rsqr;
        poly[1] = twoA0A1*c0 + a0sqr*twoC1 - twoB0B1*rsqr;
        poly[2] = a1sqr*c0 + twoA0A1*twoC1 + a0sqr*c2 - b1Sqr*rsqr;
        poly[3] = a1sqr*twoC1 + twoA0A1*c2;
        poly[4] = a1sqr*c2;

        PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
        polyroots.FindB(poly, 6);
        int count = polyroots.GetCount();
        double roots[] = polyroots.GetRoots();

        double minSqrDist = Double.POSITIVE_INFINITY;
        for(int i= 0; i < count; ++i)
        {
            // Compute distance from P(t) to circle.
            double P[] = new double[]{ 
                l0[0] + roots[i]*l[0], 
                l0[1] + roots[i]*l[1], 
                l0[2] + roots[i]*l[2], 
            };
            
            DistPoint3Circle3<Real> query(P, *mCircle);
            Real sqrDist = query.GetSquared();
            if (sqrDist < minSqrDist)
            {
                minSqrDist = sqrDist;
                mClosestPoint0 = query.GetClosestPoint0();
                mClosestPoint1 = query.GetClosestPoint1();
            }
        }

        return minSqrDist;
    }
*/
    
    /** @return double[]{ evec1[], evec2[], {eval1, eval2 } } 
     * 
     * */ 
    public static final double[][] eigenVecsAndVals2x2(double A[][]){
        double T = A[0][0] + A[1][1];
        double D = A[0][0]*A[1][1] - A[0][1]*A[1][0];
        
        double rt = FastMath.sqrt(T*T/4 - D);
        double l1 = T/2 + rt;
        double l2 = T/2 - rt;
        
        double v1x, v1y, v2x, v2y;
        if(A[1][0] == 0){
            if(A[0][1] == 0){
                v1x = 1; v1y = 0; l1 = A[0][0];
                v2x = 0; v2y = 1; l2 = A[1][1];
            }else{
                v1x = FastMath.sqrt(A[0][1]*A[0][1] / ((A[0][0] - l1)*(A[0][0] - l1) + A[0][1]*A[0][1]));
                v1y = (l1 - A[0][0])*v1x / A[0][1];
                
                v2x = FastMath.sqrt(A[0][1]*A[0][1] / ((A[0][0] - l2)*(A[0][0] - l2) + A[0][1]*A[0][1]));
                v2y = (l2 - A[0][0])*v2x / A[0][1];
            }
        }else{
            v1y = FastMath.sqrt(A[1][0]*A[1][0] / ((l1 - A[1][1])*(l1 - A[1][1]) + A[1][0]*A[1][0]));
            v1x = (l1 - A[1][1])*v1y / A[1][0];

            v2y = FastMath.sqrt(A[1][0]*A[1][0] / ((l2 - A[1][1])*(l2 - A[1][1]) + A[1][0]*A[1][0]));
            v2x = (l2 - A[1][1])*v2y / A[1][0];
        }

        return new double[][]{ {v1x, v1y}, { v2x, v2y}, {l1, l2 } };
        
    }


}