private MatrixFormulation() {
    int numRows = response.length;
    int numCols = predictors.length + ((hasIntercept) ? 1 : 0);
    this.X = createMatrixA(numRows, numCols);
    this.Xt = new DenseMatrix64F(numCols, numRows);
    CommonOps.transpose(X, Xt);
    this.XtXInv = new DenseMatrix64F(numCols, numCols);
    this.b = new DenseMatrix64F(numCols, 1);
    this.y = new DenseMatrix64F(numRows, 1);
    solveSystem(numRows, numCols);
    this.fitted = computeFittedValues();
    this.residuals = computeResiduals();
    this.sigma2 = estimateSigma2(numCols);
    this.covarianceMatrix = new DenseMatrix64F(numCols, numCols);
    CommonOps.scale(sigma2, XtXInv, covarianceMatrix);
}
 

private void computeInverse() {

        if (DEBUG) {
            System.err.println("CachedMatrixInverse.computeInverse()");
        }
        
        if (EMJL) {
            // TODO Avoid multiple copies
            DenseMatrix64F source = new DenseMatrix64F(base.getParameterAsMatrix());
            DenseMatrix64F destination = new DenseMatrix64F(getColumnDimension(), getColumnDimension());
            CommonOps.invert(source, destination);
            inverse = new WrappedMatrix.WrappedDenseMatrix(destination);
        } else {
         inverse = new WrappedMatrix.ArrayOfArray(new Matrix(base.getParameterAsMatrix()).inverse().toComponents());
        }
    }
 

private static NormalSufficientStatistics computeJointLatent(NormalSufficientStatistics below,
                                                             NormalSufficientStatistics above,
                                                             int dim) {

    DenseMatrix64F mean = new DenseMatrix64F(dim, 1);
    DenseMatrix64F precision = new DenseMatrix64F(dim, dim);
    DenseMatrix64F variance = new DenseMatrix64F(dim, dim);

    CommonOps.add(below.getRawPrecision(), above.getRawPrecision(), precision);
    safeInvert2(precision, variance, false);

    safeWeightedAverage(
            new WrappedVector.Raw(below.getRawMean().getData(), 0, dim),
            below.getRawPrecision(),
            new WrappedVector.Raw(above.getRawMean().getData(), 0, dim),
            above.getRawPrecision(),
            new WrappedVector.Raw(mean.getData(), 0, dim),
            variance,
            dim);

    return new NormalSufficientStatistics(mean, precision, variance);
}
 

private DenseMatrix64F getGradientBranchVarianceWrtAttenuationDiagonal(ContinuousDiffusionIntegrator cdi,
                                                                    int nodeIndex, DenseMatrix64F gradient) {

    double[] attenuation = elasticModel.getEigenValuesStrengthOfSelection();
    DenseMatrix64F variance = wrap(
            ((MultivariateIntegrator) cdi).getVariance(getEigenBufferOffsetIndex(0)),
            0, dim, dim);

    double ti = cdi.getBranchLength(getMatrixBufferOffsetIndex(nodeIndex));

    DenseMatrix64F res = new DenseMatrix64F(dim, 1);

    CommonOps.elementMult(variance, gradient);

    for (int k = 0; k < dim; k++) {
        double sum = 0.0;
        for (int l = 0; l < dim; l++) {
            sum -= variance.unsafe_get(k, l) * computeAttenuationFactorActualized(attenuation[k] + attenuation[l], ti);
        }
        res.unsafe_set(k, 0, sum);
    }

    return res;
}
 

@Override
public double[] chainRule(ContinuousDiffusionIntegrator cdi,
                          DiffusionProcessDelegate diffusionProcessDelegate,
                          ContinuousDataLikelihoodDelegate likelihoodDelegate,
                          BranchSufficientStatistics statistics, NodeRef node,
                          final DenseMatrix64F gradQInv, final DenseMatrix64F gradN) {

    DenseMatrix64F gradQInvDiag = ((OUDiffusionModelDelegate) diffusionProcessDelegate).getGradientVarianceWrtAttenuation(node, cdi, statistics, gradQInv);

    if (DEBUG) {
        System.err.println("gradQ = " + NormalSufficientStatistics.toVectorizedString(gradQInv));
    }

    DenseMatrix64F gradNDiag = ((OUDiffusionModelDelegate) diffusionProcessDelegate).getGradientDisplacementWrtAttenuation(node, cdi, statistics, gradN);

    CommonOps.addEquals(gradQInvDiag, gradNDiag);

    return gradQInvDiag.getData();
}
 

