Package stallone.api.coordinates

Source Code of stallone.api.coordinates.CoordinateUtilities3D

/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package stallone.api.coordinates;

import static stallone.api.API.*;
import stallone.api.doubles.IDoubleArray;
import stallone.util.Arguments;
import stallone.util.MathTools;

/**
*
* @author noe
*/
public class CoordinateUtilities3D
{

    static long t;

    /** copies the contents of v1 into v2
     */
    public static void copy(double[] v1, double[] v2)
    {
        v2[0] = v1[0];
        v2[1] = v1[1];
        v2[2] = v1[2];
    }

    /** adds two vectors
     */
    public static double[] add(double[] v1, double[] v2)
    {
        double[] v = new double[3];
        v[0] = v1[0] + v2[0];
        v[1] = v1[1] + v2[1];
        v[2] = v1[2] + v2[2];
        return (v);
    }

    /** increments vector v1 by v2
     */
    public static void increment(double[] v1, double[] v2)
    {
        v1[0] += v2[0];
        v1[1] += v2[1];
        v1[2] += v2[2];
    }

    /** decrements vector v1 by v2
     */
    public static void decrement(double[] v1, double[] v2)
    {
        v1[0] -= v2[0];
        v1[1] -= v2[1];
        v1[2] -= v2[2];
    }

    /** computes the vector a*v1 + b*v2
     */
    public static double[] addweighted(double a, double[] v1,
            double b, double[] v2)
    {
        double[] v = new double[3];
        v[0] = a * v1[0] + b * v2[0];
        v[1] = a * v1[1] + b * v2[1];
        v[2] = a * v1[2] + b * v2[2];
        return (v);
    }

    /** subtracts two vectors v1-v2
     */
    public static double[] subtract(double[] v1, double[] v2)
    {
        double[] v = new double[3];
        v[0] = v1[0] - v2[0];
        v[1] = v1[1] - v2[1];
        v[2] = v1[2] - v2[2];
        return (v);
    }

    /** subtracts two vectors v1-v2, writes into res*/
    public static void subtract2(double[] v1, double[] v2, double[] res)
    {
        res[0] = v1[0] - v2[0];
        res[1] = v1[1] - v2[1];
        res[2] = v1[2] - v2[2];
    }

    /** returns a*v, where v is a 3-dimensional vector*/
    public static double[] stretch(double a, double[] v)
    {
        double[] v1 = new double[3];
        v1[0] = a * v[0];
        v1[1] = a * v[1];
        v1[2] = a * v[2];
        return (v1);
    }

    /** computes the dot product v1*v2.*/
    public static double dotprod(double[] v1, double[] v2)
    {
        return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
    }

    public static double overlap(double[][] V1, double[][] V2)
    {
        double o = 0;
        for (int i = 0; i < V1.length; i++)
        {
            for (int j = 0; j < V2.length; j++)
            {
                o += Math.pow(doubleArrays.dot(normalize(V1[i]),
                        normalize(V2[j])), 2.0);
            }
        }
        return (o);
    }

    /**
    Determines the dimensionality of overlap between two spaces spanned
    by the vectors V1 and V2, compared to the larger of the two dimensions
    1/max(|V1|,|V2|) * sum_V1 sum_V2 dot(v1,v2)
    which is 0 for orthogonal and 1 for coplanar spaces.
     */
    public static double subspaceOverlap(double[][] V1, double[][] V2)
    {
        return (overlap(V1, V2) / Math.max(V1.length, V2.length));
    }

    /**
    Determines the dimensionality of overlap between two spaces spanned
    by the vectors V1 and V2, compared to the smaller of the two dimensions
    1/min(|V1|,|V2|) * sum_V1 sum_V2 dot(v1,v2)
    which is 0 for orthogonal and 1 for coplanar spaces.
     */
    public static double superspaceOverlap(double[][] V1, double[][] V2)
    {
        return (overlap(V1, V2) / Math.min(V1.length, V2.length));
    }

    /** general cross product function for vectors in three dimensions
     */
    public static double[] crossprod(double[] v1, double[] v2)
    {
        double[] v = new double[3];
        v[0] = v1[1] * v2[2] - v1[2] * v2[1];
        v[1] = v1[2] * v2[0] - v1[0] * v2[2];
        v[2] = v1[0] * v2[1] - v1[1] * v2[0];
        return (v);
    }

    /**
    computes the unit normal vector on the plane p1p2p3 as
    normalize(p2p1 x p2p3)
     */
    public static double[] normal0(double[] p1, double[] p2, double[] p3)
    {
        double[] v21 = subtract(p1, p2);
        double[] v23 = subtract(p3, p2);
        return (normalize(crossprod(v21, v23)));
    }

    /** returns distance between two points*/
    public static double squaredistance(double[] v1, double[] v2)
    {
        double dx = v1[0] - v2[0];
        double dy = v1[1] - v2[1];
        double dz = v1[2] - v2[2];
        return (dx * dx + dy * dy + dz * dz);
    }

