diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
index 7715ca6d23b1e0a40164d3bd1c86e4a1a910afae..29ebaf320ec590128183b77a421bf5edae261217 100644
--- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
+++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
@@ -526,7 +526,7 @@ public class SensorMeanPlaneCrossing {
* @exception RuggedException if geometry cannot be computed for some line or
* if the maximum number of evaluations is exceeded
*/
- public CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess)
+ private CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess)
throws RuggedException {
try {
diff --git a/src/main/java/org/orekit/rugged/los/FixedRotation.java b/src/main/java/org/orekit/rugged/los/FixedRotation.java
index 3638aee9e2042fb887c9c2ecd2909b5854d21f34..00983195cf1db6f5624225d36bf09b7c4b78c38a 100644
--- a/src/main/java/org/orekit/rugged/los/FixedRotation.java
+++ b/src/main/java/org/orekit/rugged/los/FixedRotation.java
@@ -74,7 +74,7 @@ public class FixedRotation implements TimeIndependentLOSTransform {
angleDriver.addObserver(new ParameterObserver() {
@Override
public void valueChanged(final double previousValue, final ParameterDriver driver) {
- // reset rotations to null, they will evaluated lazily if needed
+ // reset rotations to null, they will be evaluated lazily if needed
rotation = null;
rDS = null;
}
diff --git a/src/main/java/org/orekit/rugged/los/PolynomialRotation.java b/src/main/java/org/orekit/rugged/los/PolynomialRotation.java
index 158c6dce5f2d876563bbc612a4209664828962c3..e3224400e285c8bc4d66681a311b3f8a636896ab 100644
--- a/src/main/java/org/orekit/rugged/los/PolynomialRotation.java
+++ b/src/main/java/org/orekit/rugged/los/PolynomialRotation.java
@@ -88,7 +88,7 @@ public class PolynomialRotation implements LOSTransform {
final ParameterObserver resettingObserver = new ParameterObserver() {
@Override
public void valueChanged(final double previousValue, final ParameterDriver driver) {
- // reset rotations to null, they will evaluated lazily if needed
+ // reset rotations to null, they will be evaluated lazily if needed
angle = null;
axisDS = null;
angleDS = null;
@@ -97,7 +97,7 @@ public class PolynomialRotation implements LOSTransform {
try {
for (int i = 0; i < angleCoeffs.length; ++i) {
coefficientsDrivers[i] = new ParameterDriver(name + "[" + i + "]", angleCoeffs[i], SCALE,
- Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
+ Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
coefficientsDrivers[i].addObserver(resettingObserver);
}
} catch (OrekitException oe) {
diff --git a/src/test/java/org/orekit/rugged/linesensor/FixedRotationtest.java b/src/test/java/org/orekit/rugged/linesensor/FixedRotationtest.java
new file mode 100644
index 0000000000000000000000000000000000000000..cf2ee7bb91b9f5551d194d3d889aa93d158b6706
--- /dev/null
+++ b/src/test/java/org/orekit/rugged/linesensor/FixedRotationtest.java
@@ -0,0 +1,247 @@
+/* Copyright 2013-2016 CS Systèmes d'Information
+ * Licensed to CS Systèmes d'Information (CS) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * CS licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.rugged.linesensor;
+
+import java.net.URISyntaxException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+
+import org.hipparchus.analysis.UnivariateMatrixFunction;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
+import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
+import org.hipparchus.analysis.differentiation.UnivariateDifferentiableMatrixFunction;
+import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
+import org.hipparchus.geometry.euclidean.threed.Rotation;
+import org.hipparchus.geometry.euclidean.threed.RotationConvention;
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
+import org.hipparchus.random.UniformRandomGenerator;
+import org.hipparchus.random.Well19937a;
+import org.hipparchus.util.FastMath;
+import org.junit.Assert;
+import org.junit.Before;
+import org.junit.Test;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.los.FixedRotation;
+import org.orekit.rugged.los.LOSBuilder;
+import org.orekit.rugged.los.TimeDependentLOS;
+import org.orekit.rugged.utils.DSGenerator;
+import org.orekit.time.AbsoluteDate;
+import org.orekit.utils.ParameterDriver;
+
+public class FixedRotationtest {
+
+ private List<Vector3D> raw;
+
+ @Test
+ public void testIdentity() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xaba71348a77d77cbl));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+ LOSBuilder builder = new LOSBuilder(raw);
+ builder.addTransform(new FixedRotation("identity",
+ new Vector3D(rvg.nextVector()),
+ 0.0));
+ TimeDependentLOS tdl = builder.build();
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(raw.get(i), tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ Assert.assertEquals(1, tdl.getParametersDrivers().count());
+ Assert.assertEquals("identity", tdl.getParametersDrivers().findFirst().get().getName());
+
+ }
+ }
+
+ @Test
+ public void testCombination() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+
+ LOSBuilder builder = new LOSBuilder(raw);
+
+ Vector3D axis1 = new Vector3D(rvg.nextVector());
+ double angle1 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new FixedRotation("r1", axis1, angle1));
+ Rotation r1 = new Rotation(axis1, angle1, RotationConvention.VECTOR_OPERATOR);
+
+ Vector3D axis2 = new Vector3D(rvg.nextVector());
+ double angle2 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new FixedRotation("r2", axis2, angle2));
+ Rotation r2 = new Rotation(axis2, angle2, RotationConvention.VECTOR_OPERATOR);
+
+ Vector3D axis3 = new Vector3D(rvg.nextVector());
+ double angle3 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new FixedRotation("r3", axis3, angle3));
+ Rotation r3 = new Rotation(axis3, angle3, RotationConvention.VECTOR_OPERATOR);
+
+ TimeDependentLOS tdl = builder.build();
+ Rotation combined = r3.applyTo(r2.applyTo(r1));
+
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(combined.applyTo(raw.get(i)),
+ tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ List<ParameterDriver> drivers = tdl.getParametersDrivers().collect(Collectors.toList());
+ Assert.assertEquals(3, drivers.size());
+ ParameterDriver driver1 = drivers.get(0);
+ ParameterDriver driver2 = drivers.get(1);
+ ParameterDriver driver3 = drivers.get(2);
+ Assert.assertEquals("r1", driver1.getName());
+ Assert.assertEquals(-2 * FastMath.PI, driver1.getMinValue(), 2.0e-15);
+ Assert.assertEquals(+2 * FastMath.PI, driver1.getMaxValue(), 2.0e-15);
+ Assert.assertEquals(angle1, driver1.getValue(), 2.0e-15);
+ Assert.assertEquals("r2", driver2.getName());
+ Assert.assertEquals(-2 * FastMath.PI, driver2.getMinValue(), 2.0e-15);
+ Assert.assertEquals(+2 * FastMath.PI, driver2.getMaxValue(), 2.0e-15);
+ Assert.assertEquals(angle2, driver2.getValue(), 2.0e-15);
+ Assert.assertEquals("r3", driver3.getName());
+ Assert.assertEquals(-2 * FastMath.PI, driver3.getMinValue(), 2.0e-15);
+ Assert.assertEquals(+2 * FastMath.PI, driver3.getMaxValue(), 2.0e-15);
+ Assert.assertEquals(angle3, driver3.getValue(), 2.0e-15);
+
+ driver1.setValue(0.0);
+ driver2.setValue(0.0);
+ driver3.setValue(0.0);
+
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(raw.get(i),
+ tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ }
+ }
+
+ @Test
+ public void testDerivatives() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xddae2b46b2207e08l));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+
+ LOSBuilder builder = new LOSBuilder(raw);
+
+ builder.addTransform(new FixedRotation("r1",
+ new Vector3D(rvg.nextVector()),
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
+ builder.addTransform(new FixedRotation("r2",
+ new Vector3D(rvg.nextVector()),
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
+ builder.addTransform(new FixedRotation("r3",
+ new Vector3D(rvg.nextVector()),
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
+ TimeDependentLOS tdl = builder.build();
+ final List<ParameterDriver> selected = tdl.getParametersDrivers().collect(Collectors.toList());
+ for (final ParameterDriver driver : selected) {
+ driver.setSelected(true);
+ }
+ DSGenerator generator = new DSGenerator() {
+
+ /** {@inheritDoc} */
+ @Override
+ public List<ParameterDriver> getSelected() {
+ return selected;
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure constant(final double value) {
+ return new DerivativeStructure(selected.size(), 1, value);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure variable(final ParameterDriver driver) {
+ int index = 0;
+ for (ParameterDriver d : getSelected()) {
+ if (d == driver) {
+ return new DerivativeStructure(getSelected().size(), 1, index, driver.getValue());
+ }
+ ++index;
+ }
+ return constant(driver.getValue());
+ }
+
+ };
+ Assert.assertEquals(3, generator.getSelected().size());
+
+ FiniteDifferencesDifferentiator differentiator =
+ new FiniteDifferencesDifferentiator(4, 0.001);
+ int index = 0;
+ for (final ParameterDriver driver : selected) {
+ int[] orders = new int[selected.size()];
+ orders[index] = 1;
+ UnivariateDifferentiableMatrixFunction f =
+ differentiator.differentiate((UnivariateMatrixFunction) x -> {
+ try {
+ double oldX = driver.getValue();
+ double[][] matrix = new double[raw.size()][];
+ driver.setValue(x);
+ for (int i = 0 ; i < raw.size(); ++i) {
+ matrix[i] = tdl.getLOS(i, AbsoluteDate.J2000_EPOCH).toArray();
+ }
+ driver.setValue(oldX);
+ return matrix;
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ });
+ DerivativeStructure[][] mDS = f.value(new DerivativeStructure(1, 1, 0, driver.getValue()));
+ for (int i = 0; i < raw.size(); ++i) {
+ Vector3D los = tdl.getLOS(i, AbsoluteDate.J2000_EPOCH);
+ FieldVector3D<DerivativeStructure> losDS =
+ tdl.getLOSDerivatives(i, AbsoluteDate.J2000_EPOCH, generator);
+ Assert.assertEquals(los.getX(), losDS.getX().getValue(), 2.0e-15);
+ Assert.assertEquals(los.getY(), losDS.getY().getValue(), 2.0e-15);
+ Assert.assertEquals(los.getZ(), losDS.getZ().getValue(), 2.0e-15);
+ Assert.assertEquals(mDS[i][0].getPartialDerivative(1), losDS.getX().getPartialDerivative(orders), 2.0e-12);
+ Assert.assertEquals(mDS[i][1].getPartialDerivative(1), losDS.getY().getPartialDerivative(orders), 2.0e-12);
+ Assert.assertEquals(mDS[i][2].getPartialDerivative(1), losDS.getZ().getPartialDerivative(orders), 2.0e-12);
+ }
+ ++index;
+ }
+ }
+
+ }
+
+ @Before
+ public void setUp() throws OrekitException, URISyntaxException {
+
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ raw = new ArrayList<Vector3D>();
+ for (int i = -100; i <= 100; ++i) {
+ final double alpha = i * 0.17 / 1000;
+ raw.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
+ }
+
+ }
+
+}
diff --git a/src/test/java/org/orekit/rugged/linesensor/PolynomialRotationtest.java b/src/test/java/org/orekit/rugged/linesensor/PolynomialRotationtest.java
new file mode 100644
index 0000000000000000000000000000000000000000..e878bacef1699fc2955d0cb6c926238141b763c5
--- /dev/null
+++ b/src/test/java/org/orekit/rugged/linesensor/PolynomialRotationtest.java
@@ -0,0 +1,263 @@
+/* Copyright 2013-2016 CS Systèmes d'Information
+ * Licensed to CS Systèmes d'Information (CS) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * CS licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.rugged.linesensor;
+
+import java.net.URISyntaxException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+
+import org.hipparchus.analysis.UnivariateMatrixFunction;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
+import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
+import org.hipparchus.analysis.differentiation.UnivariateDifferentiableMatrixFunction;
+import org.hipparchus.analysis.polynomials.PolynomialFunction;
+import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
+import org.hipparchus.geometry.euclidean.threed.Rotation;
+import org.hipparchus.geometry.euclidean.threed.RotationConvention;
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
+import org.hipparchus.random.UniformRandomGenerator;
+import org.hipparchus.random.Well19937a;
+import org.hipparchus.util.FastMath;
+import org.junit.Assert;
+import org.junit.Before;
+import org.junit.Test;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.los.PolynomialRotation;
+import org.orekit.rugged.los.LOSBuilder;
+import org.orekit.rugged.los.TimeDependentLOS;
+import org.orekit.rugged.utils.DSGenerator;
+import org.orekit.time.AbsoluteDate;
+import org.orekit.utils.ParameterDriver;
+
+public class PolynomialRotationtest {
+
+ private List<Vector3D> raw;
+
+ @Test
+ public void testIdentity() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xbe0d9b530fe7f53cl));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+ LOSBuilder builder = new LOSBuilder(raw);
+ builder.addTransform(new PolynomialRotation("identity",
+ new Vector3D(rvg.nextVector()),
+ AbsoluteDate.J2000_EPOCH, 0.0));
+ TimeDependentLOS tdl = builder.build();
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(raw.get(i), tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ Assert.assertEquals(1, tdl.getParametersDrivers().count());
+ Assert.assertEquals("identity[0]", tdl.getParametersDrivers().findFirst().get().getName());
+
+ }
+ }
+
+ @Test
+ public void testFixedCombination() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xdc4cfdea38edd2bbl));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+
+ LOSBuilder builder = new LOSBuilder(raw);
+
+ Vector3D axis1 = new Vector3D(rvg.nextVector());
+ double angle1 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new PolynomialRotation("r1", axis1, AbsoluteDate.J2000_EPOCH, angle1));
+ Rotation r1 = new Rotation(axis1, angle1, RotationConvention.VECTOR_OPERATOR);
+
+ Vector3D axis2 = new Vector3D(rvg.nextVector());
+ double angle2 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new PolynomialRotation("r2", axis2, AbsoluteDate.J2000_EPOCH, angle2));
+ Rotation r2 = new Rotation(axis2, angle2, RotationConvention.VECTOR_OPERATOR);
+
+ Vector3D axis3 = new Vector3D(rvg.nextVector());
+ double angle3 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
+ builder.addTransform(new PolynomialRotation("r3", axis3, AbsoluteDate.J2000_EPOCH, angle3));
+ Rotation r3 = new Rotation(axis3, angle3, RotationConvention.VECTOR_OPERATOR);
+
+ TimeDependentLOS tdl = builder.build();
+ Rotation combined = r3.applyTo(r2.applyTo(r1));
+
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(combined.applyTo(raw.get(i)),
+ tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ List<ParameterDriver> drivers = tdl.getParametersDrivers().collect(Collectors.toList());
+ Assert.assertEquals(3, drivers.size());
+ ParameterDriver driver1 = drivers.get(0);
+ ParameterDriver driver2 = drivers.get(1);
+ ParameterDriver driver3 = drivers.get(2);
+ Assert.assertEquals("r1[0]", driver1.getName());
+ Assert.assertTrue(Double.isInfinite(driver1.getMinValue()));
+ Assert.assertTrue(driver1.getMinValue() < 0);
+ Assert.assertTrue(Double.isInfinite(driver1.getMaxValue()));
+ Assert.assertTrue(driver1.getMaxValue() > 0);
+ Assert.assertEquals(angle1, driver1.getValue(), 2.0e-15);
+ Assert.assertEquals("r2[0]", driver2.getName());
+ Assert.assertTrue(Double.isInfinite(driver2.getMinValue()));
+ Assert.assertTrue(driver2.getMinValue() < 0);
+ Assert.assertTrue(Double.isInfinite(driver2.getMaxValue()));
+ Assert.assertTrue(driver2.getMaxValue() > 0);
+ Assert.assertEquals(angle2, driver2.getValue(), 2.0e-15);
+ Assert.assertEquals("r3[0]", driver3.getName());
+ Assert.assertTrue(Double.isInfinite(driver3.getMinValue()));
+ Assert.assertTrue(driver3.getMinValue() < 0);
+ Assert.assertTrue(Double.isInfinite(driver3.getMaxValue()));
+ Assert.assertTrue(driver3.getMaxValue() > 0);
+ Assert.assertEquals(angle3, driver3.getValue(), 2.0e-15);
+
+ driver1.setValue(0.0);
+ driver2.setValue(0.0);
+ driver3.setValue(0.0);
+
+ for (int i = 0; i < raw.size(); ++i) {
+ Assert.assertEquals(0.0,
+ Vector3D.distance(raw.get(i),
+ tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
+ 2.0e-15);
+ }
+
+ }
+ }
+
+ @Test
+ public void testDerivatives() throws RuggedException, OrekitException {
+ UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xc60acfc04eb27935l));
+ UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
+ for (int k = 0; k < 20; ++k) {
+
+ LOSBuilder builder = new LOSBuilder(raw);
+
+ builder.addTransform(new PolynomialRotation("r1",
+ new Vector3D(rvg.nextVector()),
+ AbsoluteDate.J2000_EPOCH,
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
+ 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
+ builder.addTransform(new PolynomialRotation("r2",
+ new Vector3D(rvg.nextVector()),
+ AbsoluteDate.J2000_EPOCH,
+ new PolynomialFunction(new double[] {
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
+ 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
+ 1.0e-8 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)
+ })));
+ builder.addTransform(new PolynomialRotation("r3",
+ new Vector3D(rvg.nextVector()),
+ AbsoluteDate.J2000_EPOCH,
+ 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
+ 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
+ TimeDependentLOS tdl = builder.build();
+ final List<ParameterDriver> selected = tdl.getParametersDrivers().collect(Collectors.toList());
+ for (final ParameterDriver driver : selected) {
+ driver.setSelected(true);
+ }
+ DSGenerator generator = new DSGenerator() {
+
+ /** {@inheritDoc} */
+ @Override
+ public List<ParameterDriver> getSelected() {
+ return selected;
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure constant(final double value) {
+ return new DerivativeStructure(selected.size(), 1, value);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure variable(final ParameterDriver driver) {
+ int index = 0;
+ for (ParameterDriver d : getSelected()) {
+ if (d == driver) {
+ return new DerivativeStructure(getSelected().size(), 1, index, driver.getValue());
+ }
+ ++index;
+ }
+ return constant(driver.getValue());
+ }
+
+ };
+ Assert.assertEquals(7, generator.getSelected().size());
+
+ FiniteDifferencesDifferentiator differentiator =
+ new FiniteDifferencesDifferentiator(4, 0.0001);
+ int index = 0;
+ final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(7.0);
+ for (final ParameterDriver driver : selected) {
+ int[] orders = new int[selected.size()];
+ orders[index] = 1;
+ UnivariateDifferentiableMatrixFunction f =
+ differentiator.differentiate((UnivariateMatrixFunction) x -> {
+ try {
+ double oldX = driver.getValue();
+ double[][] matrix = new double[raw.size()][];
+ driver.setValue(x);
+ for (int i = 0 ; i < raw.size(); ++i) {
+ matrix[i] = tdl.getLOS(i, date).toArray();
+ }
+ driver.setValue(oldX);
+ return matrix;
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ });
+ DerivativeStructure[][] mDS = f.value(new DerivativeStructure(1, 1, 0, driver.getValue()));
+ for (int i = 0; i < raw.size(); ++i) {
+ Vector3D los = tdl.getLOS(i, date);
+ FieldVector3D<DerivativeStructure> losDS = tdl.getLOSDerivatives(i, date, generator);
+ Assert.assertEquals(los.getX(), losDS.getX().getValue(), 2.0e-15);
+ Assert.assertEquals(los.getY(), losDS.getY().getValue(), 2.0e-15);
+ Assert.assertEquals(los.getZ(), losDS.getZ().getValue(), 2.0e-15);
+ Assert.assertEquals(mDS[i][0].getPartialDerivative(1), losDS.getX().getPartialDerivative(orders), 2.0e-10);
+ Assert.assertEquals(mDS[i][1].getPartialDerivative(1), losDS.getY().getPartialDerivative(orders), 2.0e-10);
+ Assert.assertEquals(mDS[i][2].getPartialDerivative(1), losDS.getZ().getPartialDerivative(orders), 2.0e-10);
+ }
+ ++index;
+ }
+ }
+
+ }
+
+ @Before
+ public void setUp() throws OrekitException, URISyntaxException {
+
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ raw = new ArrayList<Vector3D>();
+ for (int i = -100; i <= 100; ++i) {
+ final double alpha = i * 0.17 / 1000;
+ raw.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
+ }
+
+ }
+
+}
diff --git a/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java b/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
index 3f5125b088e8dcd22149a4a8f0d947d110b26ced..18043694437be6c7b7b5a023a1d369b18c141d5c 100644
--- a/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
+++ b/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
@@ -22,6 +22,8 @@ import org.hipparchus.random.RandomGenerator;
import org.hipparchus.random.Well19937a;
import org.hipparchus.util.FastMath;
import java.io.File;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
import java.net.URISyntaxException;
import java.util.ArrayList;
import java.util.List;
@@ -47,6 +49,7 @@ import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.KeplerianPropagator;
import org.orekit.propagation.sampling.OrekitFixedStepHandler;
import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.linesensor.SensorMeanPlaneCrossing.CrossingResult;
import org.orekit.rugged.los.LOSBuilder;
import org.orekit.rugged.utils.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
@@ -54,6 +57,7 @@ import org.orekit.utils.AngularDerivativesFilter;
import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.Constants;
import org.orekit.utils.IERSConventions;
+import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
@@ -216,6 +220,72 @@ public class SensorMeanPlaneCrossingTest {
}
+ @Test
+ public void testSlowFind()
+ throws RuggedException, OrekitException, NoSuchMethodException,
+ SecurityException, IllegalAccessException, IllegalArgumentException,
+ InvocationTargetException {
+
+ final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ final List<Vector3D> los = new ArrayList<Vector3D>();
+ for (int i = -1000; i <= 1000; ++i) {
+ final double alpha = i * 0.17 / 1000;
+ los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
+ }
+
+ final LineSensor sensor = new LineSensor("perfect line",
+ new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
+ position, new LOSBuilder(los).build());
+
+ Assert.assertEquals("perfect line", sensor.getName());
+ Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
+ Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15);
+
+ SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
+ 0, 2000, true, true, 50, 1.0e-6);
+
+ double refLine = 1200.0;
+ AbsoluteDate refDate = sensor.getDate(refLine);
+ int refPixel= 1800;
+ Transform b2i = mean.getScToBody().getBodyToInertial(refDate);
+ Transform sc2i = mean.getScToBody().getScToInertial(refDate);
+ Transform sc2b = new Transform(refDate, sc2i, b2i.getInverse());
+ Vector3D p1 = sc2b.transformPosition(position);
+ Vector3D p2 = sc2b.transformPosition(new Vector3D(1, position,
+ 1.0e6, los.get(refPixel)));
+ Line line = new Line(p1, p2, 0.001);
+ BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
+ Constants.WGS84_EARTH_FLATTENING,
+ mean.getScToBody().getBodyFrame());
+ GeodeticPoint groundPoint = earth.getIntersectionPoint(line, p1, mean.getScToBody().getBodyFrame(), refDate);
+ Vector3D gpCartesian = earth.transform(groundPoint);
+ SensorMeanPlaneCrossing.CrossingResult result = mean.find(gpCartesian);
+
+ Method slowFind = SensorMeanPlaneCrossing.class.getDeclaredMethod("slowFind",
+ PVCoordinates.class,
+ Double.TYPE);
+ slowFind.setAccessible(true);
+ SensorMeanPlaneCrossing.CrossingResult slowResult =
+ (CrossingResult) slowFind.invoke(mean,
+ new PVCoordinates(gpCartesian, Vector3D.ZERO),
+ 400.0);
+
+ Assert.assertEquals(result.getLine(), slowResult.getLine(), 2.0e-8);
+ Assert.assertEquals(0.0,
+ Vector3D.distance(result.getTargetDirection(),
+ slowResult.getTargetDirection()),
+ 2.0e-13);
+ Assert.assertEquals(0.0,
+ Vector3D.distance(result.getTargetDirectionDerivative(),
+ slowResult.getTargetDirectionDerivative()),
+ 1.0e-15);
+ }
+
private SpacecraftToObservedBody createInterpolator(LineSensor sensor)
throws RuggedException, OrekitException {
Orbit orbit = new CircularOrbit(7173352.811913891,