@Override
public void setDiffusionPrecision(int precisionIndex, final double[] matrix, double logDeterminant) {
    super.setDiffusionPrecision(precisionIndex, matrix, logDeterminant);

    assert (inverseDiffusions != null);

    final int offset = dimProcess * dimProcess * precisionIndex;
    DenseMatrix64F precision = wrap(diffusions, offset, dimProcess, dimProcess);
    DenseMatrix64F variance = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.invert(precision, variance);
    unwrap(variance, inverseDiffusions, offset);

    if (DEBUG) {
        System.err.println("At precision index: " + precisionIndex);
        System.err.println("precision: " + precision);
        System.err.println("variance : " + variance);
    }
}
 

@Override
public void getBranchExpectation(double[] actualization, double[] parentValue, double[] displacement,
                                 double[] expectation) {

    assert (expectation != null);
    assert (expectation.length >= dimTrait);

    assert (actualization != null);
    assert (actualization.length >= dimTrait * dimTrait);

    assert (parentValue != null);
    assert (parentValue.length >= dimTrait);

    assert (displacement != null);
    assert (displacement.length >= dimTrait);

    DenseMatrix64F branchExpectationMatrix = new DenseMatrix64F(dimTrait, 1);
    CommonOps.mult(wrap(actualization, 0, dimTrait, dimTrait),
            wrap(parentValue, 0, dimTrait, 1),
            branchExpectationMatrix);
    CommonOps.addEquals(branchExpectationMatrix, wrap(displacement, 0, dimTrait, 1));

    unwrap(branchExpectationMatrix, expectation, 0);
}
 

private void computeIOUActualizedDisplacement(final double[] optimalRates,
                                              final int offset,
                                              final int pio,
                                              double branchLength,
                                              DenseMatrix64F inverseSelectionStrength) {
    DenseMatrix64F displacementOU = wrap(displacements, pio, dimProcess, 1);
    DenseMatrix64F optVal = wrap(optimalRates, offset, dimProcess, 1);
    DenseMatrix64F displacement = new DenseMatrix64F(dimProcess, 1);

    CommonOps.mult(inverseSelectionStrength, displacementOU, displacement);
    CommonOps.scale(-1.0, displacement);

    CommonOps.addEquals(displacement, branchLength, optVal);

    unwrap(displacement, displacements, pio + dimProcess);
}
 

private void computeIOUActualization(final int scaledOffset,
                                     DenseMatrix64F inverseSelectionStrength) {
    // YY
    DenseMatrix64F actualizationOU = wrap(actualizations, scaledOffset, dimProcess, dimProcess);

    // XX
    DenseMatrix64F temp = CommonOps.identity(dimProcess);
    CommonOps.addEquals(temp, -1.0, actualizationOU);
    DenseMatrix64F actualizationIOU = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(inverseSelectionStrength, temp, actualizationIOU);

    // YX and XX
    DenseMatrix64F actualizationYX = new DenseMatrix64F(dimProcess, dimProcess); // zeros
    DenseMatrix64F actualizationXX = CommonOps.identity(dimProcess);

    blockUnwrap(actualizationOU, actualizationXX, actualizationIOU, actualizationYX, actualizations, scaledOffset);
}
 

private void schurComplementInverse(final DenseMatrix64F A, final DenseMatrix64F D,
                                    final DenseMatrix64F C, final DenseMatrix64F B,
                                    final double[] destination, final int offset) {
    DenseMatrix64F invA = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.invert(A, invA);
    DenseMatrix64F invMatD = getSchurInverseComplement(invA, D, C, B);

    DenseMatrix64F invAB = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(invA, B, invAB);
    DenseMatrix64F invMatB = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(-1.0, invAB, invMatD, invMatB);

    DenseMatrix64F CinvA = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(C, invA, CinvA);
    DenseMatrix64F invMatC = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(-1.0, invMatD, CinvA, invMatC);

    DenseMatrix64F invMatA = new DenseMatrix64F(dimProcess, dimProcess);
    CommonOps.mult(-1.0, invMatB, CinvA, invMatA);
    CommonOps.addEquals(invMatA, invA);

    blockUnwrap(invMatA, invMatD, invMatC, invMatB, destination, offset);
}
 

