Java Code Examples for org.apache.commons.math3.exception.util.LocalizedFormats#UNABLE_TO_ORTHOGONOLIZE_MATRIX
The following examples show how to use
org.apache.commons.math3.exception.util.LocalizedFormats#UNABLE_TO_ORTHOGONOLIZE_MATRIX .
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private T[][] orthogonalizeMatrix(final T[][] m, final double threshold)
throws NotARotationMatrixException {
T x00 = m[0][0];
T x01 = m[0][1];
T x02 = m[0][2];
T x10 = m[1][0];
T x11 = m[1][1];
T x12 = m[1][2];
T x20 = m[2][0];
T x21 = m[2][1];
T x22 = m[2][2];
double fn = 0;
double fn1;
final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3);
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20));
final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20));
final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20));
final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21));
final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21));
final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21));
final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22));
final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22));
final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22));
// Xn+1
o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5));
o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5));
o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5));
o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5));
o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5));
o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5));
o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5));
o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5));
o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5));
// correction on each elements
final double corr00 = o[0][0].getReal() - m[0][0].getReal();
final double corr01 = o[0][1].getReal() - m[0][1].getReal();
final double corr02 = o[0][2].getReal() - m[0][2].getReal();
final double corr10 = o[1][0].getReal() - m[1][0].getReal();
final double corr11 = o[1][1].getReal() - m[1][1].getReal();
final double corr12 = o[1][2].getReal() - m[1][2].getReal();
final double corr20 = o[2][0].getReal() - m[2][0].getReal();
final double corr21 = o[2][1].getReal() - m[2][1].getReal();
final double corr22 = o[2][2].getReal() - m[2][2].getReal();
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o[0][0];
x01 = o[0][1];
x02 = o[0][2];
x10 = o[1][0];
x11 = o[1][1];
x12 = o[1][2];
x20 = o[2][0];
x21 = o[2][1];
x22 = o[2][2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 2
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 3
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 4
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 5
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private T[][] orthogonalizeMatrix(final T[][] m, final double threshold)
throws NotARotationMatrixException {
T x00 = m[0][0];
T x01 = m[0][1];
T x02 = m[0][2];
T x10 = m[1][0];
T x11 = m[1][1];
T x12 = m[1][2];
T x20 = m[2][0];
T x21 = m[2][1];
T x22 = m[2][2];
double fn = 0;
double fn1;
final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3);
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20));
final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20));
final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20));
final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21));
final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21));
final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21));
final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22));
final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22));
final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22));
// Xn+1
o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5));
o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5));
o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5));
o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5));
o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5));
o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5));
o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5));
o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5));
o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5));
// correction on each elements
final double corr00 = o[0][0].getReal() - m[0][0].getReal();
final double corr01 = o[0][1].getReal() - m[0][1].getReal();
final double corr02 = o[0][2].getReal() - m[0][2].getReal();
final double corr10 = o[1][0].getReal() - m[1][0].getReal();
final double corr11 = o[1][1].getReal() - m[1][1].getReal();
final double corr12 = o[1][2].getReal() - m[1][2].getReal();
final double corr20 = o[2][0].getReal() - m[2][0].getReal();
final double corr21 = o[2][1].getReal() - m[2][1].getReal();
final double corr22 = o[2][2].getReal() - m[2][2].getReal();
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o[0][0];
x01 = o[0][1];
x02 = o[0][2];
x10 = o[1][0];
x11 = o[1][1];
x12 = o[1][2];
x20 = o[2][0];
x21 = o[2][1];
x22 = o[2][2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 6
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 7
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 8
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private T[][] orthogonalizeMatrix(final T[][] m, final double threshold)
throws NotARotationMatrixException {
T x00 = m[0][0];
T x01 = m[0][1];
T x02 = m[0][2];
T x10 = m[1][0];
T x11 = m[1][1];
T x12 = m[1][2];
T x20 = m[2][0];
T x21 = m[2][1];
T x22 = m[2][2];
double fn = 0;
double fn1;
final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3);
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20));
final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20));
final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20));
final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21));
final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21));
final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21));
final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22));
final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22));
final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22));
// Xn+1
o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5));
o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5));
o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5));
o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5));
o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5));
o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5));
o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5));
o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5));
o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5));
// correction on each elements
final double corr00 = o[0][0].getReal() - m[0][0].getReal();
final double corr01 = o[0][1].getReal() - m[0][1].getReal();
final double corr02 = o[0][2].getReal() - m[0][2].getReal();
final double corr10 = o[1][0].getReal() - m[1][0].getReal();
final double corr11 = o[1][1].getReal() - m[1][1].getReal();
final double corr12 = o[1][2].getReal() - m[1][2].getReal();
final double corr20 = o[2][0].getReal() - m[2][0].getReal();
final double corr21 = o[2][1].getReal() - m[2][1].getReal();
final double corr22 = o[2][2].getReal() - m[2][2].getReal();
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o[0][0];
x01 = o[0][1];
x02 = o[0][2];
x10 = o[1][0];
x11 = o[1][1];
x12 = o[1][2];
x20 = o[2][0];
x21 = o[2][1];
x22 = o[2][2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 9
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 10
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private T[][] orthogonalizeMatrix(final T[][] m, final double threshold)
throws NotARotationMatrixException {
T x00 = m[0][0];
T x01 = m[0][1];
T x02 = m[0][2];
T x10 = m[1][0];
T x11 = m[1][1];
T x12 = m[1][2];
T x20 = m[2][0];
T x21 = m[2][1];
T x22 = m[2][2];
double fn = 0;
double fn1;
final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3);
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20));
final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20));
final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20));
final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21));
final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21));
final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21));
final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22));
final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22));
final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22));
// Xn+1
o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5));
o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5));
o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5));
o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5));
o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5));
o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5));
o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5));
o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5));
o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5));
// correction on each elements
final double corr00 = o[0][0].getReal() - m[0][0].getReal();
final double corr01 = o[0][1].getReal() - m[0][1].getReal();
final double corr02 = o[0][2].getReal() - m[0][2].getReal();
final double corr10 = o[1][0].getReal() - m[1][0].getReal();
final double corr11 = o[1][1].getReal() - m[1][1].getReal();
final double corr12 = o[1][2].getReal() - m[1][2].getReal();
final double corr20 = o[2][0].getReal() - m[2][0].getReal();
final double corr21 = o[2][1].getReal() - m[2][1].getReal();
final double corr22 = o[2][2].getReal() - m[2][2].getReal();
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o[0][0];
x01 = o[0][1];
x02 = o[0][2];
x10 = o[1][0];
x11 = o[1][1];
x12 = o[1][2];
x20 = o[2][0];
x21 = o[2][1];
x22 = o[2][2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 11
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 12
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private T[][] orthogonalizeMatrix(final T[][] m, final double threshold)
throws NotARotationMatrixException {
T x00 = m[0][0];
T x01 = m[0][1];
T x02 = m[0][2];
T x10 = m[1][0];
T x11 = m[1][1];
T x12 = m[1][2];
T x20 = m[2][0];
T x21 = m[2][1];
T x22 = m[2][2];
double fn = 0;
double fn1;
final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3);
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20));
final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20));
final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20));
final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21));
final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21));
final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21));
final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22));
final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22));
final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22));
// Xn+1
o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5));
o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5));
o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5));
o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5));
o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5));
o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5));
o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5));
o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5));
o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5));
// correction on each elements
final double corr00 = o[0][0].getReal() - m[0][0].getReal();
final double corr01 = o[0][1].getReal() - m[0][1].getReal();
final double corr02 = o[0][2].getReal() - m[0][2].getReal();
final double corr10 = o[1][0].getReal() - m[1][0].getReal();
final double corr11 = o[1][1].getReal() - m[1][1].getReal();
final double corr12 = o[1][2].getReal() - m[1][2].getReal();
final double corr20 = o[2][0].getReal() - m[2][0].getReal();
final double corr21 = o[2][1].getReal() - m[2][1].getReal();
final double corr22 = o[2][2].getReal() - m[2][2].getReal();
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o[0][0];
x01 = o[0][1];
x02 = o[0][2];
x10 = o[1][0];
x11 = o[1][1];
x12 = o[1][2];
x20 = o[2][0];
x21 = o[2][1];
x22 = o[2][2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
Example 13
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Perfect orthogonality on a 3X3 matrix.
* @param m initial matrix (not exactly orthogonal)
* @param threshold convergence threshold for the iterative
* orthogonality correction (convergence is reached when the
* difference between two steps of the Frobenius norm of the
* correction is below this threshold)
* @return an orthogonal matrix close to m
* @exception NotARotationMatrixException if the matrix cannot be
* orthogonalized with the given threshold after 10 iterations
*/
private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException {
double[] m0 = m[0];
double[] m1 = m[1];
double[] m2 = m[2];
double x00 = m0[0];
double x01 = m0[1];
double x02 = m0[2];
double x10 = m1[0];
double x11 = m1[1];
double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0;
double fn1;
double[][] o = new double[3][3];
double[] o0 = o[0];
double[] o1 = o[1];
double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0;
while (++i < 11) {
// Mt.Xn
double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1
o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements
double corr00 = o0[0] - m0[0];
double corr01 = o0[1] - m0[1];
double corr02 = o0[2] - m0[2];
double corr10 = o1[0] - m1[0];
double corr11 = o1[1] - m1[1];
double corr12 = o1[2] - m1[2];
double corr20 = o2[0] - m2[0];
double corr21 = o2[1] - m2[1];
double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test
if (FastMath.abs(fn1 - fn) <= threshold) {
return o;
}
// prepare next iteration
x00 = o0[0];
x01 = o0[1];
x02 = o0[2];
x10 = o1[0];
x11 = o1[1];
x12 = o1[2];
x20 = o2[0];
x21 = o2[1];
x22 = o2[2];
fn = fn1;
}
// the algorithm did not converge after 10 iterations
throw new NotARotationMatrixException(
LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX,
i - 1);
}
