Apache
/
commons-math
mirror of https://github.com/apache/commons-math.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

replaced arbitrarily small thresholds by 0 where it was sensible with respect to IEEE754 

removed unnecessary else clauses
removed double indices where possible
fixed some comments


git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@574264 13f79535-47bb-0310-9956-ffa450edef68
This commit is contained in:
Luc Maisonobe 2007-09-10 14:39:43 +00:00
parent c4cc69cf52
commit 02a39b2078
1 changed files with 66 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
src/java/org/apache/commons/math/geometry/Rotation.java
View File

@ -220,7 +220,7 @@ public class Rotation implements Serializable {
// There are different ways to compute the quaternions elements // There are different ways to compute the quaternions elements
// from the matrix. They all involve computing one element from // from the matrix. They all involve computing one element from
// the diagonal of the matrix, and computing the three other ones // the diagonal of the matrix, and computing the three other ones
// unsing a formula involving a division by the first element, // using a formula involving a division by the first element,
// which unfortunately can be null. Since the norm of the // which unfortunately can be null. Since the norm of the
// quaternion is 1, we know at least one element has an absolute // quaternion is 1, we know at least one element has an absolute
// value greater or equal to 0.5, so it is always possible to // value greater or equal to 0.5, so it is always possible to
@ -292,9 +292,9 @@ public class Rotation implements Serializable {
double u2u2 = Vector3D.dotProduct(u2, u2); double u2u2 = Vector3D.dotProduct(u2, u2);
double v1v1 = Vector3D.dotProduct(v1, v1); double v1v1 = Vector3D.dotProduct(v1, v1);
double v2v2 = Vector3D.dotProduct(v2, v2); double v2v2 = Vector3D.dotProduct(v2, v2);
if ((u1u1 < 1.0e-15) || (u2u2 < 1.0e-15) if ((u1u1 == 0) || (u2u2 == 0) || (v1v1 == 0) || (v2v2 == 0)) {
|| (v1v1 < 1.0e-15) || (v2v2 < 1.0e-15))
throw new ArithmeticException("null norm"); throw new ArithmeticException("null norm");
}
double u1x = u1.getX(); double u1x = u1.getX();
double u1y = u1.getY(); double u1y = u1.getY();
@ -304,7 +304,7 @@ public class Rotation implements Serializable {
double u2y = u2.getY(); double u2y = u2.getY();
double u2z = u2.getZ(); double u2z = u2.getZ();
// renormalize v1 in order to have (v1'|v1') = (u1|u1) // normalize v1 in order to have (v1'|v1') = (u1|u1)
double coeff = Math.sqrt (u1u1 / v1v1); double coeff = Math.sqrt (u1u1 / v1v1);
double v1x = coeff * v1.getX(); double v1x = coeff * v1.getX();
double v1y = coeff * v1.getY(); double v1y = coeff * v1.getY();
@ -341,7 +341,7 @@ public class Rotation implements Serializable {
+ k.getY() * (u1z * u2x - u1x * u2z) + k.getY() * (u1z * u2x - u1x * u2z)
+ k.getZ() * (u1x * u2y - u1y * u2x); + k.getZ() * (u1x * u2y - u1y * u2x);
if (c < (1.0e-10 * u1u1 * u2u2)) { if (c == 0) {
// the (q1, q2, q3) vector is in the (u1, u2) plane // the (q1, q2, q3) vector is in the (u1, u2) plane
// we try other vectors // we try other vectors
Vector3D u3 = Vector3D.crossProduct(u1, u2); Vector3D u3 = Vector3D.crossProduct(u1, u2);
@ -352,7 +352,6 @@ public class Rotation implements Serializable {
double v3x = v3.getX(); double v3x = v3.getX();
double v3y = v3.getY(); double v3y = v3.getY();
double v3z = v3.getZ(); double v3z = v3.getZ();
double u3u3 = u1u1 * u2u2 - u1u2 * u1u2;
double dx3 = v3x - u3x; double dx3 = v3x - u3x;
double dy3 = v3y - u3y; double dy3 = v3y - u3y;
@ -364,7 +363,7 @@ public class Rotation implements Serializable {
+ k.getY() * (u1z * u3x - u1x * u3z) + k.getY() * (u1z * u3x - u1x * u3z)
+ k.getZ() * (u1x * u3y - u1y * u3x); + k.getZ() * (u1x * u3y - u1y * u3x);
if (c < (1.0e-10 * u1u1 * u3u3)) { if (c == 0) {
// the (q1, q2, q3) vector is aligned with u1: // the (q1, q2, q3) vector is aligned with u1:
// we try (u2, u3) and (v2, v3) // we try (u2, u3) and (v2, v3)
k = new Vector3D(dy2 * dz3 - dz2 * dy3, k = new Vector3D(dy2 * dz3 - dz2 * dy3,
@ -374,7 +373,7 @@ public class Rotation implements Serializable {
+ k.getY() * (u2z * u3x - u2x * u3z) + k.getY() * (u2z * u3x - u2x * u3z)
+ k.getZ() * (u2x * u3y - u2y * u3x); + k.getZ() * (u2x * u3y - u2y * u3x);
if (c < (1.0e-10 * u2u2 * u3u3)) { if (c == 0) {
// the (q1, q2, q3) vector is aligned with everything // the (q1, q2, q3) vector is aligned with everything
// this is really the identity rotation // this is really the identity rotation
q0 = 1.0; q0 = 1.0;
@ -424,7 +423,7 @@ public class Rotation implements Serializable {
public Rotation(Vector3D u, Vector3D v) { public Rotation(Vector3D u, Vector3D v) {
double normProduct = u.getNorm() * v.getNorm(); double normProduct = u.getNorm() * v.getNorm();
if (normProduct < 1.0e-15) { if (normProduct == 0) {
throw new ArithmeticException("null norm"); throw new ArithmeticException("null norm");
} }
@ -525,15 +524,14 @@ public class Rotation implements Serializable {
*/ */
public Vector3D getAxis() { public Vector3D getAxis() {
double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; double squaredSine = q1 * q1 + q2 * q2 + q3 * q3;
if (squaredSine < 1.0e-12) { if (squaredSine == 0) {
return new Vector3D(1, 0, 0); return new Vector3D(1, 0, 0);
} else if (q0 < 0) { } else if (q0 < 0) {
double inverse = 1 / Math.sqrt(squaredSine); double inverse = 1 / Math.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
} else {
double inverse = -1 / Math.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
} }
double inverse = -1 / Math.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
} }
/** Get the angle of the rotation. /** Get the angle of the rotation.
@ -544,9 +542,8 @@ public class Rotation implements Serializable {
return 2 * Math.asin(Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3)); return 2 * Math.asin(Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3));
} else if (q0 < 0) { } else if (q0 < 0) {
return 2 * Math.acos(-q0); return 2 * Math.acos(-q0);
} else {
return 2 * Math.acos(q0);
} }
return 2 * Math.acos(q0);
} }
/** Get the Cardan or Euler angles corresponding to the instance. /** Get the Cardan or Euler angles corresponding to the instance.
@ -931,79 +928,82 @@ public class Rotation implements Serializable {
*/ */
private double[][] orthogonalizeMatrix(double[][] m, double threshold) private double[][] orthogonalizeMatrix(double[][] m, double threshold)
throws NotARotationMatrixException { throws NotARotationMatrixException {
double x00 = m[0][0]; double[] m0 = m[0];
double x01 = m[0][1]; double[] m1 = m[1];
double x02 = m[0][2]; double[] m2 = m[2];
double x10 = m[1][0]; double x00 = m0[0];
double x11 = m[1][1]; double x01 = m0[1];
double x12 = m[1][2]; double x02 = m0[2];
double x20 = m[2][0]; double x10 = m1[0];
double x21 = m[2][1]; double x11 = m1[1];
double x22 = m[2][2]; double x12 = m1[2];
double x20 = m2[0];
double x21 = m2[1];
double x22 = m2[2];
double fn = 0; double fn = 0;
double fn1; double fn1;
double[][] o = new double[3][]; double[][] o = new double[3][3];
o[0] = new double[3]; double[] o0 = o[0];
o[1] = new double[3]; double[] o1 = o[1];
o[2] = new double[3]; double[] o2 = o[2];
// iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
int i = 0; int i = 0;
while (++i < 11) { while (++i < 11) {
// Mt.Xn // Mt.Xn
double mx00 = m[0][0] * x00 + m[1][0] * x10 + m[2][0] * x20; double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
double mx10 = m[0][1] * x00 + m[1][1] * x10 + m[2][1] * x20; double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
double mx20 = m[0][2] * x00 + m[1][2] * x10 + m[2][2] * x20; double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
double mx01 = m[0][0] * x01 + m[1][0] * x11 + m[2][0] * x21; double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
double mx11 = m[0][1] * x01 + m[1][1] * x11 + m[2][1] * x21; double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
double mx21 = m[0][2] * x01 + m[1][2] * x11 + m[2][2] * x21; double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
double mx02 = m[0][0] * x02 + m[1][0] * x12 + m[2][0] * x22; double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
double mx12 = m[0][1] * x02 + m[1][1] * x12 + m[2][1] * x22; double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
double mx22 = m[0][2] * x02 + m[1][2] * x12 + m[2][2] * x22; double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
// Xn+1 // Xn+1
o[0][0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m[0][0]); o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
o[0][1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m[0][1]); o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
o[0][2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m[0][2]); o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
o[1][0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m[1][0]); o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
o[1][1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m[1][1]); o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
o[1][2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m[1][2]); o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
o[2][0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m[2][0]); o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
o[2][1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m[2][1]); o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
o[2][2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m[2][2]); o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
// correction on each elements // correction on each elements
double corr00 = o[0][0] - m[0][0]; double corr00 = o0[0] - m0[0];
double corr01 = o[0][1] - m[0][1]; double corr01 = o0[1] - m0[1];
double corr02 = o[0][2] - m[0][2]; double corr02 = o0[2] - m0[2];
double corr10 = o[1][0] - m[1][0]; double corr10 = o1[0] - m1[0];
double corr11 = o[1][1] - m[1][1]; double corr11 = o1[1] - m1[1];
double corr12 = o[1][2] - m[1][2]; double corr12 = o1[2] - m1[2];
double corr20 = o[2][0] - m[2][0]; double corr20 = o2[0] - m2[0];
double corr21 = o[2][1] - m[2][1]; double corr21 = o2[1] - m2[1];
double corr22 = o[2][2] - m[2][2]; double corr22 = o2[2] - m2[2];
// Frobenius norm of the correction // Frobenius norm of the correction
fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02
+ corr10 * corr10 + corr11 * corr11 + corr12 * corr12 + corr10 * corr10 + corr11 * corr11 + corr12 * corr12
+ corr20 * corr20 + corr21 * corr21 + corr22 * corr22; + corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
// convergence test // convergence test
if (Math.abs(fn1 - fn) <= threshold) if (Math.abs(fn1 - fn) <= threshold)
return o; return o;
// prepare next iteration // prepare next iteration
x00 = o[0][0]; x00 = o0[0];
x01 = o[0][1]; x01 = o0[1];
x02 = o[0][2]; x02 = o0[2];
x10 = o[1][0]; x10 = o1[0];
x11 = o[1][1]; x11 = o1[1];
x12 = o[1][2]; x12 = o1[2];
x20 = o[2][0]; x20 = o2[0];
x21 = o[2][1]; x21 = o2[1];
x22 = o[2][2]; x22 = o2[2];
fn = fn1; fn = fn1;
} }