private void getParameters(ArrayList<Double> time, ArrayList<Vector3D> waypoints, double[] parametersXToPack, double[] parametersYToPack, double[] parametersZToPack) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { timeMatrix.setElement(i, j, MathTools.pow(time.get(i), 2 - j)); } } System.out.println(timeMatrix); timeMatrix.invert(); Vector3D tempVector = new Vector3D(); for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersXToPack[i] = (tempVector.dot(new Vector3D(waypoints.get(0).getX(), waypoints.get(1).getX(), waypoints.get(2).getX()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersYToPack[i] = (tempVector.dot(new Vector3D(waypoints.get(0).getY(), waypoints.get(1).getY(), waypoints.get(2).getY()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersZToPack[i] = (tempVector.dot(new Vector3D(waypoints.get(0).getZ(), waypoints.get(1).getZ(), waypoints.get(2).getZ()))); } }