diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java index aa5a0f12c64f8bf3bc79b37b9ef33e95a80f13a3..c1e9530c20f04946b4237fb5233139fe7eb6bf17 100644 --- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java +++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java @@ -356,17 +356,36 @@ public class SensorMeanPlaneCrossing { // as we know the solution is improved in the second stage of inverse location. // We expect two or three evaluations only. Each new evaluation shows up quickly in // the performances as it involves frames conversions - final double[] searchHistory = new double[maxEval]; + final double[] crossingLineHistory = new double[maxEval]; + final DerivativeStructure[] betaHistory = new DerivativeStructure[maxEval]; boolean atMin = false; boolean atMax = false; for (int i = 0; i < maxEval; ++i) { - searchHistory[i] = crossingLine; + crossingLineHistory[i] = crossingLine; final FieldVector3D<DerivativeStructure> targetDirection = evaluateLine(crossingLine, targetPV, bodyToInert, scToInert); - final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal); + betaHistory[i] = FieldVector3D.angle(targetDirection, meanPlaneNormal); - final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1); + final double deltaL; + if (i == 0) { + // simple Newton iteration + deltaL = (0.5 * FastMath.PI - betaHistory[i].getValue()) / betaHistory[i].getPartialDerivative(1); + crossingLine += deltaL; + } else { + // inverse cubic iteration + final double a0 = betaHistory[i - 1].getValue() - 0.5 * FastMath.PI; + final double l0 = crossingLineHistory[i - 1]; + final double d0 = betaHistory[i - 1].getPartialDerivative(1); + final double a1 = betaHistory[i].getValue() - 0.5 * FastMath.PI; + final double l1 = crossingLineHistory[i]; + final double d1 = betaHistory[i].getPartialDerivative(1); + final double a1Ma0 = a1 - a0; + crossingLine = ((l0 * (a1 - 3 * a0) - a0 * a1Ma0 / d0) * a1 * a1 + + (l1 * (3 * a1 - a0) - a1 * a1Ma0 / d1) * a0 * a0 + ) / (a1Ma0 * a1Ma0 * a1Ma0); + deltaL = crossingLine - l1; + } if (FastMath.abs(deltaL) <= accuracy) { // return immediately, without doing any additional evaluation! for (int k = cachedResults.length - 1; k > 0; --k) { @@ -375,9 +394,8 @@ public class SensorMeanPlaneCrossing { cachedResults[0] = new CrossingResult(sensor.getDate(crossingLine), crossingLine, target, targetDirection); return cachedResults[0]; } - crossingLine += deltaL; for (int j = 0; j < i; ++j) { - if (FastMath.abs(crossingLine - searchHistory[j]) <= 1.0) { + if (FastMath.abs(crossingLine - crossingLineHistory[j]) <= 1.0) { // rare case: we are stuck in a loop! // switch to a more robust (but slower) algorithm in this case for (int k = cachedResults.length - 1; k > 0; --k) { diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java index 994b684571ebfef5ac9bffacb44d9997cf565baf..c134d1fe15d66b2b1ce749b0873f694f02036c89 100644 --- a/src/test/java/org/orekit/rugged/api/RuggedTest.java +++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java @@ -573,8 +573,8 @@ public class RuggedTest { sensors.add(new LineSensor("line-" + i, lineDatation, position, los)); } - int firstLine = -10 * dimension; - int lastLine = 100 * dimension; + int firstLine = 0; + int lastLine = dimension; AbsoluteDate minDate = sensors.get(0).getDate(firstLine).shiftedBy(-1.0); AbsoluteDate maxDate = sensors.get(sensors.size() - 1).getDate(lastLine).shiftedBy(+1.0); @@ -922,7 +922,7 @@ public class RuggedTest { GeodeticPoint[] gp = rugged.directLocation("curved", lineNumber); for (int i = 0; i < gp.length; ++i) { SensorPixel pixel = rugged.inverseLocation("curved", gp[i], firstLine, lastLine); - Assert.assertEquals(lineNumber, pixel.getLineNumber(), 1.5e-7); + Assert.assertEquals(lineNumber, pixel.getLineNumber(), 5.0e-4); Assert.assertEquals(i, pixel.getPixelNumber(), 3.1e-7); }