@Override
void computePartialPrecision(int ido, int jdo, int imo, int jmo,
                             DenseMatrix64F Pip, DenseMatrix64F Pjp, DenseMatrix64F Pk) {

    final double[] diagQdi = vectorDiagQdi;
    System.arraycopy(diagonal1mActualizations, ido, diagQdi, 0, dimTrait);
    oneMinus(diagQdi);
    final double[] diagQdj = vectorDiagQdj;
    System.arraycopy(diagonal1mActualizations, jdo, diagQdj, 0, dimTrait);
    oneMinus(diagQdj);

    final DenseMatrix64F QdiPipQdi = matrix0;
    final DenseMatrix64F QdjPjpQdj = matrix1;
    diagonalDoubleProduct(Pip, diagQdi, QdiPipQdi);
    diagonalDoubleProduct(Pjp, diagQdj, QdjPjpQdj);
    CommonOps.add(QdiPipQdi, QdjPjpQdj, Pk);

    if (DEBUG) {
        System.err.println("Qdi: " + Arrays.toString(diagQdi));
        System.err.println("\tQdiPipQdi: " + QdiPipQdi);
        System.err.println("\tQdj: " + Arrays.toString(diagQdj));
        System.err.println("\tQdjPjpQdj: " + QdjPjpQdj);
    }
}
 

public void testJacobian() {
    System.out.println("\nTest LKJ Jacobian.");

    // Matrix
    double[][] jacobianMat = transform.computeJacobianMatrixInverse(unconstrained);
    double jacobianDetBis = Math.log(CommonOps.det(new DenseMatrix64F(jacobianMat)));

    // Determinant
    double jacobianDet = (new Transform.InverseMultivariate(transform)).getLogJacobian(unconstrained, 0, unconstrained.length);

    System.out.println("Log Jacobiant Det direct=" + jacobianDet);
    System.out.println("Log Jacobiant Det matrix=" + jacobianDetBis);

    assertEquals("jacobian log det",
            format.format(jacobianDet),
            format.format(jacobianDetBis));
}
 

/**
 * Converts a vector from sample space into eigen space. If {@link #doWhitening} is true, then the vector
 * is also L2 normalized after projection.
 * 
 * @param sampleData
 *            Sample space vector
 * @return Eigen space projected vector
 * @throws Exception
 */
public double[] sampleToEigenSpace(double[] sampleData) throws Exception {
	if (!isPcaInitialized) {
		// initiallize PCA correctly!
		throw new Exception("PCA is not correctly initiallized!");
	}
	if (sampleData.length != sampleSize) {
		throw new IllegalArgumentException("Unexpected vector length!");
	}
	DenseMatrix64F sample = new DenseMatrix64F(sampleSize, 1, true, sampleData);
	DenseMatrix64F projectedSample = new DenseMatrix64F(numComponents, 1);
	// mean subtraction
	CommonOps.sub(sample, means, sample);
	// projection
	CommonOps.mult(V_t, sample, projectedSample);
	// whitening
	if (doWhitening) { // always perform this normalization step when whitening is used
		return Normalization.normalizeL2(projectedSample.data);
	} else {
		return projectedSample.data;
	}
}
 

private void solveSystem(int numRows, int numCols) {
    LinearSolver<DenseMatrix64F> qrSolver = LinearSolverFactory.qr(numRows, numCols);
    QRDecomposition<DenseMatrix64F> decomposition = qrSolver.getDecomposition();
    qrSolver.setA(X);
    y.setData(response);
    qrSolver.solve(this.y, this.b);
    DenseMatrix64F R = decomposition.getR(null, true);
    LinearSolver<DenseMatrix64F> linearSolver = LinearSolverFactory.linear(numCols);
    linearSolver.setA(R);
    DenseMatrix64F Rinverse = new DenseMatrix64F(numCols, numCols);
    linearSolver.invert(Rinverse); // stores solver's solution inside of Rinverse.
    CommonOps.multOuter(Rinverse, this.XtXInv);
}
 

public static DenseMatrix64F wrapSpherical(final double[] source, final int offset,
                                           final int dim,
                                           final double[] buffer) {
    fillSpherical(source, offset, dim, buffer);
    DenseMatrix64F res = DenseMatrix64F.wrap(dim, dim, buffer);
    CommonOps.transpose(res); // Column major.
    return res;
}
 

public static void symmPosDefInvert(DenseMatrix64F P, DenseMatrix64F P_inv) {
    LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.symmPosDef(P.getNumCols());
    DenseMatrix64F Pbis = new DenseMatrix64F(P);
    if (!solver.setA(Pbis)) {
        CommonOps.invert(P, P_inv);
    } else {
        solver.invert(P_inv);
    }
}
 

private void computeTransformedMatrix() {
    DenseMatrix64F baseMatrix = wrapSpherical(offDiagonalParameter.getParameterValues(), 0, dim);
    DenseMatrix64F diagonalMatrix = wrapDiagonal(diagonalParameter.getParameterValues(), 0, dim);

    CommonOps.mult(baseMatrix, diagonalMatrix, temp);
    CommonOps.invert(baseMatrix);
    CommonOps.mult(temp, baseMatrix, transformedMatrix);

    compositionKnown = true;
}
 

private static void rotateNd(double[] x, int dim) {

        // Get first `dim` locations
        DenseMatrix64F matrix = new DenseMatrix64F(dim, dim);
        for (int row = 0; row < dim; ++row) {
            for (int col = 0; col < dim; ++col) {
                matrix.set(row, col, x[col * dim + row]);
            }
        }

        // Do a QR decomposition
        QRDecomposition<DenseMatrix64F> qr = DecompositionFactory.qr(dim, dim);
        qr.decompose(matrix);
        DenseMatrix64F qm = qr.getQ(null, true);
        DenseMatrix64F rm = qr.getR(null, true);

        // Reflection invariance
        if (rm.get(0,0) < 0) {
            CommonOps.scale(-1, rm);
            CommonOps.scale(-1, qm);
        }

        // Compute Q^{-1}
        DenseMatrix64F qInv = new DenseMatrix64F(dim, dim);
        CommonOps.transpose(qm, qInv);

        // Apply to each location
        for (int location = 0; location < x.length / dim; ++location) {
            WrappedVector locationVector = new WrappedVector.Raw(x, location * dim, dim);
            MissingOps.matrixVectorMultiple(qInv, locationVector, locationVector, dim);
        }
    }
 

DenseMatrix64F getPrecisionBranch(double branchPrecision){
    if (!hasDrift) {
        DenseMatrix64F P1 = new DenseMatrix64F(dimTrait, dimTrait);
        CommonOps.scale(branchPrecision, Pd, P1);
        return P1;
    } else {
        return DenseMatrix64F.wrap(dimTrait, dimTrait, precisionBuffer);
    }
}
 

DenseMatrix64F getVarianceBranch(double branchPrecision){
    if (!hasDrift) {
        final DenseMatrix64F V1 = new DenseMatrix64F(dimTrait, dimTrait);
        CommonOps.scale(1.0 / branchPrecision, Vd, V1);
        return V1;
    } else {
        DenseMatrix64F P = getPrecisionBranch(branchPrecision);
        DenseMatrix64F V = new DenseMatrix64F(dimTrait, dimTrait);
        CommonOps.invert(P, V);
        return V;
    }
}
 

public ConditionalPrecisionAndTransform2(final DenseMatrix64F precision,
                                         final int[] missingIndices,
                                         final int[] notMissingIndices) {

    assert (missingIndices.length + notMissingIndices.length == precision.getNumRows());
    assert (missingIndices.length + notMissingIndices.length == precision.getNumCols());

    this.missingIndices = missingIndices;
    this.notMissingIndices = notMissingIndices;

    DenseMatrix64F P11 = new DenseMatrix64F(missingIndices.length, missingIndices.length);
    MissingOps.gatherRowsAndColumns(precision, P11, missingIndices, missingIndices);

    P11Inv = new DenseMatrix64F(missingIndices.length, missingIndices.length);
    CommonOps.invert(P11, P11Inv);

    DenseMatrix64F P12 = new DenseMatrix64F(missingIndices.length, notMissingIndices.length);
    MissingOps.gatherRowsAndColumns(precision, P12, missingIndices, notMissingIndices);

    DenseMatrix64F P11InvP12 = new DenseMatrix64F(missingIndices.length, notMissingIndices.length);
    CommonOps.mult(P11Inv, P12, P11InvP12);

    this.affineTransform = P11InvP12;

    this.numMissing = missingIndices.length;
    this.numNotMissing = notMissingIndices.length;
}
 

