Skip to content
Snippets Groups Projects
Commit 81fbd617 authored by Luc Maisonobe's avatar Luc Maisonobe
Browse files

Attempt to find an escape solution in some inverse location cases.

parent a3ea007a
No related branches found
No related tags found
No related merge requests found
...@@ -308,17 +308,24 @@ public class SensorMeanPlaneCrossing { ...@@ -308,17 +308,24 @@ public class SensorMeanPlaneCrossing {
Transform scToInert = midScToInert; Transform scToInert = midScToInert;
boolean atMin = false; boolean atMin = false;
boolean atMax = false; boolean atMax = false;
double deltaL = Double.NaN;
for (int i = 0; i < maxEval; ++i) { for (int i = 0; i < maxEval; ++i) {
final FieldVector3D<DerivativeStructure> targetDirection = final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert); evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal); final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1); final double previousDeltaL = deltaL;
deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
if (FastMath.abs(deltaL) <= accuracy) { if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation! // return immediately, without doing any additional evaluation!
return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetDirection); return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetDirection);
} }
if (FastMath.abs(deltaL + previousDeltaL) <= 0.01 * FastMath.abs(deltaL)) {
// we are stuck in a loop between two values!
// try to escape from the loop by targeting the middle point
deltaL = 0.5 * deltaL;
}
crossingLine += deltaL; crossingLine += deltaL;
if (crossingLine < minLine) { if (crossingLine < minLine) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment