diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java index 1707d7539c51f5dcd6b83d0200ed4775cf7c190e..12444ef8d3b1beaa5befbf3bdd292c547b858e40 100644 --- a/src/main/java/org/orekit/rugged/api/Rugged.java +++ b/src/main/java/org/orekit/rugged/api/Rugged.java @@ -453,6 +453,10 @@ 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 39f883a17412b03518b3fd957495c0ec80a663b2..529d0095bfbdb797404edb994431ae34c91c1aec 100644 --- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java +++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java @@ -496,6 +496,23 @@ 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 @@ -506,19 +523,6 @@ 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); @@ -528,39 +532,16 @@ public class SensorMeanPlaneCrossing { return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(), targetDirection); } catch (NoBracketingException nbe) { - 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); + 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); } 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