private DenseMatrix64F scaleGradient(double scalar, DenseMatrix64F gradient) {
    DenseMatrix64F result = gradient.copy();
    if (scalar == 0.0) {
        CommonOps.fill(result, 0.0);
    } else {
        CommonOps.scale(scalar, result);
    }
    return result;
}
 

public void makeGradientMatrices0(DenseMatrix64F matrix1, DenseMatrix64F logDetComponent,
                                  BranchSufficientStatistics statistics, double differentialScaling) {

    final NormalSufficientStatistics above = statistics.getAbove();
    final NormalSufficientStatistics branch = statistics.getBranch();

    DenseMatrix64F Qi = above.getRawPrecision();
    CommonOps.scale(differentialScaling, branch.getRawVariance(), matrix1); //matrix1 = Si
    CommonOps.mult(Qi, matrix1, logDetComponent); //matrix0 = logDetComponent
    CommonOps.mult(logDetComponent, Qi, matrix1); //matrix1 = QuadraticComponent

}
 

private DenseMatrix64F getGradientVarianceWrtAttenuationDiagonal(ContinuousDiffusionIntegrator cdi,
                                                                 BranchSufficientStatistics statistics,
                                                                 int nodeIndex, DenseMatrix64F gradient) {
    // wrt to q_i actualization
    DenseMatrix64F gradActualization = getGradientVarianceWrtActualizationDiagonal(cdi, statistics, nodeIndex, gradient);

    // wrt to Gamma stationary variance
    DenseMatrix64F gradStationary = getGradientBranchVarianceWrtAttenuationDiagonal(cdi, nodeIndex, gradient);

    CommonOps.addEquals(gradActualization, gradStationary);
    return gradActualization;

}
 

private DenseMatrix64F getGradientDisplacementWrtAttenuationDiagonal(ContinuousDiffusionIntegrator cdi, BranchSufficientStatistics statistics,
                                                                         NodeRef node, DenseMatrix64F gradient) {
        int nodeIndex = node.getNumber();
        // q_i
//        double[] qi = new double[dim];
//        cdi.getBranchActualization(getMatrixBufferOffsetIndex(nodeIndex), qi);
//        DenseMatrix64F qi = wrapDiagonal(actualization, 0, dim);
        // q_i^-1
//        DenseMatrix64F qiInv = wrapDiagonalInverse(actualization, 0, dim);
        // ni
        DenseMatrix64F ni = statistics.getAbove().getRawMean();
        // beta_i
        DenseMatrix64F betai = wrap(getDriftRate(node), 0, dim, 1);

        // factor
//        DenseMatrix64F tmp = new DenseMatrix64F(dim, 1);
        DenseMatrix64F resFull = new DenseMatrix64F(dim, dim);
        DenseMatrix64F resDiag = new DenseMatrix64F(dim, 1);
        CommonOps.add(ni, -1, betai, resDiag);
//        MissingOps.diagDiv(qi, resDiag);
        CommonOps.multTransB(gradient, resDiag, resFull);

        // Extract diag
        CommonOps.extractDiag(resFull, resDiag);

        // Wrt attenuation
        double ti = cdi.getBranchLength(getMatrixBufferOffsetIndex(nodeIndex));
        chainRuleActualizationWrtAttenuationDiagonal(ti, resDiag);

        return resDiag;

    }
 

private void actualizeDisplacementGradient(ContinuousDiffusionIntegrator cdi,
                                               int nodeIndex, DenseMatrix64F gradient) {
        // q_i
        double[] qi = new double[dim * dim];
        cdi.getBranch1mActualization(getMatrixBufferOffsetIndex(nodeIndex), qi);
        DenseMatrix64F Actu = wrap(qi, 0, dim, dim);
        CommonOps.scale(-1.0, Actu);
//        for (int i = 0; i < dim; i++) {
//            Actu.unsafe_set(i, i, Actu.unsafe_get(i, i) - 1.0);
//        }
        DenseMatrix64F tmp = new DenseMatrix64F(dim, 1);
        CommonOps.mult(Actu, gradient, tmp);
        CommonOps.scale(-1.0, tmp, gradient);
    }
 