    /** returns distance between two points*/
    public static double distance(double[] v1, double[] v2)
    {
        double dx = v1[0] - v2[0];
        double dy = v1[1] - v2[1];
        double dz = v1[2] - v2[2];
        return (Math.sqrt(dx * dx + dy * dy + dz * dz));
    }

    public static double[] distances(double[][] arr, int[][] partners)
    {
        double[] res = new double[partners.length];
        for (int i = 0; i < res.length; i++)
        {
            res[i] = distance(arr[partners[i][0]], arr[partners[i][1]]);
        }
        return (res);
    }

    public static double distancePointSurface(double[] o, double[] surfv1, double[] surfv2, double[] p)
    {
        double[] n = crossprod(surfv1, surfv2);
        double[][] M =
        {
            doubleArrays.normalize(surfv1),
            doubleArrays.normalize(surfv2),
            doubleArrays.normalize(n)
        };
        M = doubleArrays.transpose(M);
        IDoubleArray arrM = doublesNew.array(M);
        double[] o_new = alg.solve(arrM, doublesNew.array(o)).getArray();
        double[] p_new = alg.solve(arrM, doublesNew.array(p)).getArray();
        double[] x = doubleArrays.subtract(p_new, o_new);
        return (Math.abs(x[2]));
    }

    /** general function to compute angle between two vectors*/
    public static double angleRad(double[] v1, double[] v2)
    {
        double sqnorm = Math.sqrt(dotprod(v1, v1) * dotprod(v2, v2));
        if (sqnorm == 0)
        {
            return (0.0);
        }
        else
        {
            return (MathTools.acos(MathTools.bound(dotprod(v1, v2) / sqnorm, -1, 1)));
        }
    }


    /** general function to compute angle between two vectors in degrees
     */
    public static double angleDeg(double[] v1, double[] v2)
    {
        double a = MathTools.acos(dotprod(v1, v2) / (norm(v1) * norm(v2)));
        return (180.0 * a / Math.PI);
    }

