Merge branch 'release/0.7.0'

Author: Sylvain Bertrand

Diff for: build.gradle

@@ -8,12 +8,12 @@ repositories {
     }
 }
 
-apply plugin: 'java'
-apply plugin: 'maven-publish'
-apply plugin: 'com.jfrog.bintray'
+apply plugin: "java"
+apply plugin: "maven-publish"
+apply plugin: "com.jfrog.bintray"
 
 sourceCompatibility = 1.8
-version = '0.6.1'
+version = "0.7.0"
 group = "us.ihmc"
 
 project.ext.fullVersion = version
@@ -39,34 +39,34 @@ jar {
 }
 
 task sourceJar(type: Jar, dependsOn: classes) {
-   classifier = 'sources'
+   classifier = "sources"
    from sourceSets.main.allJava
 }
 
 task testJar(type: Jar, dependsOn: [classes, testClasses]) {
-   classifier = 'test'
+   classifier = "test"
    from sourceSets.test.output
 }
 
 // Task to generate javadocs into relevant website directory
 task genJavadocs(type: Javadoc, dependsOn: sourceJar) {
 	source = sourceSets.main.allJava
 	classpath = configurations.compile
-	destinationDir = file('websitedocs/website/static/javadocs')
+	destinationDir = file("websitedocs/website/static/javadocs")
 	setFailOnError(false)
 }
 
 //Ensure the replacestyles.sh script is run before the following task:
 
 // Task to rename the javadocs directory with the version number
 task javadocsVersion (dependsOn: genJavadocs) {
-	file('websitedocs/website/static/javadocs').renameTo(file('websitedocs/website/static/javadocs' + '-' + version))
+	file("websitedocs/website/static/javadocs").renameTo(file("websitedocs/website/static/javadocs" + "-" + version))
 }
 
 //Task to create a new version in docusaurus
 task docsVersion (type: Exec, dependsOn: javadocsVersion) {
-	workingDir 'websitedocs/website'
-	commandLine 'yarn', 'run', 'version', version
+	workingDir "websitedocs/website"
+	commandLine "yarn", "run", "version", version
 }
 
 publishing {
@@ -81,25 +81,25 @@ publishing {
              asNode().children().last() + {
                  resolveStrategy = Closure.DELEGATE_FIRST
                  name project.name
-                 description 'Euclid Core is library providing the common tools useful for geometric applications and vectorial math.'
+                 description "Euclid Core is library providing the common tools useful for geometric applications and vectorial math."
                  url project.ext.vcsUrl
                  licenses {
                      license {
                          name project.ext.licenseName
                          url project.ext.licenseURL
-                         distribution 'repo'
+                         distribution "repo"
                      }
                  }
                  developers {
                      developer {
-                         id 'sbertrand'
-                         name 'Sylvain Bertrand'
-                         email '[email protected]'
+                         id "sbertrand"
+                         name "Sylvain Bertrand"
+                         email "[email protected]"
                      }
                      developer {
-                         id 'dcalvert'
-                         name 'Duncan Calvert'
-                         email '[email protected]'
+                         id "dcalvert"
+                         name "Duncan Calvert"
+                         email "[email protected]"
                      }
                  }
              }
@@ -131,15 +131,15 @@ bintray {
    pkg {
       repo = "maven-release"
       name = project.ext.publicationName
-      userOrg = 'ihmcrobotics'
+      userOrg = "ihmcrobotics"
       desc = "IHMC Open Robotics Software Project ${project.name}"
 
       websiteUrl = project.ext.vcsUrl
       issueTrackerUrl = "${project.ext.vcsUrl}/issues"
       vcsUrl = "${project.ext.vcsUrl}.git"
 
       licenses = [project.ext.bintrayLicenseName]
-      labels = ['ihmc', 'java', 'geometry', 'vector', 'math']
+      labels = ["ihmc", "java", "geometry", "vector", "math"]
       publicDownloadNumbers = true
 
       version {
@@ -159,9 +159,8 @@ repositories {
 
 dependencies {
     testCompile group: "junit", name: "junit", version: "4.12"
-    testCompile group: 'org.pitest', name: 'pitest', version: '1.2.0'
-    testCompile group: 'org.pitest', name: 'pitest-command-line', version: '1.2.0'
-    testCompile group: 'us.ihmc', name: 'ihmc-continuous-integration-framework', version: '0.9.4'
+    testCompile group: "org.pitest", name: "pitest", version: "1.2.0"
+    testCompile group: "org.pitest", name: "pitest-command-line", version: "1.2.0"
 
-    compile group: 'org.ejml', name: 'dense64', version: '0.30'
+    compile group: "org.ejml", name: "dense64", version: "0.30"
 }

Diff for: src/us/ihmc/euclid/axisAngle/AxisAngle.java

@@ -199,14 +199,10 @@ public final double getAngle()
    @Override
    public boolean equals(Object object)
    {
-      try
-      {
-         return equals((AxisAngle) object);
-      }
-      catch (ClassCastException e)
-      {
+      if (object instanceof AxisAngleReadOnly)
+         return equals((AxisAngleReadOnly) object);
+      else
          return false;
-      }
    }
 
    /**

Diff for: src/us/ihmc/euclid/axisAngle/AxisAngle32.java

@@ -238,14 +238,10 @@ public final float getZ32()
    @Override
    public boolean equals(Object object)
    {
-      try
-      {
+      if (object instanceof AxisAngleReadOnly)
          return equals((AxisAngleReadOnly) object);
-      }
-      catch (ClassCastException e)
-      {
+      else
          return false;
-      }
    }
 
    /**

Diff for: src/us/ihmc/euclid/axisAngle/interfaces/AxisAngleReadOnly.java

@@ -16,6 +16,7 @@
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
 import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics;
 import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly;
+import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollBasics;
 
 /**
  * Read-only interface for an axis-angle object.
@@ -181,6 +182,13 @@ default void get(QuaternionBasics quaternionToPack)
       quaternionToPack.setAxisAngle(getX(), getY(), getZ(), getAngle());
    }
 
+   /** {@inheritDoc} */
+   @Override
+   default void get(YawPitchRollBasics yawPitchRollToPack)
+   {
+      yawPitchRollToPack.setAxisAngle(getX(), getY(), getZ(), getAngle());
+   }
+
    /** {@inheritDoc} */
    @Override
    default void getRotationVector(Vector3DBasics rotationVectorToPack)
@@ -189,6 +197,7 @@ default void getRotationVector(Vector3DBasics rotationVectorToPack)
    }
 
    /** {@inheritDoc} */
+   @Deprecated
    @Override
    default void getYawPitchRoll(double[] yawPitchRollToPack)
    {
@@ -405,14 +414,10 @@ default void inverseTransform(Matrix3DReadOnly matrixOriginal, Matrix3DBasics ma
     */
    default boolean equals(AxisAngleReadOnly other)
    {
-      try
-      {
-         return getX() == other.getX() && getY() == other.getY() && getZ() == other.getZ() && getAngle() == other.getAngle();
-      }
-      catch (NullPointerException e)
-      {
+      if (other == null)
          return false;
-      }
+      else
+         return getX() == other.getX() && getY() == other.getY() && getZ() == other.getZ() && getAngle() == other.getAngle();
    }
 
    /**

Diff for: src/us/ihmc/euclid/matrix/Matrix3D.java

@@ -231,14 +231,10 @@ public double getM22()
    @Override
    public boolean equals(Object object)
    {
-      try
-      {
+      if (object instanceof Matrix3DReadOnly)
          return equals((Matrix3DReadOnly) object);
-      }
-      catch (ClassCastException e)
-      {
+      else
          return false;
-      }
    }
 
    /**

Diff for: src/us/ihmc/euclid/matrix/RotationMatrix.java

@@ -151,13 +151,12 @@ public RotationMatrix(Orientation3DReadOnly orientation)
     * Creates a new rotation matrix representing the same orientation as the given rotation vector
     * {@code rotationVector}.
     * <p>
-    * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation.
-    * A rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle
-    * of the same axis-angle.
+    * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A
+    * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the
+    * same axis-angle.
     * </p>
     *
-    * @param rotationVector the rotation vector used to initialize this rotation matrix. Not
-    *           modified.
+    * @param rotationVector the rotation vector used to initialize this rotation matrix. Not modified.
     */
    public RotationMatrix(Vector3DReadOnly rotationVector)
    {
@@ -173,6 +172,18 @@ public void setToZero()
       setIdentity();
    }
 
+   /**
+    * {@inheritDoc}
+    * <p>
+    * Note that this rotation matrix becomes invalid and has to be updated before being usable again.
+    * </p>
+    */
+   @Override
+   public void setToNaN()
+   {
+      setUnsafe(Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN);
+   }
+
    /**
     * Orthonormalization of the rotation matrix using the
     * <a href="https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process"> Gram-Schmidt method</a>.
@@ -369,8 +380,7 @@ public void set(Orientation3DReadOnly orientation3DReadOnly)
    }
 
    /**
-    * Sets this rotation matrix to equal the 3D matrix {@code matrix} and then normalizes
-    * {@code this}.
+    * Sets this rotation matrix to equal the 3D matrix {@code matrix} and then normalizes {@code this}.
     *
     * @param matrix the matrix to copy the values from. Not modified.
     * @throws NotARotationMatrixException if the normalization failed.
@@ -485,8 +495,8 @@ public void setRotationMatrix(double m00, double m01, double m02, double m10, do
    }
 
    /**
-    * Sets this rotation matrix to represent a counter clockwise rotation around the z-axis of an
-    * angle {@code yaw}.
+    * Sets this rotation matrix to represent a counter clockwise rotation around the z-axis of an angle
+    * {@code yaw}.
     *
     * <pre>
     *        / cos(yaw) -sin(yaw) 0 \
@@ -502,8 +512,8 @@ public void setToYawMatrix(double yaw)
    }
 
    /**
-    * Sets this rotation matrix to represent a counter clockwise rotation around the y-axis of an
-    * angle {@code pitch}.
+    * Sets this rotation matrix to represent a counter clockwise rotation around the y-axis of an angle
+    * {@code pitch}.
     *
     * <pre>
     *        /  cos(pitch) 0 sin(pitch) \
@@ -519,8 +529,8 @@ public void setToPitchMatrix(double pitch)
    }
 
    /**
-    * Sets this rotation matrix to represent a counter clockwise rotation around the x-axis of an
-    * angle {@code roll}.
+    * Sets this rotation matrix to represent a counter clockwise rotation around the x-axis of an angle
+    * {@code roll}.
     *
     * <pre>
     *        / 1     0          0     \
@@ -791,9 +801,9 @@ public void prependRollRotation(double roll)
     * </p>
     *
     * @param rf the other rotation matrix used for the interpolation. Not modified.
-    * @param alpha the percentage used for the interpolation. A value of 0 will result in not
-    *           modifying this rotation matrix, while a value of 1 is equivalent to setting this
-    *           rotation matrix to {@code rf}.
+    * @param alpha the percentage used for the interpolation. A value of 0 will result in not modifying
+    *           this rotation matrix, while a value of 1 is equivalent to setting this rotation matrix
+    *           to {@code rf}.
     */
    public void interpolate(RotationMatrixReadOnly rf, double alpha)
    {
@@ -826,8 +836,8 @@ public void interpolate(RotationMatrixReadOnly r0, RotationMatrixReadOnly rf, do
     * {@code transform}.
     * </p>
     * <p>
-    * Note: the transformation of a {@code RotationMatrix} strongly differs from the transformation
-    * of a {@link Matrix3DBasics}.
+    * Note: the transformation of a {@code RotationMatrix} strongly differs from the transformation of
+    * a {@link Matrix3DBasics}.
     * </p>
     */
    @Override
@@ -839,12 +849,12 @@ public void applyTransform(Transform transform)
    /**
     * {@inheritDoc}
     * <p>
-    * this = R<sup>T</sup> * this where 'R' is the 3-by-3 matrix representing the rotation part of
-    * the {@code transform}.
+    * this = R<sup>T</sup> * this where 'R' is the 3-by-3 matrix representing the rotation part of the
+    * {@code transform}.
     * </p>
     * <p>
-    * Note: the transformation of a {@code RotationMatrix} strongly differs from the transformation
-    * of a {@link Matrix3DBasics}.
+    * Note: the transformation of a {@code RotationMatrix} strongly differs from the transformation of
+    * a {@link Matrix3DBasics}.
     * </p>
     */
    @Override
@@ -917,24 +927,20 @@ public double getM22()
    }
 
    /**
-    * Tests if the given {@code object}'s class is the same as this, in which case the method
-    * returns {@link #equals(Matrix3DReadOnly)}, it returns {@code false} otherwise or if the
-    * {@code object} is {@code null}.
+    * Tests if the given {@code object}'s class is the same as this, in which case the method returns
+    * {@link #equals(Matrix3DReadOnly)}, it returns {@code false} otherwise or if the {@code object} is
+    * {@code null}.
     *
     * @param object the object to compare against this. Not modified.
     * @return {@code true} if {@code object} and this are exactly equal, {@code false} otherwise.
     */
    @Override
    public boolean equals(Object object)
    {
-      try
-      {
+      if (object instanceof Matrix3DReadOnly)
          return equals((Matrix3DReadOnly) object);
-      }
-      catch (ClassCastException e)
-      {
+      else
          return false;
-      }
    }
 
    /**
@@ -954,8 +960,8 @@ public boolean epsilonEquals(RotationMatrix other, double epsilon)
    /**
     * Tests if {@code this} and {@code other} represent the same orientation to an {@code epsilon}.
     * <p>
-    * Two rotation matrices are considered geometrically equal if the magnitude of their difference
-    * is less than or equal to {@code epsilon}.
+    * Two rotation matrices are considered geometrically equal if the magnitude of their difference is
+    * less than or equal to {@code epsilon}.
     * </p>
     * <p>
     * Note that {@code this.geometricallyEquals(other, epsilon) == true} does not necessarily imply