diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java index 12444ef8d3b1beaa5befbf3bdd292c547b858e40..1707d7539c51f5dcd6b83d0200ed4775cf7c190e 100644 --- a/src/main/java/org/orekit/rugged/api/Rugged.java +++ b/src/main/java/org/orekit/rugged/api/Rugged.java @@ -453,10 +453,6 @@ public class Rugged { crossingResult.getTargetDirection().toVector3D(), MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY); final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate()); - if (Double.isNaN(coarsePixel)) { - // target is out of search interval - return null; - } // fix line by considering the closest pixel exact position and line-of-sight // (this pixel might point towards a direction slightly above or below the mean sensor plane) diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java index 529d0095bfbdb797404edb994431ae34c91c1aec..39f883a17412b03518b3fd957495c0ec80a663b2 100644 --- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java +++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java @@ -496,23 +496,6 @@ public class SensorMeanPlaneCrossing { public CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess) throws RuggedException { - // set up function evaluating to 0.0 where target matches line - final UnivariateFunction f = new UnivariateFunction() { - /** {@inheritDoc} */ - @Override - public double value(final double x) throws RuggedExceptionWrapper { - try { - final AbsoluteDate date = sensor.getDate(x); - final FieldVector3D<DerivativeStructure> targetDirection = - evaluateLine(x, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date)); - final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal); - return 0.5 * FastMath.PI - beta.getValue(); - } catch (RuggedException re) { - throw new RuggedExceptionWrapper(re); - } - } - }; - try { // safety check @@ -523,6 +506,19 @@ public class SensorMeanPlaneCrossing { startValue = initialGuess; } + // set up function evaluating to 0.0 where target matches line + final UnivariateFunction f = new UnivariateFunction() { + /** {@inheritDoc} */ + @Override + public double value(final double x) throws RuggedExceptionWrapper { + try { + return targetOffset(x, targetPV).getValue(); + } catch (RuggedException re) { + throw new RuggedExceptionWrapper(re); + } + } + }; + final UnivariateSolver solver = new BracketingNthOrderBrentSolver(accuracy, 5); final double crossingLine = solver.solve(maxEval, f, minLine, maxLine, startValue); @@ -532,16 +528,39 @@ public class SensorMeanPlaneCrossing { return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(), targetDirection); } catch (NoBracketingException nbe) { - final double fMinLine = f.value(minLine); - final double fMaxLine = f.value(maxLine); - final double expectedLine = (fMaxLine * minLine - fMinLine * maxLine) / (fMaxLine - fMinLine); - throw new InverseLocOutOfLineRangeException(expectedLine, minLine, maxLine); + final DerivativeStructure fMinLine = targetOffset(minLine, targetPV); + final DerivativeStructure fMaxLine = targetOffset(maxLine, targetPV); + final double linearExpectedLine = (fMaxLine.getValue() * minLine - fMinLine.getValue() * maxLine) / + (fMaxLine.getValue() - fMinLine.getValue()); + final double newtonExpectedLine; + if (linearExpectedLine < midLine) { + newtonExpectedLine = minLine - fMinLine.getPartialDerivative(1) / fMinLine.getValue(); + } else { + newtonExpectedLine = maxLine - fMaxLine.getPartialDerivative(1) / fMaxLine.getValue(); + } + throw new InverseLocOutOfLineRangeException(newtonExpectedLine, minLine, maxLine); } catch (RuggedExceptionWrapper rew) { throw rew.getException(); } } + /** Target offset. + * @param lineNumber current line number + * @param targetPV target ground point + * @return target direction in spacecraft frame, with its first derivative + * with respect to line number + * @exception RuggedException if geometry cannot be computed + */ + private DerivativeStructure targetOffset(final double lineNumber, final PVCoordinates targetPV) + throws RuggedException { + final AbsoluteDate date = sensor.getDate(lineNumber); + final FieldVector3D<DerivativeStructure> targetDirection = + evaluateLine(lineNumber, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date)); + final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal); + return beta.subtract(0.5 * FastMath.PI); + }; + /** Evaluate geometry for a given line number. * @param lineNumber current line number * @param targetPV target ground point