Tuple problem in processing

I’m working with a magnetometer and want to port a calibration program to processing. The problem is it relies on Tuples and was originally written in C#. It looked a lot like Java so I stuck it in a sketch to see what was different. The first thing was Tuple. I solved that by finding a library that allows you to use Tuples. The syntax was different than the original code but close. I kept fiddling with it until the errors went away I thought. I hard coded some example variable values instead of making a Swing gui for now and then I ran it. I get some java.lang.reflect.InvocationTargetException. It seems to be when it tests value of p in the LUPDecompostion Tuple function it fails. I can’t tell if it is because how I translated or if there is some other error.

What did I do wrong?

Here’s where the original MagMaster code is found:
https://github.com/YuriMat/MagMasterSource/archive/master.zip

Here’s where the javatuple library is found:
https://www.javatuples.org/download.html

import org.javatuples.Pair;
import org.javatuples.Quartet;
import org.javatuples.Triplet;

public static double[][] A;
public  static int[] B;
    
 //   MyPanel controlPanel;
    
    public String textBoxXPlus_X_0,textBoxXPlus_X_180,textBoxXPlus_Y_0,textBoxXPlus_Y_180,textBoxXPlus_Z_0,textBoxXPlus_Z_180;
    public String textBoxYPlus_X_0,textBoxYPlus_X_180,textBoxYPlus_Y_0,textBoxYPlus_Y_180,textBoxYPlus_Z_0,textBoxYPlus_Z_180;
    public String textBoxZPlus_X_0,textBoxZPlus_X_180,textBoxZPlus_Y_0,textBoxZPlus_Y_180,textBoxZPlus_Z_0,textBoxZPlus_Z_180;
    
    public String textBoxXMinus_X_0,textBoxXMinus_X_180,textBoxXMinus_Y_0,textBoxXMinus_Y_180,textBoxXMinus_Z_0,textBoxXMinus_Z_180;
    public String textBoxYMinus_X_0,textBoxYMinus_X_180,textBoxYMinus_Y_0,textBoxYMinus_Y_180,textBoxYMinus_Z_0,textBoxYMinus_Z_180;
    public String textBoxZMinus_X_0,textBoxZMinus_X_180,textBoxZMinus_Y_0,textBoxZMinus_Y_180,textBoxZMinus_Z_0,textBoxZMinus_Z_180;

       
       void setup() {
  
             textBoxXPlus_X_0 = "-836";
             textBoxXPlus_X_180 = "-866";
             textBoxXPlus_Y_0 = "-191";
             textBoxXPlus_Y_180 = "404";
             textBoxXPlus_Z_0 = "15";
             textBoxXPlus_Z_180 = "-258";
             textBoxYPlus_X_0 = "-523";
             textBoxYPlus_X_180 = "205";
             textBoxYPlus_Y_0 = "-576";
             textBoxYPlus_Y_180 = "-716";
             textBoxYPlus_Z_0 = "-267";
             textBoxYPlus_Z_180 = "16";
             textBoxZPlus_X_0 = "-481";
             textBoxZPlus_X_180 = "166";
             textBoxZPlus_Y_0 = "208";
             textBoxZPlus_Y_180 = "-101";
             textBoxZPlus_Z_0 = "-743";
             textBoxZPlus_Z_180 = "-821";
    
             textBoxXMinus_X_0 = "504";
             textBoxXMinus_X_180 = "558";
             textBoxXMinus_Y_0 = "-243";
             textBoxXMinus_Y_180 = "350";
             textBoxXMinus_Z_0 = "-321";
             textBoxXMinus_Z_180 = "-16";
             textBoxYMinus_X_0 = "-461";
             textBoxYMinus_X_180 = "169";
             textBoxYMinus_Y_0 = "747";
             textBoxYMinus_Y_180 = "765";
             textBoxYMinus_Z_0 = "7";
             textBoxYMinus_Z_180 = "-334";
             textBoxZMinus_X_0 = "-538";
             textBoxZMinus_X_180 = "163";
             textBoxZMinus_Y_0 = "-98";
             textBoxZMinus_Y_180 = "364";
             textBoxZMinus_Z_0 = "492";
             textBoxZMinus_Z_180 = "503";

          
       }
       void draw(){
         calculate_transformation_matrix();

       }
        
        public void calculate_transformation_matrix()
        {
            //Axis X--------------------------------------------------------------------------------------------------
            double[] Xplus_center = new double[3];
            //Centers of the circles
            Xplus_center[0] = (Double.parseDouble(textBoxXPlus_X_0) + Double.parseDouble(textBoxXPlus_X_180)) / 2;
            Xplus_center[1] = (Double.parseDouble(textBoxXPlus_Y_0) + Double.parseDouble(textBoxXPlus_Y_180)) / 2;
            Xplus_center[2] = (Double.parseDouble(textBoxXPlus_Z_0) + Double.parseDouble(textBoxXPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Xminus_center = new double[3];
            Xminus_center[0] = (Double.parseDouble(textBoxXMinus_X_0) + Double.parseDouble(textBoxXMinus_X_180)) / 2;
            Xminus_center[1] = (Double.parseDouble(textBoxXMinus_Y_0) + Double.parseDouble(textBoxXMinus_Y_180)) / 2;
            Xminus_center[2] = (Double.parseDouble(textBoxXMinus_Z_0) + Double.parseDouble(textBoxXMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Xvector = new double[3];
            Xvector[0] = Xplus_center[0] - Xminus_center[0];
            Xvector[1] = Xplus_center[1] - Xminus_center[1];
            Xvector[2] = Xplus_center[2] - Xminus_center[2];

            //Axis Y--------------------------------------------------------------------------------------------------
            double[] Yplus_center = new double[3];
            //Centers of the circles
            Yplus_center[0] = (Double.parseDouble(textBoxYPlus_X_0) + Double.parseDouble(textBoxYPlus_X_180)) / 2;
            Yplus_center[1] = (Double.parseDouble(textBoxYPlus_Y_0) + Double.parseDouble(textBoxYPlus_Y_180)) / 2;
            Yplus_center[2] = (Double.parseDouble(textBoxYPlus_Z_0) + Double.parseDouble(textBoxYPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Yminus_center = new double[3];
            Yminus_center[0] = (Double.parseDouble(textBoxYMinus_X_0) + Double.parseDouble(textBoxYMinus_X_180)) / 2;
            Yminus_center[1] = (Double.parseDouble(textBoxYMinus_Y_0) + Double.parseDouble(textBoxYMinus_Y_180)) / 2;
            Yminus_center[2] = (Double.parseDouble(textBoxYMinus_Z_0) + Double.parseDouble(textBoxYMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Yvector = new double[3];
            Yvector[0] = Yplus_center[0] - Yminus_center[0];
            Yvector[1] = Yplus_center[1] - Yminus_center[1];
            Yvector[2] = Yplus_center[2] - Yminus_center[2];

            //Axis Z--------------------------------------------------------------------------------------------------
            double[] Zplus_center = new double[3];
            //Centers of the circles
            Zplus_center[0] = (Double.parseDouble(textBoxZPlus_X_0) + Double.parseDouble(textBoxZPlus_X_180)) / 2;
            Zplus_center[1] = (Double.parseDouble(textBoxZPlus_Y_0) + Double.parseDouble(textBoxZPlus_Y_180)) / 2;
            Zplus_center[2] = (Double.parseDouble(textBoxZPlus_Z_0) + Double.parseDouble(textBoxZPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Zminus_center = new double[3];
            Zminus_center[0] = (Double.parseDouble(textBoxZMinus_X_0) + Double.parseDouble(textBoxZMinus_X_180)) / 2;
            Zminus_center[1] = (Double.parseDouble(textBoxZMinus_Y_0) + Double.parseDouble(textBoxZMinus_Y_180)) / 2;
            Zminus_center[2] = (Double.parseDouble(textBoxZMinus_Z_0) + Double.parseDouble(textBoxZMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Zvector = new double[3];
            Zvector[0] = Zplus_center[0] - Zminus_center[0];
            Zvector[1] = Zplus_center[1] - Zminus_center[1];
            Zvector[2] = Zplus_center[2] - Zminus_center[2];

            // Rotation matrix--------------------------------------------------------------------------------------
            // rotation_matrix[a][b], a - number of the rows, b - number of the columbs
            double[][] rotation_matrix = new double[3][];
            rotation_matrix[0] = new double[3];
            rotation_matrix[1] = new double[3];
            rotation_matrix[2] = new double[3];
            //Deviding by main value, for example for X axis - deviding by X coordinate, for Y axis by Y coordinate, for Z axis by Z cordinate
            rotation_matrix[0][0] = Xvector[0] / Xvector[0]; rotation_matrix[0][1] = Yvector[0] / Yvector[1]; rotation_matrix[0][2] = Zvector[0] / Zvector[2];
            rotation_matrix[1][0] = Xvector[1] / Xvector[0]; rotation_matrix[1][1] = Yvector[1] / Yvector[1]; rotation_matrix[1][2] = Zvector[1] / Zvector[2];
            rotation_matrix[2][0] = Xvector[2] / Xvector[0]; rotation_matrix[2][1] = Yvector[2] / Yvector[1]; rotation_matrix[2][2] = Zvector[2] / Zvector[2];
            //Matrix inversion
            rotation_matrix = InvertMatrix(rotation_matrix);

            //Determinating of the corrected by ratation matrix centers of the circles 
            Xplus_center = MatrixVectorMultiply(rotation_matrix, Xplus_center);
            Xminus_center = MatrixVectorMultiply(rotation_matrix, Xminus_center);
            Yplus_center = MatrixVectorMultiply(rotation_matrix, Yplus_center);
            Yminus_center = MatrixVectorMultiply(rotation_matrix, Yminus_center);
            Zplus_center = MatrixVectorMultiply(rotation_matrix, Zplus_center);
            Zminus_center = MatrixVectorMultiply(rotation_matrix, Zminus_center);

            //Determinating of the elipsoid center---------------------------------------------------------------------------
            double[] center = new double[3];
            center[0] = (Xplus_center[0] + Xminus_center[0] + Yplus_center[0] + Yminus_center[0] + Zplus_center[0] + Zminus_center[0]) / 6;
            center[1] = (Xplus_center[1] + Xminus_center[1] + Yplus_center[1] + Yminus_center[1] + Zplus_center[1] + Zminus_center[1]) / 6;
            center[2] = (Xplus_center[2] + Xminus_center[2] + Yplus_center[2] + Yminus_center[2] + Zplus_center[2] + Zminus_center[2]) / 6;

            //Determinating of the radius of the future sphere-----------------------------------------------------------------------
            double x_length = Math.abs(Xplus_center[0] - Xminus_center[0])/2;
            double y_length = Math.abs(Yplus_center[1] - Yminus_center[1])/2;
            double z_length = Math.abs(Zplus_center[2] - Zminus_center[2])/2;
            double[] Xplus_0 = new double[3];
            Xplus_0[0] = Double.parseDouble(textBoxXPlus_X_0); 
            Xplus_0[1] = Double.parseDouble(textBoxXPlus_Y_0); 
            Xplus_0[2] = Double.parseDouble(textBoxXPlus_Z_0);
            Xplus_0 = MatrixVectorMultiply(rotation_matrix, Xplus_0);
            double[] Yplus_0 = new double[3];
            Yplus_0[0] = Double.parseDouble(textBoxYPlus_X_0);
            Yplus_0[1] = Double.parseDouble(textBoxYPlus_Y_0);
            Yplus_0[2] = Double.parseDouble(textBoxYPlus_Z_0);
            Yplus_0 = MatrixVectorMultiply(rotation_matrix, Yplus_0);
            double[] Zplus_0 = new double[3];
            Zplus_0[0] = Double.parseDouble(textBoxZPlus_X_0);
            Zplus_0[1] = Double.parseDouble(textBoxZPlus_Y_0);
            Zplus_0[2] = Double.parseDouble(textBoxZPlus_Z_0);
            Zplus_0 = MatrixVectorMultiply(rotation_matrix, Zplus_0);
            double x_abs = Math.sqrt(x_length * x_length + Xplus_0[1] * Xplus_0[1] + Xplus_0[2] * Xplus_0[2]);
            double y_abs = Math.sqrt(Yplus_0[0] * Yplus_0[0] + y_length * y_length + Yplus_0[2] * Yplus_0[2]);
            double z_abs = Math.sqrt(Zplus_0[0] * Zplus_0[0] + Zplus_0[1] * Zplus_0[1] + z_length * z_length);
            //sphere radius
            double sphere_radius = (x_abs + y_abs + z_abs) / 3;

            //Scales for the each axis------------------------------------------------
            //Diameter of the sphere
            double diameter = sphere_radius * 2;
            double kx = Math.abs(diameter / (Xplus_center[0] - Xminus_center[0]));
            double ky = Math.abs(diameter / (Yplus_center[1] - Yminus_center[1]));
            double kz = Math.abs(diameter / (Zplus_center[2] - Zminus_center[2]));

            //Multiplying elements of matrix by scales
            rotation_matrix[0][0] = rotation_matrix[0][0] * kx; rotation_matrix[0][1] = rotation_matrix[0][1] * ky; rotation_matrix[0][2] = rotation_matrix[0][2] * kz;
            rotation_matrix[1][0] = rotation_matrix[1][0] * kx; rotation_matrix[1][1] = rotation_matrix[1][1] * ky; rotation_matrix[1][2] = rotation_matrix[1][2] * kz;
            rotation_matrix[2][0] = rotation_matrix[2][0] * kx; rotation_matrix[2][1] = rotation_matrix[2][1] * ky; rotation_matrix[2][2] = rotation_matrix[2][2] * kz;
            
            //Bias
            double[] bias = new double[3];
            bias[0] = center[0];
            bias[1] = center[1];
            bias[2] = center[2];

            //Indication
            //Transformation matrix
     /*       textBox_matrixX_x.Text = rotation_matrix[0][0].ToString("0.ddd"); textBox_matrixY_x.Text = rotation_matrix[0][1].ToString("0.###"); textBox_matrixZ_x.Text = rotation_matrix[0][2].ToString("0.###");
            textBox_matrixX_y.Text = rotation_matrix[1][0].ToString("0.###"); textBox_matrixY_y.Text = rotation_matrix[1][1].ToString("0.###"); textBox_matrixZ_y.Text = rotation_matrix[1][2].ToString("0.###");
            textBox_matrixX_z.Text = rotation_matrix[2][0].ToString("0.###"); textBox_matrixY_z.Text = rotation_matrix[2][1].ToString("0.###"); textBox_matrixZ_z.Text = rotation_matrix[2][2].ToString("0.###");
            //Bias
            textBox_biasX.Text = bias[0].ToString("0.###");
            textBox_biasY.Text = bias[1].ToString("0.###");
            textBox_biasZ.Text = bias[2].ToString("0.###");  */
        }

        public static double[] MatrixVectorMultiply(double[][] matrixA, double[] vectorB)
        {
            int aRows = matrixA.length; int aCols = matrixA[0].length;
            int bRows = vectorB.length;
         //   if (aCols != bRows)
           //     throw new Exception("Non-conformable matrices in MatrixProduct");
            double[] result = new double[aRows];
            for (int i = 0; i < aRows; ++i) // each row of A
                for (int k = 0; k < aCols; ++k)
                    result[i] += matrixA[i][k] * vectorB[k];
            return result;
        }
        
 
        
               public static double[][] InvertMatrix(double[][] A)
        {
            int n = A.length;
            //e will represent each column in the identity matrix
            double[] e;
            //x will hold the inverse matrix to be returned
            double[][] x = new double[n][];
            for (int i = 0; i < n; i++)
            {
                x[i] = new double[A[i].length];
            }
            /*
            * solve will contain the vector solution for the LUP decomposition as we solve
            * for each vector of x.  We will combine the solutions into the double[][] array x.
            * */
            double[] solve;

            //Get the LU matrix and P matrix (as an array)
          //  Tuple<double[][], int[]> results = LUPDecomposition(A);
            Pair<double[][], int[]>results = Pair.with(LUPDecomposition.getValue0(),LUPDecomposition.getValue1());             

           // double[][] LU = results.Item1;
            double[][] LU = results.getValue0();
         //   int[] P = results.Item2;
            int[] P = results.getValue1();

            /*
            * Solve AX = e for each column ei of the identity matrix using LUP decomposition
            * */
            for (int i = 0; i < n; i++)
            {
                e = new double[A[i].length];
                e[i] = 1;
                solve = LUPSolve(LU, P, e);
                for (int j = 0; j < solve.length; j++)
                {
                    x[j][i] = solve[j];
                }
            }
            return x;
        }
      
        


        public static double[] LUPSolve(double[][] LU, int[] pi, double[] b)
        {
            int n = LU.length - 1;
            double[] x = new double[n + 1];
            double[] y = new double[n + 1];
            double suml = 0;
            double sumu = 0;
            double lij = 0;

            /*
            * Solve for y using formward substitution
            * */
            for (int i = 0; i <= n; i++)
            {
                suml = 0;
                for (int j = 0; j <= i - 1; j++)
                {
                    /*
                    * Since we've taken L and U as a singular matrix as an input
                    * the value for L at index i and j will be 1 when i equals j, not LU[i][j], since
                    * the diagonal values are all 1 for L.
                    * */
                    if (i == j)
                    {
                        lij = 1;
                    }
                    else
                    {
                        lij = LU[i][j];
                    }
                    suml = suml + (lij * y[j]);
                }
                y[i] = b[pi[i]] - suml;
            }
            //Solve for x by using back substitution
            for (int i = n; i >= 0; i--)
            {
                sumu = 0;
                for (int j = i + 1; j <= n; j++)
                {
                    sumu = sumu + (LU[i][j] * x[j]);
                }
                x[i] = (y[i] - sumu) / LU[i][i];
            }
            return x;
        }

     /////////////   public static Tuple<double[][], int[]> LUPDecomposition(double[][] A)
                public static Pair<double[][], int[]>LUPDecomposition = Pair.with(A, B);
        {
            int n = A.length - 1;
            /*
            * pi represents the permutation matrix.  We implement it as an array
            * whose value indicates which column the 1 would appear.  We use it to avoid 
            * dividing by zero or small numbers.
            * */
            int[] pi = new int[n + 1];
            double p = 0;
            int kp = 0;
            int pik = 0;
            int pikp = 0;
            double aki = 0;
            double akpi = 0;

            //Initialize the permutation matrix, will be the identity matrix
            for (int j = 0; j <= n; j++)
            {
                pi[j] = j;
            }

            for (int k = 0; k <= n; k++)
            {
              
              
                             /*
                * In finding the permutation matrix p that avoids dividing by zero
                * we take a slightly different approach.  For numerical stability
                * We find the element with the largest 
                * absolute value of those in the current first column (column k).  If all elements in
                * the current first column are zero then the matrix is singluar and throw an
                * error.
                * */

              p = 0;
                for (int i = k; i <= n; i++)
                {
                    if (Math.abs(A[i][k]) > p)
                    {
                        p = Math.abs(A[i][k]);
                        kp = i;
                    }
                }
                if (p == 0)
                {
                  //  throw new Exception("singular matrix");
                }
                /*
                * These lines update the pivot array (which represents the pivot matrix)
                * by exchanging pi[k] and pi[kp].
                * */
                pik = pi[k];
                pikp = pi[kp];
                pi[k] = pikp;
                pi[kp] = pik;

                /*
                * Exchange rows k and kpi as determined by the pivot
                * */
                for (int i = 0; i <= n; i++)
                {
                    aki = A[k][i];
                    akpi = A[kp][i];
                    A[k][i] = akpi;
                    A[kp][i] = aki;
                }

                /*
                    * Compute the Schur complement
                    * */
                for (int i = k + 1; i <= n; i++)
                {
                    A[i][k] = A[i][k] / A[k][k];
                    for (int j = k + 1; j <= n; j++)
                    {
                        A[i][j] = A[i][j] - (A[i][k] * A[k][j]);
                    }
                }
            }
     //  return   
       Pair<double[][],int[]> ReturnTuple = Pair.with(A,pi);
       println(ReturnTuple);

        }


This was originally posted under processing coding questions because it was a coding question. Yes, it uses a library but the question is still why doesn’t it work?

I’ve looked at it again and I see that It is trying to make a Tuple function type. Is that even possible to do when java/processing doesn’t have a Tuple type?

Another thing I notice is that this LUPDecomposition Tuple function required me to put a “;” and then the curly bracket. That doesn’t seem right.

Is there some way that I could make it a different type of function but still compute the elements of Tuple?

Which line throws java.lang.reflect.InvocationTargetException?

It says line 333. There isn’t anything there but that line is right after the posted code below and before a comment about permutation matrix.

           for (int k = 0; k <= n; k++)
            {
         

which is in that LUPDecomposition function.

I think I will abandon messing with Tuples to do this. I searched LUPDecomposition which is quite complicated, but found a previous library I explored a little and that is jama. It will also do LUPDecomposition and has a class by that name.

Here is the link for jama:
https://math.nist.gov/javanumerics/jama/

I got it to work by using the Jama library. It has a LUDecompostion class, so I messed with that and got it to give me the int[] P pivot thing and the double[][]LU lower and upper triangular factor things instead of getting them from a Tuple function. I stuck those into the LUPsolve from the code I had and got the same result as the example.

Here is the slightly modified code now using Jama:

import org.javatuples.Pair;
import org.javatuples.Quartet;
import org.javatuples.Triplet;
import Jama.*;

    
 //   MyPanel controlPanel;
    
    public String textBoxXPlus_X_0,textBoxXPlus_X_180,textBoxXPlus_Y_0,textBoxXPlus_Y_180,textBoxXPlus_Z_0,textBoxXPlus_Z_180;
    public String textBoxYPlus_X_0,textBoxYPlus_X_180,textBoxYPlus_Y_0,textBoxYPlus_Y_180,textBoxYPlus_Z_0,textBoxYPlus_Z_180;
    public String textBoxZPlus_X_0,textBoxZPlus_X_180,textBoxZPlus_Y_0,textBoxZPlus_Y_180,textBoxZPlus_Z_0,textBoxZPlus_Z_180;
    
    public String textBoxXMinus_X_0,textBoxXMinus_X_180,textBoxXMinus_Y_0,textBoxXMinus_Y_180,textBoxXMinus_Z_0,textBoxXMinus_Z_180;
    public String textBoxYMinus_X_0,textBoxYMinus_X_180,textBoxYMinus_Y_0,textBoxYMinus_Y_180,textBoxYMinus_Z_0,textBoxYMinus_Z_180;
    public String textBoxZMinus_X_0,textBoxZMinus_X_180,textBoxZMinus_Y_0,textBoxZMinus_Y_180,textBoxZMinus_Z_0,textBoxZMinus_Z_180;

     
       void setup() {
         
        
  
             textBoxXPlus_X_0 = "-836";
             textBoxXPlus_X_180 = "-866";
             textBoxXPlus_Y_0 = "-191";
             textBoxXPlus_Y_180 = "404";
             textBoxXPlus_Z_0 = "15";
             textBoxXPlus_Z_180 = "-258";
             textBoxYPlus_X_0 = "-523";
             textBoxYPlus_X_180 = "205";
             textBoxYPlus_Y_0 = "-576";
             textBoxYPlus_Y_180 = "-716";
             textBoxYPlus_Z_0 = "-267";
             textBoxYPlus_Z_180 = "16";
             textBoxZPlus_X_0 = "-481";
             textBoxZPlus_X_180 = "166";
             textBoxZPlus_Y_0 = "208";
             textBoxZPlus_Y_180 = "-101";
             textBoxZPlus_Z_0 = "-743";
             textBoxZPlus_Z_180 = "-821";
    
             textBoxXMinus_X_0 = "504";
             textBoxXMinus_X_180 = "558";
             textBoxXMinus_Y_0 = "-243";
             textBoxXMinus_Y_180 = "350";
             textBoxXMinus_Z_0 = "-321";
             textBoxXMinus_Z_180 = "-16";
             textBoxYMinus_X_0 = "-461";
             textBoxYMinus_X_180 = "169";
             textBoxYMinus_Y_0 = "747";
             textBoxYMinus_Y_180 = "765";
             textBoxYMinus_Z_0 = "7";
             textBoxYMinus_Z_180 = "-334";
             textBoxZMinus_X_0 = "-538";
             textBoxZMinus_X_180 = "163";
             textBoxZMinus_Y_0 = "-98";
             textBoxZMinus_Y_180 = "364";
             textBoxZMinus_Z_0 = "492";
             textBoxZMinus_Z_180 = "503";

          
       }
       void draw(){
         calculate_transformation_matrix();
        // println(bias);

       }
        
        public void calculate_transformation_matrix()
        {
            //Axis X--------------------------------------------------------------------------------------------------
            double[] Xplus_center = new double[3];
            //Centers of the circles
            Xplus_center[0] = (Double.parseDouble(textBoxXPlus_X_0) + Double.parseDouble(textBoxXPlus_X_180)) / 2;
            Xplus_center[1] = (Double.parseDouble(textBoxXPlus_Y_0) + Double.parseDouble(textBoxXPlus_Y_180)) / 2;
            Xplus_center[2] = (Double.parseDouble(textBoxXPlus_Z_0) + Double.parseDouble(textBoxXPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Xminus_center = new double[3];
            Xminus_center[0] = (Double.parseDouble(textBoxXMinus_X_0) + Double.parseDouble(textBoxXMinus_X_180)) / 2;
            Xminus_center[1] = (Double.parseDouble(textBoxXMinus_Y_0) + Double.parseDouble(textBoxXMinus_Y_180)) / 2;
            Xminus_center[2] = (Double.parseDouble(textBoxXMinus_Z_0) + Double.parseDouble(textBoxXMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Xvector = new double[3];
            Xvector[0] = Xplus_center[0] - Xminus_center[0];
            Xvector[1] = Xplus_center[1] - Xminus_center[1];
            Xvector[2] = Xplus_center[2] - Xminus_center[2];

            //Axis Y--------------------------------------------------------------------------------------------------
            double[] Yplus_center = new double[3];
            //Centers of the circles
            Yplus_center[0] = (Double.parseDouble(textBoxYPlus_X_0) + Double.parseDouble(textBoxYPlus_X_180)) / 2;
            Yplus_center[1] = (Double.parseDouble(textBoxYPlus_Y_0) + Double.parseDouble(textBoxYPlus_Y_180)) / 2;
            Yplus_center[2] = (Double.parseDouble(textBoxYPlus_Z_0) + Double.parseDouble(textBoxYPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Yminus_center = new double[3];
            Yminus_center[0] = (Double.parseDouble(textBoxYMinus_X_0) + Double.parseDouble(textBoxYMinus_X_180)) / 2;
            Yminus_center[1] = (Double.parseDouble(textBoxYMinus_Y_0) + Double.parseDouble(textBoxYMinus_Y_180)) / 2;
            Yminus_center[2] = (Double.parseDouble(textBoxYMinus_Z_0) + Double.parseDouble(textBoxYMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Yvector = new double[3];
            Yvector[0] = Yplus_center[0] - Yminus_center[0];
            Yvector[1] = Yplus_center[1] - Yminus_center[1];
            Yvector[2] = Yplus_center[2] - Yminus_center[2];

            //Axis Z--------------------------------------------------------------------------------------------------
            double[] Zplus_center = new double[3];
            //Centers of the circles
            Zplus_center[0] = (Double.parseDouble(textBoxZPlus_X_0) + Double.parseDouble(textBoxZPlus_X_180)) / 2;
            Zplus_center[1] = (Double.parseDouble(textBoxZPlus_Y_0) + Double.parseDouble(textBoxZPlus_Y_180)) / 2;
            Zplus_center[2] = (Double.parseDouble(textBoxZPlus_Z_0) + Double.parseDouble(textBoxZPlus_Z_180)) / 2;
            //Centers of the circles
            double[] Zminus_center = new double[3];
            Zminus_center[0] = (Double.parseDouble(textBoxZMinus_X_0) + Double.parseDouble(textBoxZMinus_X_180)) / 2;
            Zminus_center[1] = (Double.parseDouble(textBoxZMinus_Y_0) + Double.parseDouble(textBoxZMinus_Y_180)) / 2;
            Zminus_center[2] = (Double.parseDouble(textBoxZMinus_Z_0) + Double.parseDouble(textBoxZMinus_Z_180)) / 2;
            //Vector from the center of minus circle to the center of plus circle
            double[] Zvector = new double[3];
            Zvector[0] = Zplus_center[0] - Zminus_center[0];
            Zvector[1] = Zplus_center[1] - Zminus_center[1];
            Zvector[2] = Zplus_center[2] - Zminus_center[2];

            // Rotation matrix--------------------------------------------------------------------------------------
            // rotation_matrix[a][b], a - number of the rows, b - number of the columbs
            double[][] rotation_matrix = new double[3][];
          // Matrix rotation_matrix(3,3);
            rotation_matrix[0] = new double[3];
            rotation_matrix[1] = new double[3];
            rotation_matrix[2] = new double[3];
            //Deviding by main value, for example for X axis - deviding by X coordinate, for Y axis by Y coordinate, for Z axis by Z cordinate
            rotation_matrix[0][0] = Xvector[0] / Xvector[0]; rotation_matrix[0][1] = Yvector[0] / Yvector[1]; rotation_matrix[0][2] = Zvector[0] / Zvector[2];
            rotation_matrix[1][0] = Xvector[1] / Xvector[0]; rotation_matrix[1][1] = Yvector[1] / Yvector[1]; rotation_matrix[1][2] = Zvector[1] / Zvector[2];
            rotation_matrix[2][0] = Xvector[2] / Xvector[0]; rotation_matrix[2][1] = Yvector[2] / Yvector[1]; rotation_matrix[2][2] = Zvector[2] / Zvector[2];
            //Matrix inversion
            rotation_matrix = InvertMatrix(rotation_matrix);

            //Determinating of the corrected by ratation matrix centers of the circles 
            Xplus_center = MatrixVectorMultiply(rotation_matrix, Xplus_center);
            Xminus_center = MatrixVectorMultiply(rotation_matrix, Xminus_center);
            Yplus_center = MatrixVectorMultiply(rotation_matrix, Yplus_center);
            Yminus_center = MatrixVectorMultiply(rotation_matrix, Yminus_center);
            Zplus_center = MatrixVectorMultiply(rotation_matrix, Zplus_center);
            Zminus_center = MatrixVectorMultiply(rotation_matrix, Zminus_center);

            //Determinating of the elipsoid center---------------------------------------------------------------------------
            double[] center = new double[3];
            center[0] = (Xplus_center[0] + Xminus_center[0] + Yplus_center[0] + Yminus_center[0] + Zplus_center[0] + Zminus_center[0]) / 6;
            center[1] = (Xplus_center[1] + Xminus_center[1] + Yplus_center[1] + Yminus_center[1] + Zplus_center[1] + Zminus_center[1]) / 6;
            center[2] = (Xplus_center[2] + Xminus_center[2] + Yplus_center[2] + Yminus_center[2] + Zplus_center[2] + Zminus_center[2]) / 6;

            //Determinating of the radius of the future sphere-----------------------------------------------------------------------
            double x_length = Math.abs(Xplus_center[0] - Xminus_center[0])/2;
            double y_length = Math.abs(Yplus_center[1] - Yminus_center[1])/2;
            double z_length = Math.abs(Zplus_center[2] - Zminus_center[2])/2;
            double[] Xplus_0 = new double[3];
            Xplus_0[0] = Double.parseDouble(textBoxXPlus_X_0); 
            Xplus_0[1] = Double.parseDouble(textBoxXPlus_Y_0); 
            Xplus_0[2] = Double.parseDouble(textBoxXPlus_Z_0);
            Xplus_0 = MatrixVectorMultiply(rotation_matrix, Xplus_0);
            double[] Yplus_0 = new double[3];
            Yplus_0[0] = Double.parseDouble(textBoxYPlus_X_0);
            Yplus_0[1] = Double.parseDouble(textBoxYPlus_Y_0);
            Yplus_0[2] = Double.parseDouble(textBoxYPlus_Z_0);
            Yplus_0 = MatrixVectorMultiply(rotation_matrix, Yplus_0);
            double[] Zplus_0 = new double[3];
            Zplus_0[0] = Double.parseDouble(textBoxZPlus_X_0);
            Zplus_0[1] = Double.parseDouble(textBoxZPlus_Y_0);
            Zplus_0[2] = Double.parseDouble(textBoxZPlus_Z_0);
            Zplus_0 = MatrixVectorMultiply(rotation_matrix, Zplus_0);
            double x_abs = Math.sqrt(x_length * x_length + Xplus_0[1] * Xplus_0[1] + Xplus_0[2] * Xplus_0[2]);
            double y_abs = Math.sqrt(Yplus_0[0] * Yplus_0[0] + y_length * y_length + Yplus_0[2] * Yplus_0[2]);
            double z_abs = Math.sqrt(Zplus_0[0] * Zplus_0[0] + Zplus_0[1] * Zplus_0[1] + z_length * z_length);
            //sphere radius
            double sphere_radius = (x_abs + y_abs + z_abs) / 3;

            //Scales for the each axis------------------------------------------------
            //Diameter of the sphere
            double diameter = sphere_radius * 2;
            double kx = Math.abs(diameter / (Xplus_center[0] - Xminus_center[0]));
            double ky = Math.abs(diameter / (Yplus_center[1] - Yminus_center[1]));
            double kz = Math.abs(diameter / (Zplus_center[2] - Zminus_center[2]));

            //Multiplying elements of matrix by scales
            rotation_matrix[0][0] = rotation_matrix[0][0] * kx; rotation_matrix[0][1] = rotation_matrix[0][1] * ky; rotation_matrix[0][2] = rotation_matrix[0][2] * kz;
            rotation_matrix[1][0] = rotation_matrix[1][0] * kx; rotation_matrix[1][1] = rotation_matrix[1][1] * ky; rotation_matrix[1][2] = rotation_matrix[1][2] * kz;
            rotation_matrix[2][0] = rotation_matrix[2][0] * kx; rotation_matrix[2][1] = rotation_matrix[2][1] * ky; rotation_matrix[2][2] = rotation_matrix[2][2] * kz;
           
            println(rotation_matrix[0][0],rotation_matrix[0][1],rotation_matrix[0][2]);
            
            //Bias
            double[] bias = new double[3];
            bias[0] = center[0];
            bias[1] = center[1];
            bias[2] = center[2];

            //Indication
            //Transformation matrix
     /*       textBox_matrixX_x.Text = rotation_matrix[0][0].ToString("0.ddd"); textBox_matrixY_x.Text = rotation_matrix[0][1].ToString("0.###"); textBox_matrixZ_x.Text = rotation_matrix[0][2].ToString("0.###");
            textBox_matrixX_y.Text = rotation_matrix[1][0].ToString("0.###"); textBox_matrixY_y.Text = rotation_matrix[1][1].ToString("0.###"); textBox_matrixZ_y.Text = rotation_matrix[1][2].ToString("0.###");
            textBox_matrixX_z.Text = rotation_matrix[2][0].ToString("0.###"); textBox_matrixY_z.Text = rotation_matrix[2][1].ToString("0.###"); textBox_matrixZ_z.Text = rotation_matrix[2][2].ToString("0.###");
            //Bias
            textBox_biasX.Text = bias[0].ToString("0.###");
            textBox_biasY.Text = bias[1].ToString("0.###");
            textBox_biasZ.Text = bias[2].ToString("0.###");  */
        }

        public static double[] MatrixVectorMultiply(double[][] matrixA, double[] vectorB)
        {
            int aRows = matrixA.length; int aCols = matrixA[0].length;
            int bRows = vectorB.length;
         //   if (aCols != bRows)
           //     throw new Exception("Non-conformable matrices in MatrixProduct");
            double[] result = new double[aRows];
            for (int i = 0; i < aRows; ++i) // each row of A
                for (int k = 0; k < aCols; ++k)
                    result[i] += matrixA[i][k] * vectorB[k];
            return result;
        }
        
 
        
               public static double[][] InvertMatrix(double[][] A)
        {
          
         
            int n = A.length;
            //e will represent each column in the identity matrix
            double[] e;
            //x will hold the inverse matrix to be returned
            double[][] x = new double[n][];
            for (int i = 0; i < n; i++)
            {
                x[i] = new double[A[i].length];
            }
            /*
            * solve will contain the vector solution for the LUP decomposition as we solve
            * for each vector of x.  We will combine the solutions into the double[][] array x.
            * */
            double[] solve;

            //Get the LU matrix and P matrix (as an array)
          //  Tuple<double[][], int[]> results = LUPDecomposition(A);
          
            Matrix MatrixA = new Matrix(A);

        //  Pair<double[][], int[]>results = Pair.with(LUPDecomp(A));
        
            LUDecomposition LUPDecomp = new LUDecomposition(MatrixA);

            double[][] LU = MatrixA.getArrayCopy();
            int[] P = LUPDecomp.getPivot();
            
           // double[][] LU = results.Item1;
         //   int[] P = results.Item2;
         

            /*
            * Solve AX = e for each column ei of the identity matrix using LUP decomposition
            * */
            for (int i = 0; i < n; i++)
            {
                e = new double[A[i].length];
                e[i] = 1;
                solve = LUPSolve(LU, P, e);
              
                for (int j = 0; j < solve.length; j++)
                {
                    x[j][i] = solve[j];
                }
            } 
            return x;
        }
      
        


        public static double[] LUPSolve(double[][] LU, int[] pi, double[] b)
        {
            int n = LU.length - 1;
            double[] x = new double[n + 1];
            double[] y = new double[n + 1];
            double suml = 0;
            double sumu = 0;
            double lij = 0;

            /*
            * Solve for y using formward substitution
            * */
            for (int i = 0; i <= n; i++)
            {
                suml = 0;
                for (int j = 0; j <= i - 1; j++)
                {
                    /*
                    * Since we've taken L and U as a singular matrix as an input
                    * the value for L at index i and j will be 1 when i equals j, not LU[i][j], since
                    * the diagonal values are all 1 for L.
                    * */
                    if (i == j)
                    {
                        lij = 1;
                    }
                    else
                    {
                        lij = LU[i][j];
                    }
                    suml = suml + (lij * y[j]);
                }
                y[i] = b[pi[i]] - suml;
            }
            //Solve for x by using back substitution
            for (int i = n; i >= 0; i--)
            {
                sumu = 0;
                for (int j = i + 1; j <= n; j++)
                {
                    sumu = sumu + (LU[i][j] * x[j]);
                }
                x[i] = (y[i] - sumu) / LU[i][i];
            }
            return x;
        }


1 Like