commons-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From l..@apache.org
Subject svn commit: r574264 - /commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java
Date Mon, 10 Sep 2007 14:39:43 GMT
Author: luc
Date: Mon Sep 10 07:39:43 2007
New Revision: 574264

URL: http://svn.apache.org/viewvc?rev=574264&view=rev
Log:
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

Modified:
    commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java

Modified: commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java?rev=574264&r1=574263&r2=574264&view=diff
==============================================================================
--- commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java (original)
+++ commons/proper/math/trunk/src/java/org/apache/commons/math/geometry/Rotation.java Mon
Sep 10 07:39:43 2007
@@ -220,7 +220,7 @@
     // There are different ways to compute the quaternions elements
     // from the matrix. They all involve computing one element from
     // 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
     // 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
@@ -292,9 +292,9 @@
   double u2u2 = Vector3D.dotProduct(u2, u2);
   double v1v1 = Vector3D.dotProduct(v1, v1);
   double v2v2 = Vector3D.dotProduct(v2, v2);
-  if ((u1u1 < 1.0e-15) || (u2u2 < 1.0e-15)
-      || (v1v1 < 1.0e-15) || (v2v2 < 1.0e-15))
+  if ((u1u1 == 0) || (u2u2 == 0) || (v1v1 == 0) || (v2v2 == 0)) {
     throw new ArithmeticException("null norm");
+  }
 
   double u1x = u1.getX();
   double u1y = u1.getY();
@@ -304,7 +304,7 @@
   double u2y = u2.getY();
   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 v1x   = coeff * v1.getX();
   double v1y   = coeff * v1.getY();
@@ -341,7 +341,7 @@
            + k.getY() * (u1z * u2x - u1x * u2z)
            + 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
     // we try other vectors
     Vector3D u3 = Vector3D.crossProduct(u1, u2);
@@ -352,7 +352,6 @@
     double v3x  = v3.getX();
     double v3y  = v3.getY();
     double v3z  = v3.getZ();
-    double u3u3 = u1u1 * u2u2 - u1u2 * u1u2;
 
     double dx3 = v3x - u3x;
     double dy3 = v3y - u3y;
@@ -364,7 +363,7 @@
       + k.getY() * (u1z * u3x - u1x * u3z)
       + 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:
       // we try (u2, u3) and (v2, v3)
       k = new Vector3D(dy2 * dz3 - dz2 * dy3,
@@ -374,7 +373,7 @@
         + k.getY() * (u2z * u3x - u2x * u3z)
         + 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
         // this is really the identity rotation
         q0 = 1.0;
@@ -424,7 +423,7 @@
   public Rotation(Vector3D u, Vector3D v) {
 
     double normProduct = u.getNorm() * v.getNorm();
-    if (normProduct < 1.0e-15) {
+    if (normProduct == 0) {
       throw new ArithmeticException("null norm");
     }
 
@@ -525,15 +524,14 @@
    */
   public Vector3D getAxis() {
     double squaredSine = q1 * q1 + q2 * q2 + q3 * q3;
-    if (squaredSine < 1.0e-12) {
+    if (squaredSine == 0) {
       return new Vector3D(1, 0, 0);
     } else if (q0 < 0) {
       double inverse = 1 / Math.sqrt(squaredSine);
       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.
@@ -544,9 +542,8 @@
       return 2 * Math.asin(Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3));
     } else if (q0 < 0) {
       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.
@@ -931,79 +928,82 @@
    */
   private double[][] orthogonalizeMatrix(double[][] m, double threshold)
     throws NotARotationMatrixException {
-    double x00 = m[0][0];
-    double x01 = m[0][1];
-    double x02 = m[0][2];
-    double x10 = m[1][0];
-    double x11 = m[1][1];
-    double x12 = m[1][2];
-    double x20 = m[2][0];
-    double x21 = m[2][1];
-    double x22 = m[2][2];
+    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][];
-    o[0] = new double[3];
-    o[1] = new double[3];
-    o[2] = new double[3];
+    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 = m[0][0] * x00 + m[1][0] * x10 + m[2][0] * x20;
-      double mx10 = m[0][1] * x00 + m[1][1] * x10 + m[2][1] * x20;
-      double mx20 = m[0][2] * x00 + m[1][2] * x10 + m[2][2] * x20;
-      double mx01 = m[0][0] * x01 + m[1][0] * x11 + m[2][0] * x21;
-      double mx11 = m[0][1] * x01 + m[1][1] * x11 + m[2][1] * x21;
-      double mx21 = m[0][2] * x01 + m[1][2] * x11 + m[2][2] * x21;
-      double mx02 = m[0][0] * x02 + m[1][0] * x12 + m[2][0] * x22;
-      double mx12 = m[0][1] * x02 + m[1][1] * x12 + m[2][1] * x22;
-      double mx22 = m[0][2] * x02 + m[1][2] * x12 + m[2][2] * x22;
+      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
-      o[0][0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m[0][0]);
-      o[0][1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m[0][1]);
-      o[0][2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m[0][2]);
-      o[1][0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m[1][0]);
-      o[1][1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m[1][1]);
-      o[1][2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m[1][2]);
-      o[2][0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m[2][0]);
-      o[2][1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m[2][1]);
-      o[2][2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m[2][2]);
+      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 = o[0][0] - m[0][0];
-      double corr01 = o[0][1] - m[0][1];
-      double corr02 = o[0][2] - m[0][2];
-      double corr10 = o[1][0] - m[1][0];
-      double corr11 = o[1][1] - m[1][1];
-      double corr12 = o[1][2] - m[1][2];
-      double corr20 = o[2][0] - m[2][0];
-      double corr21 = o[2][1] - m[2][1];
-      double corr22 = o[2][2] - m[2][2];
+      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;
+          + corr10 * corr10 + corr11 * corr11 + corr12 * corr12
+          + corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
 
       // convergence test
       if (Math.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];
+      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;
 
     }



Mime
View raw message