diff --git a/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java b/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java index 617b940e3..0792a86c9 100644 --- a/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java +++ b/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java @@ -847,6 +847,25 @@ public class Rotation implements Serializable { } + /** Apply the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to (it can be the same + * array as in) + */ + public void applyTo(final double[] in, final double[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final double s = q1 * x + q2 * y + q3 * z; + + out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; + out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y; + out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z; + + } + /** Apply the inverse of the rotation to a vector. * @param u vector to apply the inverse of the rotation to * @return a new vector which such that u is its image by the rotation @@ -866,6 +885,26 @@ public class Rotation implements Serializable { } + /** Apply the inverse of the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to (it can be the same + * array as in) + */ + public void applyInverseTo(final double[] in, final double[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final double s = q1 * x + q2 * y + q3 * z; + final double m0 = -q0; + + out[0] = 2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x; + out[1] = 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y; + out[2] = 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z; + + } + /** Apply the instance to another rotation. * Applying the instance to a rotation is computing the composition * in an order compliant with the following rule : let u be any diff --git a/src/site/xdoc/changes.xml b/src/site/xdoc/changes.xml index 61fb88757..f1ce30a23 100644 --- a/src/site/xdoc/changes.xml +++ b/src/site/xdoc/changes.xml @@ -52,6 +52,10 @@ The type attribute can be add,update,fix,remove. If the output is not quite correct, check for invisible trailing spaces! --> + + Added applyTo and applyInverseTo methods in the Rotation class that + handle directly arrays instead of Vector3D instances. + Added adapters for simple bound constraints optimization that can be used for all direct optimization methods, including the ones that do not diff --git a/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java b/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java index e54d4846f..6666f5a55 100644 --- a/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java +++ b/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java @@ -438,6 +438,31 @@ public class RotationTest { } + @Test + public void testArray() { + + Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7); + + for (double x = -0.9; x < 0.9; x += 0.2) { + for (double y = -0.9; y < 0.9; y += 0.2) { + for (double z = -0.9; z < 0.9; z += 0.2) { + Vector3D u = new Vector3D(x, y, z); + Vector3D v = r.applyTo(u); + double[] inOut = new double[] { x, y, z }; + r.applyTo(inOut, inOut); + Assert.assertEquals(v.getX(), inOut[0], 1.0e-10); + Assert.assertEquals(v.getY(), inOut[1], 1.0e-10); + Assert.assertEquals(v.getZ(), inOut[2], 1.0e-10); + r.applyInverseTo(inOut, inOut); + Assert.assertEquals(u.getX(), inOut[0], 1.0e-10); + Assert.assertEquals(u.getY(), inOut[1], 1.0e-10); + Assert.assertEquals(u.getZ(), inOut[2], 1.0e-10); + } + } + } + + } + @Test public void testApplyInverseTo() {