private double[] actualizeRootGradientFull(ContinuousDiffusionIntegrator cdi,
                                           int nodeIndex, DenseMatrix64F gradient) {
    // q_i
    double[] qi = new double[dim * dim];
    cdi.getBranchActualization(getMatrixBufferOffsetIndex(nodeIndex), qi);
    DenseMatrix64F Actu = wrap(qi, 0, dim, dim);
    DenseMatrix64F tmp = new DenseMatrix64F(dim, 1);
    CommonOps.mult(Actu, gradient, tmp);
    return tmp.getData();
}
 

@Override
public double[] chainRule(BranchSufficientStatistics statistics, NodeRef node,
                          DenseMatrix64F gradQInv, DenseMatrix64F gradN) {

    final double rate = branchRateModel.getBranchRate(tree, node);
    final double differential = branchRateModel.getBranchRateDifferential(tree, node);
    final double scaling = differential / rate;

    // Q_i w.r.t. rate
    DenseMatrix64F gradMatQInv = matrixJacobianQInv;
    CommonOps.scale(scaling, statistics.getBranch().getRawVariance(), gradMatQInv);

    double[] gradient = new double[1];
    for (int i = 0; i < gradMatQInv.getNumElements(); i++) {
        gradient[0] += gradMatQInv.get(i) * gradQInv.get(i);
    }

    // n_i w.r.t. rate
    // TODO: Fix delegate to (possibly) un-link drift from arbitrary rate
    DenseMatrix64F gradMatN = matrixJacobianN;
    CommonOps.scale(scaling, statistics.getBranch().getRawDisplacement(), gradMatN);
    for (int i = 0; i < gradMatN.numRows; i++) {
        gradient[0] += gradMatN.get(i) * gradN.get(i);
    }

    return gradient;

}
 

@Override
public void calculatePreOrderRoot(int priorBufferIndex, int rootNodeIndex, int precisionIndex) {

    super.calculatePreOrderRoot(priorBufferIndex, rootNodeIndex, precisionIndex);

    updatePrecisionOffsetAndDeterminant(precisionIndex);

    final DenseMatrix64F Pd = wrap(diffusions, precisionOffset, dimTrait, dimTrait);
    final DenseMatrix64F Vd = wrap(inverseDiffusions, precisionOffset, dimTrait, dimTrait);

    int rootOffset = dimPartial * rootNodeIndex;

    // TODO For each trait in parallel
    for (int trait = 0; trait < numTraits; ++trait) {

        @SuppressWarnings("SpellCheckingInspection") final DenseMatrix64F Proot = wrap(preOrderPartials, rootOffset + dimTrait, dimTrait, dimTrait);
        @SuppressWarnings("SpellCheckingInspection") final DenseMatrix64F Vroot = wrap(preOrderPartials, rootOffset + dimTrait + dimTrait * dimTrait, dimTrait, dimTrait);

        // TODO Block below is for the conjugate prior ONLY
        {
            final DenseMatrix64F tmp = matrix0;

            MissingOps.safeMult(Pd, Proot, tmp);
            unwrap(tmp, preOrderPartials, rootOffset + dimTrait);

            CommonOps.mult(Vd, Vroot, tmp);
            unwrap(tmp, preOrderPartials, rootOffset + dimTrait + dimTrait * dimTrait);
        }
        rootOffset += dimPartialForTrait;
    }
}
 

private static void transformMatrixGeneral(DenseMatrix64F matrix, DenseMatrix64F rotation) {
    int dim = matrix.getNumRows();
    DenseMatrix64F tmp = new DenseMatrix64F(dim, dim);
    DenseMatrix64F rotationInverse = new DenseMatrix64F(dim, dim);
    CommonOps.invert(rotation, rotationInverse);
    CommonOps.mult(rotationInverse, matrix, tmp);
    CommonOps.multTransB(tmp, rotationInverse, matrix);
}