    /** compute angle between three points p1,p2,p3
    as angle between p2->p1 and p2->p3*/
    public static double angleRad(double[] p1, double[] p2, double[] p3)
    {
        double[] v1 =
        {
            p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]
        };
        double[] v2 =
        {
            p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]
        };
        double result = angleRad(v1, v2);
        return (result);
    }

    /** compute angle between three points p1,p2,p3
    as angle between p2->p1 and p2->p3*/
    public static double angleDeg(double[] p1, double[] p2, double[] p3)
    {
        double[] v1 =
        {
            p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]
        };
        double[] v2 =
        {
            p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]
        };
        double result = angleDeg(v1, v2);
        return (result);
    }

    /** torsion angle by 3-dimensional coordinates in radians!*/
    public static double torsionRad(double[] p1, double[] p2,
            double[] p3, double[] p4)
    {
        double[] v21 = subtract(p1, p2);
        double[] v23 = subtract(p3, p2);
        double[] cv1 = crossprod(v21, v23);
        double[] v32 = subtract(p2, p3);
        double[] v34 = subtract(p4, p3);
        double[] cv2 = crossprod(v32, v34);
        double angle = angleRad(cv1, cv2);
        if (dotprod(v34, cv1) > 0)
        {
            angle = -angle;
        }
        return (angle);
    }

    /** torsion angle by 3-dimensional coordinates in degrees!*/
    public static double torsionDeg(double[] p1, double[] p2,
            double[] p3, double[] p4)
    {
        return (180.0 * torsionRad(p1, p2, p3, p4) / Math.PI);
    }

    public static double torsionDeg(double[][] crd,
            int i1, int i2, int i3, int i4)
    {
        return (torsionDeg(crd[i1], crd[i2], crd[i3], crd[i4]));
    }

    public static double torsionDeg(double[][] crd, int[] indexes)
    {
        return (torsionDeg(crd[indexes[0]], crd[indexes[1]],
                crd[indexes[2]], crd[indexes[3]]));
    }

    public static double[] torsionsDeg(double[][] crd, int[][] indexes)
    {
        double[] res = new double[indexes.length];
        for (int i = 0; i < res.length; i++)
        {
            res[i] = torsionDeg(crd[indexes[i][0]], crd[indexes[i][1]],
                    crd[indexes[i][2]], crd[indexes[i][3]]);
        }
        return (res);
    }

    /**
    For a planar triangle 1,2,3, computes d13, based on d12,d23 and
    theta23 = angle(d12,d23) using the cosine theorem, i.e.:
    d13^2 = d12^2 + d23^2 + d12 d23 cos(theta23)
    @param theta23 the angle between d12, d23 in radians.
    @return c^2 (not c !!!!)
     */
    public static double cosineTheorem(double d12, double d23, double theta123)
    {
        return (d12 * d12 + d23 * d23 - 2 * d12 * d23 * Math.cos(theta123));
    }

    /**
    Computes the 1-4 distance in an arbitrary (nonplanar) quadrangle,
    given d12, d13, d14, theta123 = angle(1,2,3), theta234 = angle(2,3,4)
    and phi = angle(plane(1,2,3), plane(2,3,4)).
    @param theta123 the angle between 1,2,3 in radians.
    @param theta234 the angle between 2,3,4 in radians.
    @param phi the angle between the planes (1,2,3) and (2,3,4), in radians
    @return d14
     */
    public static double dist14(double d12, double d23, double d34,
            double theta123, double theta234, double phi)
    {
        double alpha123 = Math.PI - theta123;
        double alpha234 = Math.PI - theta234;
        double a1 = Math.cos(alpha123) * d12;
        double a2 = Math.sin(alpha123) * d12;
        double b1 = Math.cos(alpha234) * d34;
        double b2 = Math.sin(alpha234) * d34;
        double c = a1 + d23 + b1;
        double e_sq = cosineTheorem(a2, b2, phi);
        return (Math.sqrt(c * c + e_sq));
    }

    /** returns the norm of the vector*/
    public static double norm(double[] v)
    {
        return (Math.sqrt(dotprod(v, v)));
    }

    /** returns vector v normalized */
    public static double[] normalize(double[] v)
    {
        double[] v0 = new double[3];
        double n = norm(v);
        v0[0] = v[0] / n;
        v0[1] = v[1] / n;
        v0[2] = v[2] / n;
        return (v0);
    }

    /** Translate the coordinate set by [x,y,z]. */
    public static void translate(double[][] crds,
            double x, double y, double z)
    {
        for (int i = 0; i < crds.length; i++)
        {
            crds[i][0] = crds[i][0] + x;
            crds[i][1] = crds[i][1] + y;
            crds[i][2] = crds[i][2] + z;
        }
    }

    /** Translate some selected coordinates out of crds by [x,y,z]
    crds: the coordinate array
    select: a list with the point indexes which are to be translated
     */
    public static void translate(double[][] crds, int[] select,
            double x, double y, double z)
    {
        for (int i = 0; i < select.length; i++)
        {
            crds[select[i]][0] = crds[select[i]][0] + x;
            crds[select[i]][1] = crds[select[i]][1] + y;
            crds[select[i]][2] = crds[select[i]][2] + z;
        }
    }

    /** Computes a 3x3 rotation matrix for a rotation around the
    3D-vector r clockwise by an angle psi, which is returned
    explicitly in the matrix expression.
    psi: angle in radians
     */
    public static double[][] rotationMatrix(double[] r, double psi)
    {
        double[] r0 = normalize(r);
        double[][] Mr = new double[3][3];
        double cospsi = Math.cos(psi);
        double sinpsi = Math.sin(psi);
        double one_cospsi = 1 - cospsi;

        Mr[0][0] = r0[0] * r0[0] * one_cospsi + cospsi;
        Mr[0][1] = r0[0] * r0[1] * one_cospsi - r0[2] * sinpsi;
        Mr[0][2] = r0[0] * r0[2] * one_cospsi + r0[1] * sinpsi;
        Mr[1][0] = r0[0] * r0[1] * one_cospsi + r0[2] * sinpsi;
        Mr[1][1] = r0[1] * r0[1] * one_cospsi + cospsi;
        Mr[1][2] = r0[1] * r0[2] * one_cospsi - r0[0] * sinpsi;
        Mr[2][0] = r0[0] * r0[2] * one_cospsi - r0[1] * sinpsi;
        Mr[2][1] = r0[1] * r0[2] * one_cospsi + r0[0] * sinpsi;
        Mr[2][2] = r0[2] * r0[2] * one_cospsi + cospsi;
        return (Mr);
    }

    /**
    multiplies two 3x3 - matrixes
    NOT TESTED!!
     */
    public static double[][] matrixMultiply(double[][] M1, double[][] M2)
    {
        double[][] M = new double[3][3];
        M[0][0] = M1[0][0] * M2[0][0] + M1[0][1] * M2[1][0] + M1[0][2] * M2[2][0];
        M[0][1] = M1[0][0] * M2[0][1] + M1[0][1] * M2[1][1] + M1[0][2] * M2[2][1];
        M[0][2] = M1[0][0] * M2[0][2] + M1[0][1] * M2[1][2] + M1[0][2] * M2[2][2];
        M[1][0] = M1[1][0] * M2[0][0] + M1[1][1] * M2[1][0] + M1[1][2] * M2[2][0];
        M[1][1] = M1[1][0] * M2[0][1] + M1[1][1] * M2[1][1] + M1[1][2] * M2[2][1];
        M[1][2] = M1[1][0] * M2[0][2] + M1[1][1] * M2[1][2] + M1[1][2] * M2[2][2];
        M[2][0] = M1[2][0] * M2[0][0] + M1[2][1] * M2[1][0] + M1[2][2] * M2[2][0];
        M[2][1] = M1[2][0] * M2[0][1] + M1[2][1] * M2[1][1] + M1[2][2] * M2[2][1];
        M[2][2] = M1[2][0] * M2[0][2] + M1[2][1] * M2[1][2] + M1[2][2] * M2[2][2];
        return (M);
    }

    /**
    computes the 3x3 matrix determinant
    NOT TESTED!!
     */
    public static double det(double[][] M)
    {
        double res =
                M[0][0] * M[1][1] * M[2][2]
                + M[0][1] * M[1][2] * M[2][0]
                + M[0][2] * M[1][0] * M[2][1]
                - M[0][2] * M[1][1] * M[2][0]
                - M[0][1] * M[1][0] * M[2][2]
                - M[0][0] * M[1][2] * M[2][1];
        return (res);
    }

    /**
    applies the matrix M to all selected coordinates
    NOT TESTED!!
     */
    public static void applyMatrix(double[][] crds, double[][] M,
            int[] selection)
    {
        for (int i = 0; i < selection.length; i++)
        {
            crds[selection[i]][0] =
                    crds[selection[i]][0] * M[0][0]
                    + crds[selection[i]][1] * M[0][1]
                    + crds[selection[i]][2] * M[0][2];
            crds[selection[i]][1] =
                    crds[selection[i]][0] * M[1][0]
                    + crds[selection[i]][1] * M[1][1]
                    + crds[selection[i]][2] * M[1][2];
            crds[selection[i]][2] =
                    crds[selection[i]][0] * M[2][0]
                    + crds[selection[i]][1] * M[2][1]
                    + crds[selection[i]][2] * M[2][2];
        }
    }

    /** rotates selected points out of a coordinate set rightwise
    by angle psi around axis r1->r2.
    crds: a 3xN coordinate matrix
    movingAtoms: an array of indexes referring to crds.
    Specifies the points which are moved in the rotation
    nAtoms: the number of Atoms in the list movingAtoms
    r1,r2: rotation axis r1->r2
    psi: angle in radians
     */
    public static void rotatePointsRad(double[][] crds, int[] movingAtoms, int nMoving,
            double[] r1, double[] r2, double psiRad)
    {
        double[][] Mr = rotationMatrix(subtract(r2, r1), psiRad);
        double rx, ry, rz;
        int a;

        for (int i = 0; i < nMoving; i++)
        {
            a = movingAtoms[i];
            // translate:
            crds[a][0] = crds[a][0] - r2[0];
            crds[a][1] = crds[a][1] - r2[1];
            crds[a][2] = crds[a][2] - r2[2];
            // rotate:
            rx = Mr[0][0] * crds[a][0] + Mr[0][1] * crds[a][1]
                    + Mr[0][2] * crds[a][2];
            ry = Mr[1][0] * crds[a][0] + Mr[1][1] * crds[a][1]
                    + Mr[1][2] * crds[a][2];
            rz = Mr[2][0] * crds[a][0] + Mr[2][1] * crds[a][1]
                    + Mr[2][2] * crds[a][2];
            // translate back:
            crds[a][0] = rx + r2[0];
            crds[a][1] = ry + r2[1];
            crds[a][2] = rz + r2[2];
        }
    }

    /** same as above, rotates all moving Atoms */
    public static void rotatePointsRad(double[][] crds, int[] movingAtoms,
            double[] r1, double[] r2, double psiRad)
    {
        rotatePointsRad(crds, movingAtoms, movingAtoms.length,
                r1, r2, psiRad);
    }

    /** same as above only one atom
     */
    public static void rotateRad(double[] crd, double[] r1, double[] r2, double psiRad)
    {
        double[][] crds =
        {
            crd
        };
        int[] movingAtoms =
        {
            0
        };
        rotatePointsRad(crds, movingAtoms, 1, r1, r2, psiRad);
    }

    public static void rotateDeg(double[] crd, double[] r1, double[] r2, double psiDeg)
    {
        rotateRad(crd, r1, r2, Math.PI * psiDeg / 180.0);
    }

    /** same as above, but psi is given in degrees
     */
    public static void rotatePointsDeg(double[][] crds, int[] movingAtoms, int nMoving,
            double[] r1, double[] r2, double psiDeg)
    {
        rotatePointsRad(crds, movingAtoms, nMoving,
                r1, r2, Math.PI * psiDeg / 180.0);
    }

    /** same as above, but psi is given in degrees
     */
    public static void rotatePointsDeg(double[][] crds, int[] movingAtoms,
            double[] r1, double[] r2, double psiDeg)
    {
        rotatePointsRad(crds, movingAtoms, movingAtoms.length,
                r1, r2, Math.PI * psiDeg / 180.0);
    }

    /** returns the possible solutions for a point p whose distances
    d1,d2,d3 to three points p1,p2,p3 is given.
    d1,d2,d3: scalar distances
    p1,p2,p3: three-dimensional vectors
    s1,s2: two 3-dimensional double arrays for the solutions
    returns true, if successful, false otherwise
     */
    public static boolean triangulate(double[] p1, double[] p2, double[] p3,
            double d1, double d2, double d3,
            double[] s1, double[] s2)
    {
        double[] v12;
        double[] v120;
        double[] v13;
        double[] v23;
        double[] pF;
        double[] vi;
        double[] vi0;
        double[] vFF;
        double[] vF3;
        double[] pF_2;
        double[] pF_3;
        double[] vN;
        double[] vN0;
        double d12sq, d12, d13sq, d13, d23sq, d23, arg, dF, r, dF_2;
        double dp3flat, dFFsq, dF3, beta, d3v12, dF_3, r_3;
        // compute some values
        v12 = subtract(p2, p1);
        d12sq = dotprod(v12, v12);
        d12 = Math.sqrt(d12sq);
        v120 = stretch(1 / d12, v12);
        v13 = subtract(p3, p1);
        d13sq = dotprod(v13, v13);
        d13 = Math.sqrt(d13sq);
        v23 = subtract(p3, p2);
        d23sq = dotprod(v23, v23);
        d23 = Math.sqrt(d23sq);
        // Winkel zwischen p und v12
        arg = (-d2 * d2 + d1 * d1 + d12sq) / (2 * d1 * d12);
        if (arg < -1 || arg > 1)
        {
            return (false);
        }
        // Abstand und Fusspunkt zu v12
        dF = d1 * arg;
        r = d1 * Math.sin(MathTools.acos(arg));
        pF = addweighted(1, p1, dF, v120);
        // Vektor, der senkrecht auf v12 steht und nach p3 zeigt:
        dF_2 = (-d13sq + d12sq + d23sq) / (2 * d12);
        pF_2 = addweighted(1, p2, -dF_2, v120);
        vi = subtract(p3, pF_2);
        vi0 = normalize(vi);
        vFF = subtract(pF_2, pF);
        dFFsq = dotprod(vFF, vFF);
        // Abstand von p auf p3 in der vi-ebene
        arg = d3 * d3 - dFFsq;
        if (arg < 0)
        {
            return (false);
        }
        dp3flat = Math.sqrt(arg);
        vF3 = subtract(p3, pF);
        dF3 = norm(vF3);
        beta = angleRad(vi, vF3);
        d3v12 = Math.cos(beta) * dF3;
        // Winkel zwischen vi und p
        arg = (-dp3flat * dp3flat + r * r + d3v12 * d3v12) / (2 * r * d3v12);
        if (arg < -1 || arg > 1)
        {
            return (false);
        }
        dF_3 = r * arg;
        r_3 = r * Math.sin(MathTools.acos(arg));
        pF_3 = addweighted(1, pF, dF_3, vi0);
        // Senkrechte auf p1-p2-p3-Ebene und p berechnen
        vN = crossprod(v12, v13);
        vN0 = normalize(vN);
        s1[0] = pF_3[0] + r_3 * vN0[0];
        s1[1] = pF_3[1] + r_3 * vN0[1];
        s1[2] = pF_3[2] + r_3 * vN0[2];
        s2[0] = pF_3[0] - r_3 * vN0[0];
        s2[1] = pF_3[1] - r_3 * vN0[1];
        s2[2] = pF_3[2] - r_3 * vN0[2];
        return (true);
    }

    /**
    @return the indexes of all points which are equal in both arrays
    up to a given tolerance.
     */
    public static int[] equalPoints(double[][] arr1, double[][] arr2, double tol)
    {
        if (arr1.length != arr2.length)
        {
            throw (new IllegalArgumentException("incompatible arrays"));
        }
        int[] ep = new int[arr1.length];
        int k = 0;
        for (int i = 0; i < arr1.length; i++)
        {
            if (distance(arr1[i], arr2[i]) <= tol)
            {
                ep[k++] = i;
            }
        }
        return (intArrays.subarray(ep, 0, k));
    }


    /* obtain, from a set of points the index of the one which is
    closest to a reference point
    p the reference points
    points the set of points*/
    public static int closestPoint(double[] p, double[][] points,
            int nPoints)
    {
        int si = 0, i = 0;
        double[] d = subtract(p, points[0]);
        double ssd = dotprod(d, d);
        double sd;

        for (i = 1; i < nPoints; i++)
        {
            d = subtract(p, points[i]);
            sd = dotprod(d, d);
            if (sd < ssd)
            {
                ssd = sd;
                si = i;
            }
        }

        return (si);
    }

    /*
    Returns an array with upper and lower bounds for the x, y, and
    z-direction of the coordinate set.
    Form: [xmin, xmax, ymin, ymax, zmin, zmax]
     */
    public static double[] bounds(double[][] crds)
    {
        double[] b =
        {
            crds[0][0], crds[0][0],
            crds[0][1], crds[0][1],
            crds[0][2], crds[0][2]
        };

        for (int i = 1; i < crds.length; i++)
        {
            if (crds[i][0] < b[0])
            {
                b[0] = crds[i][0];
            }
            if (crds[i][0] > b[1])
            {
                b[1] = crds[i][0];
            }
            if (crds[i][1] < b[2])
            {
                b[2] = crds[i][1];
            }
            if (crds[i][1] > b[3])
            {
                b[3] = crds[i][1];
            }
            if (crds[i][2] < b[4])
            {
                b[4] = crds[i][2];
            }
            if (crds[i][2] > b[5])
            {
                b[5] = crds[i][2];
            }
        }

        return (b);
    }

    /**
    ERROR: May overestimate distance for segments as it does not consider
    that the nearest points may be within the segments.
     */
    /*private static double lineDistance
    (double[] a1, double[] a2, double[] b1, double[] b2,
    boolean segmentsOnly)
    {
    double[] a12 = subtract(a2,a1);
    double[] b12 = subtract(b2,b1);
    double[] a1b1 = subtract(b1,a1);
    double A = - dotprod(a12,a12);
    double B = dotprod(b12,a12);
    double C = - dotprod(a12,a1b1);
    double D = - dotprod(a12,b12);
    double E = dotprod(b12,b12);
    double F = - dotprod(b12,a1b1);
    double[][] M = {{A,B},{D,E}};
    double[][] b = {{C},{F}};
    DenseDoubleMatrix2D matM = new DenseDoubleMatrix2D(M);
    Algebra alg = new Algebra();

    double[][] X = {{0},{0}};

    if (alg.det(matM) == 0) // vectors parallel. choose X[0][0] = 0
    X[1][0] = - dotprod(a12,a1b1)/dotprod(a12,b12);
    else
    {
    DenseDoubleMatrix2D matb = new DenseDoubleMatrix2D(b);
    LUDecomposition lud = new LUDecomposition(matM);
    X = (lud.solve(matb).toArray());
    }

    if (segmentsOnly &&
    ((X[0][0] < 0.0) || (X[0][0] > 1.0) ||
    (X[1][0] < 0.0) || (X[1][0] > 1.0)))
    {
    double[] possible = {distance(a1,b1), distance(a2,b2),
    distance(a1,b2), distance(a2,b1),
    pointSegmentDistance(a1,b1,b12),
    pointSegmentDistance(a2,b1,b12),
    pointSegmentDistance(b1,a1,a12),
    pointSegmentDistance(b2,a1,a12)};
    return(doubleArrays.min(possible));
    }
    else
    {
    double[] L1 = addweighted(1.0, a1, X[0][0], a12);
    double[] L2 = addweighted(1.0, b1, X[1][0], b12);
    return(distance(L1, L2));
    }
    }*/
    public static double pointLineDistance(double[] p, double[] a, double[] va)
    {
        double h = dotprod(va, subtract(p, a)) / dotprod(va, va);
        double[] L = addweighted(1.0, a, h, va);
        return (distance(L, p));
    }

    public static double pointSegmentDistance(double[] p, double[] a, double[] va)
    {
        double h = dotprod(va, subtract(p, a)) / dotprod(va, va);
        h = Math.min(h, 1.0);
        h = Math.max(h, 0.0);
        double[] L = addweighted(1.0, a, h, va);
        return (distance(L, p));
    }

    /**
    returns -1, 0, 1 if the lines specified by a1->a2, b1->b2 are "below"
    "in" or "above" the plane normal to the vector connecting their nearest
    points
     */
    private static int lineOrientation(double[] a1, double[] a2, double[] b1, double[] b2)
    {
        double[] a12 = subtract(a2, a1);
        double[] b12 = subtract(b2, b1);
        double[] a1b1 = subtract(b1, a1);
        double[] n = crossprod(a12, b12);
        return (MathTools.sign(dotprod(n, a1b1)));
    }

    /*public static double lineDistance
    (double[] a1, double[] a2, double[] b1, double[] b2)
    {return(lineDistance(a1,a2,b1,b2,false));}

    public static double segmentDistance
    (double[] a1, double[] a2, double[] b1, double[] b2)
    {return(lineDistance(a1,a2,b1,b2,true));}
     *
     */

    /*
    private static boolean lineCrossing
    (double[] a11, double[] a21, double[] b11, double[] b21,
    double[] a12, double[] a22, double[] b12, double[] b22,
    boolean segmentsOnly)
    {
    int[] orient = {lineOrientation(a11,a21,b11,b21),
    lineOrientation(a12,a22,b12,b22)};
    if (orient[0] == orient[1])
    return(false); // lines do not cross - no distance minimum within
    if (!segmentsOnly)
    return(true); // lines cross somewhere

    double[] va1 = subtract(a12,a11);
    double[] va2 = subtract(a22,a21);
    double[] vb1 = subtract(b12,b11);
    double[] vb2 = subtract(b22,b21);

    // determine distance minimum by bracketing the line orientation
    double tolSeg = 0.01;
    double tolLin = 1e-4;
    double[] T = {0,1};
    double t = 0;
    double d = lineDistance(a11,a21,b11,b21);
    double ds = segmentDistance(a11,a21,b11,b21);
    if (ds < tolSeg)
    return(true);

    for (int i=0; (i<1000) && (d > tolLin); i++)
    {
    t = 0.5*(T[1]+T[0]);
    double[] a1t = addweighted(1,a11,t,va1);
    double[] a2t = addweighted(1,a21,t,va2);
    double[] b1t = addweighted(1,b11,t,vb1);
    double[] b2t = addweighted(1,b21,t,vb2);
    int o = lineOrientation(a1t,a2t,b1t,b2t);
    double dnew = lineDistance(a1t,a2t,b1t,b2t);
    if (o == orient[0])
    T[0] = t;
    else
    T[1] = t;
    if (dnew < d)
    d = dnew;

    //System.out.println(t+"\t"+d+"\t"+segmentDistance(a1t,a2t,b1t,b2t));

    if (d <= tolLin)
    ds = segmentDistance(a1t,a2t,b1t,b2t);
    }


    if (ds < tolSeg)
    return(true); // yes, crossing in segment!
    else
    return(false); // no, crosses somewhere else
    }

    public static boolean lineCrossing
    (double[] a11, double[] a12, double[] a21, double[] a22,
    double[] b11, double[] b12, double[] b21, double[] b22)
    {return(lineCrossing(a11,a12,a21,a22,b11,b12,b21,b22,false));}

    public static boolean segmentCrossing
    (double[] a11, double[] a12, double[] a21, double[] a22,
    double[] b11, double[] b12, double[] b21, double[] b22)
    {return(lineCrossing(a11,a12,a21,a22,b11,b12,b21,b22,true));}
     */
    /**
    Transforms the selected (sel) group of points in referenceCrds in the same way
    as the transformation of the rigid body (i1-i2-i3) describes from
    referenceCrds to buildCrds and puts the resulting coordinates in buildCrds.
     */
    public static void transformRigid(double[][] buildCrds, double[][] referenceCrds,
            int i1, int i2, int i3, int[] sel)
    {
        if (i1 < 0 || i2 < 0 || i3 < 0)
        {
            throw (new RuntimeException("Trying to build transform nonexisting pos."));
        }

        for (int i = 0; i < sel.length; i++)
        {
            copy(referenceCrds[sel[i]], buildCrds[sel[i]]);
        }

        double[] p10 = buildCrds[i1];
        double[] p1r = referenceCrds[i1];
        double[] p20 = buildCrds[i2];
        double[] p2r = referenceCrds[i2];
        double[] p30 = buildCrds[i3];
        double[] p3r = referenceCrds[i3];
        double[] p1copy = doubleArrays.copy(p1r);

        double[] t = CoordinateUtilities3D.subtract(p20, p2r);
        CoordinateUtilities3D.translate(buildCrds, sel, t[0], t[1], t[2]);
        p1copy = CoordinateUtilities3D.add(p1copy, t);

        double[] n0 = CoordinateUtilities3D.crossprod(CoordinateUtilities3D.subtract(p20, p10),
                CoordinateUtilities3D.subtract(p20, p30));
        double[] vCaNr = CoordinateUtilities3D.subtract(p2r, p3r);
        double[] nr = CoordinateUtilities3D.crossprod(CoordinateUtilities3D.subtract(p2r, p1r),
                vCaNr);
        double[] vrot1 = CoordinateUtilities3D.crossprod(nr, n0);
        double[] virt1 = CoordinateUtilities3D.add(p20, vrot1);
        double arot1 = CoordinateUtilities3D.angleRad(nr, n0);

        if (arot1 > 0)
        {
            // rotate into new 1-2-3 plane
            CoordinateUtilities3D.rotatePointsRad(buildCrds, sel, p20, virt1, arot1);
            CoordinateUtilities3D.rotateRad(p1copy, p20, CoordinateUtilities3D.add(p20, vrot1), arot1);
        }

        double arot2 = CoordinateUtilities3D.angleRad(CoordinateUtilities3D.subtract(p1copy, p20),
                CoordinateUtilities3D.subtract(p10, p20));
        if (arot2 > 0)
        {
            double[] n2 = CoordinateUtilities3D.crossprod(CoordinateUtilities3D.subtract(p1copy, p20),
                    CoordinateUtilities3D.subtract(p10, p20));

            // rotate within 1-2-3 plane
            double[] virt2 = CoordinateUtilities3D.add(p20, n2);
            CoordinateUtilities3D.rotatePointsRad(buildCrds, sel, p20, virt2, arot2);
            CoordinateUtilities3D.rotateRad(p1copy, p20, virt2, arot2);
        }
    }

    /**
    Diagonalizes the matrix Mcov, writes out the eigenvectors and eigenvalues,
    sorted from the largest to the smallest
     */
    /*
    public static void largestEVanalysis(double[][] Mcov,
    String evecOut, String evalOut)
    {
    Cern ED = new Cern(new DenseDoubleMatrix2D(Mcov));
    double[][] evec = ED.getEigenvectors();
    double[] eval = ED.getEigenvalues();

    // reorder eigenvectors according to eigenvalues;
    double[][] evecT = doubleArrays.transpose(evec);
    double[][] evecTsort = new double[evecT.length][evecT[0].length];
    int[] order = doubleArrays.sortedIndexes(eval);
    // descending order:
    order = IntArrays.add(IntArrays.multiply(-1,order), order.length-1);
    for (int i=0; i<order.length; i++)
    evecTsort[order[i]] = evecT[i];

    double[][] pcBase = (doubleArrays.transpose(evecTsort));
    double[] evalsort = doubleArrays.copy(eval);
    java.util.Arrays.sort(evalsort);
    double[] eigenvalues = doubleArrays.mirror(evalsort);

    doubleArrays.save(pcBase, evecOut);
    doubleArrays.save(eigenvalues, evalOut);
    }

    public static double[] powerMethod(SparseDoubleMatrix2D M)
    {
    Algebra algebra = new Algebra();
    DoubleMatrix2D p = new DenseDoubleMatrix2D(1,M.rows());
    for (int i=0; i<p.columns(); i++)
    p.set(0, i, Math.random());
    double norm = doubleArrays.norm(p.toArray()[0]);
    for (int i=0; i<p.size(); i++)
    p.set(0, i, p.get(0,i)/norm);

    double err = Double.POSITIVE_INFINITY;
    for (int i=0; i<100000; i++)
    {
    DoubleMatrix2D pnew = p.zMult(M, null);
    double[][] vnew = pnew.toArray();
    double sum = doubleArrays.sum(vnew[0]);
    double lambda = doubleArrays.norm(vnew[0]);
    for (int j=0; j<p.size(); j++)
    p.set(0, j, vnew[0][j]/lambda);
    //p = pnew;
    System.out.println(i+"\t"+lambda+"\t"+sum);
    }

    return(null);
    }
     */
    public static void DEBUG_printCrd(double[] crd)
    {
        System.out.println(crd[0] + ", " + crd[1] + ", " + crd[2]);
    }

    public static void DEBUG_printCrdMatrix(double[][] crd)
    {
        for (int i = 0; i < crd.length; i++)
        {
            System.out.print(crd[i][0]);
            System.out.print(crd[i][1]);
            System.out.println(crd[i][2]);
        }
    }

    public static void timer(String msg)
    {
        if (msg == null)
        {
            t = System.currentTimeMillis();
        }
        else
        {
            System.out.println(msg + "\t" + (double) (System.currentTimeMillis() - t) / 1000);
        }
    }

    public static void main(String[] args)
    {
        if (args.length == 0)
        {
            System.out.println("usage: LinalgTools [mode]\n"
                    + "          -subspaceOverlap <V1> <V2>\n"
                    + "          -superspaceOverlap <V1> <V2>\n"
                    + "          -largestEVanalysis (-ibin <matrix-bin> | -itxt <matrix-bin>) (-o <evecOut> <evalOut>)\n"
                    + "          -powerMethod <matrix>");
            System.out.println("          -eigenvalues <matrix>");
            System.exit(0);
        }
        Arguments arg = new Arguments(args);

        if (arg.hasCommand("subspaceOverlap"))
        {
            double[][] V1 = doubleArrays.loadMatrix(arg.getArgument("subspaceOverlap", 0));
            double[][] V2 = doubleArrays.loadMatrix(arg.getArgument("subspaceOverlap", 1));
            System.out.println(CoordinateUtilities3D.subspaceOverlap(V1, V2));
        }
        if (arg.hasCommand("superspaceOverlap"))
        {
            double[][] V1 = doubleArrays.loadMatrix(arg.getArgument("superspaceOverlap", 0));
            double[][] V2 = doubleArrays.loadMatrix(arg.getArgument("superspaceOverlap", 1));
            System.out.println(CoordinateUtilities3D.superspaceOverlap(V1, V2));
        }


    }
}
TOP

Related Classes of stallone.api.coordinates.CoordinateUtilities3D

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.