Более эффективное жесткое 3D-преобразование в Java

Question

Я хочу вычислить жесткое трехмерное преобразование между двумя наборами трехмерных точек.Я сам погуглил, не нашел подходящей реализации и сам реализовал ее с помощью библиотеки Apache Commons Math, основанной на этом руководстве .Реализацию можно увидеть ниже:

  public static RigidTransformation3dAnswer computeRigidTransformation3D(RealMatrix src,
      RealMatrix dst) {
    if (src.getRowDimension() == dst.getRowDimension() && src.getColumnDimension() == dst
        .getColumnDimension()) {

      int n = src.getRowDimension();
      RealMatrix centroidSrc = computeCentroid(src);
      RealMatrix centroidDst = computeCentroid(dst);

      RealMatrix aa = src.subtract(tile(centroidSrc, n));
      RealMatrix bb = dst.subtract(tile(centroidDst, n));

      RealMatrix h = aa.transpose().multiply(bb);
      SingularValueDecomposition singularValueDecomposition = new SingularValueDecomposition(h);

      RealMatrix u = singularValueDecomposition.getU();
      RealMatrix vt = singularValueDecomposition.getVT();
      RealMatrix rotationMatrix = vt.transpose().multiply(u.transpose());

      if (new LUDecomposition(rotationMatrix).getDeterminant() < 0) {
        vt.setColumn(2, vt.getColumnVector(2).mapMultiplyToSelf(-1).toArray());
        rotationMatrix = vt.transpose().multiply(u.transpose());
      }

      RealMatrix transpose = (rotationMatrix.scalarMultiply(-1).multiply(centroidSrc.transpose()))
          .add(centroidDst.transpose());

      RigidTransformation3dAnswer answer = new RigidTransformation3dAnswer();
      answer.setRotationMatrix(rotationMatrix);
      answer.setTranslationMatrix(transpose);
      return answer;
    }
    return null;
  }

private static RealMatrix tile(RealMatrix a, int n) {
    RealMatrix realMatrix = new Array2DRowRealMatrix(n, a.getColumnDimension());
    for (int i = 0; i < n; i++) {
      realMatrix.setEntry(i, 0, a.getEntry(0, 0));
      realMatrix.setEntry(i, 1, a.getEntry(0, 1));
      realMatrix.setEntry(i, 2, a.getEntry(0, 2));
    }
    return realMatrix;
  }

private static RealMatrix computeCentroid(RealMatrix mat) {
    double sumX = 0;
    double sumY = 0;
    double sumZ = 0;
    double[][] returnArray = new double[1][3];
    for (int i = 0; i < mat.getRowDimension(); i++) {
      double a = mat.getEntry(i, 0);
      sumX = sumX + a;
      a = mat.getEntry(i, 1);
      sumY = sumY + a;
      a = mat.getEntry(i, 2);
      sumZ = sumZ + a;
    }
    double centroidX = sumX / (double) mat.getRowDimension();
    double centroidY = sumY / (double) mat.getRowDimension();
    double centroidZ = sumZ / (double) mat.getRowDimension();
    returnArray[0][0] = centroidX;
    returnArray[0][1] = centroidY;
    returnArray[0][2] = centroidZ;
    return new Array2DRowRealMatrix(returnArray);
  }

Хотя эта реализация работает отлично, она не очень эффективна и занимает около 50 мс на рабочем столе, что не подходит, потому что я хочу использовать ее в приложении Android.Итак, вот три вопроса:

a) существует ли более эффективный библиотечный или каркасный метод, который можно использовать для вычисления жесткого трехмерного преобразования?

b) Если нет, существует ли эвристика и, следовательно,более эффективна реализация такого преобразования, если, например, я исключаю перевод и хочу только вращение вокруг вертикальной оси Z?

c) Если ни у одной из этих двух точек нет ответа, есть лиспособ повысить эффективность моего кода?