SwitchEventHandler.java
/* Copyright 2022-2025 Romain Serra
* Licensed to CS GROUP (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.propagation.numerical;
import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.analysis.differentiation.GradientField;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.ode.events.Action;
import org.hipparchus.util.FastMath;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.frames.Frame;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.EventDetector;
import org.orekit.propagation.events.FieldEventDetector;
import org.orekit.propagation.events.handlers.EventHandler;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.AbsolutePVCoordinates;
import org.orekit.utils.DataDictionary;
import org.orekit.utils.DerivativeStateUtils;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import java.util.Arrays;
import java.util.List;
/**
* Class handling the State Transition Matrix update in the presence of dynamics discontinuities.
* Reference: Eq. (29-30) in Russell, R. P., “Primer Vector Theory Applied to Global Low-Thrust Trade Studies,”
* Journal of Guidance, Control, and Dynamics, Vol. 30, No. 2, 2007, pp. 460-472.
*
* @author Romain Serra
* @since 13.1
* @see NumericalPropagator
*/
class SwitchEventHandler implements EventHandler {
/** Original event handler to wrap. */
private final EventHandler wrappedHandler;
/** Matrices harvester. */
private final NumericalPropagationHarvester matricesHarvester;
/** Differential equations. */
private final NumericalTimeDerivativesEquations timeDerivativesEquations;
/** Attitude provider. */
private final AttitudeProvider attitudeProvider;
/** Event detectors in Taylor differential algebra. */
private final List<FieldEventDetector<Gradient>> fieldDetectors;
/** Flag on propagation direction. */
private boolean isForward;
/** Field version of detector defining dynamics switch. */
private FieldEventDetector<Gradient> switchFieldDetector;
/**
* Constructor.
* @param wrappedHandler original event handler
* @param matricesHarvester matrices harvester
* @param timeDerivativesEquations differential equations
* @param attitudeProvider attitude provider
* @param fieldDetectors non-empty event detectors in Taylor differential algebra
*/
SwitchEventHandler(final EventHandler wrappedHandler, final NumericalPropagationHarvester matricesHarvester,
final NumericalTimeDerivativesEquations timeDerivativesEquations,
final AttitudeProvider attitudeProvider, final List<FieldEventDetector<Gradient>> fieldDetectors) {
this.wrappedHandler = wrappedHandler;
this.matricesHarvester = matricesHarvester;
this.timeDerivativesEquations = timeDerivativesEquations;
this.attitudeProvider = attitudeProvider;
this.fieldDetectors = fieldDetectors;
}
@Override
public SpacecraftState resetState(final EventDetector detector, final SpacecraftState oldState) {
if (switchFieldDetector == null) {
// failed to find corresponding Field detector
return wrappedHandler.resetState(detector, oldState);
} else {
final SpacecraftState newState = updateState(oldState);
switchFieldDetector = null;
return newState;
}
}
@Override
public void init(final SpacecraftState initialState, final AbsoluteDate target, final EventDetector detector) {
isForward = initialState.getDate().isBeforeOrEqualTo(target);
switchFieldDetector = null;
final GradientField field = fieldDetectors.get(0).getThreshold().getField();
final FieldAbsoluteDate<Gradient> fieldTarget = new FieldAbsoluteDate<>(field, target);
final FieldSpacecraftState<Gradient> fieldState = new FieldSpacecraftState<>(field, initialState);
fieldDetectors.forEach(fieldEventDetector -> fieldEventDetector.init(fieldState, fieldTarget));
EventHandler.super.init(initialState, target, detector);
}
@Override
public Action eventOccurred(final SpacecraftState s, final EventDetector detector, final boolean increasing) {
final Action action = wrappedHandler.eventOccurred(s, detector, increasing);
if (action == Action.RESET_DERIVATIVES) {
switchFieldDetector = findSwitchDetector(detector, s);
if (switchFieldDetector != null) {
return Action.RESET_STATE;
}
}
return action;
}
/**
* Method finding the Field detector corresponding to a non-Field, triggered one.
* @param detector triggered event detector
* @param state state
* @return Field detector
*/
private FieldEventDetector<Gradient> findSwitchDetector(final EventDetector detector, final SpacecraftState state) {
final GradientField field = fieldDetectors.get(0).getThreshold().getField();
final FieldSpacecraftState<Gradient> fieldState = new FieldSpacecraftState<>(field, state);
fieldDetectors.forEach(fieldDetector -> fieldDetector.reset(fieldState, fieldState.getDate()));
final double g = detector.g(state);
return findSwitchDetector(g, fieldState);
}
/**
* Method finding the Field detector corresponding to a non-Field, triggered one.
* @param g value of triggered detector
* @param fieldState Field state
* @return Field detector
*/
private FieldEventDetector<Gradient> findSwitchDetector(final double g,
final FieldSpacecraftState<Gradient> fieldState) {
for (final FieldEventDetector<Gradient> fieldDetector : fieldDetectors) {
final Gradient fieldG = fieldDetector.g(fieldState);
if (FastMath.abs(fieldG.getValue() - g) < 1e-11) {
return fieldDetector;
}
}
return null;
}
/**
* Update state, more precisely the additional data corresponding to the STM.
* @param stateAtSwitch original state
* @return updated state
*/
private SpacecraftState updateState(final SpacecraftState stateAtSwitch) {
final RealMatrix cartesianFactorMatrix = computeUpdateMatrix(stateAtSwitch);
// update STM
final String stmName = matricesHarvester.getStmName();
final RealMatrix oldStm = matricesHarvester.toSquareMatrix(stateAtSwitch.getAdditionalState(stmName));
final RealMatrix stm = cartesianFactorMatrix.multiply(oldStm).add(oldStm);
final DataDictionary additionalData = new DataDictionary(stateAtSwitch.getAdditionalDataValues());
additionalData.put(stmName, matricesHarvester.toArray(stm.getData()));
// update model parameters Jacobian if present
if (!matricesHarvester.getJacobiansColumnsNames().isEmpty()) {
for (final String parameterName : matricesHarvester.getJacobiansColumnsNames()) {
final RealVector oldJacobian = MatrixUtils.createRealVector(stateAtSwitch.getAdditionalState(parameterName));
final RealVector jacobian = cartesianFactorMatrix.operate(oldJacobian).add(oldJacobian);
additionalData.put(parameterName, jacobian.toArray());
}
}
return stateAtSwitch.withAdditionalData(additionalData);
}
/**
* Compute matrix needed to update STM and the like.
* @param state state
* @return matrix
*/
private RealMatrix computeUpdateMatrix(final SpacecraftState state) {
final double twoThreshold = switchFieldDetector.getThreshold().getValue() * 2.;
final double dt = isForward ? -twoThreshold : twoThreshold;
final SpacecraftState stateBefore = shift(state, dt);
final double[] derivativesBefore = timeDerivativesEquations.computeTimeDerivatives(stateBefore);
final SpacecraftState stateAfter = shift(state, -dt);
final double[] derivativesAfter = timeDerivativesEquations.computeTimeDerivatives(stateAfter);
final int stateDimension = matricesHarvester.getStateDimension();
final double[] deltaDerivatives = new double[stateDimension];
for (int i = 3; i < deltaDerivatives.length; i++) {
deltaDerivatives[i] = derivativesAfter[i] - derivativesBefore[i];
}
final Gradient g = evaluateG(buildCartesianState(state, state.getPVCoordinates()));
final double gLastDerivative = g.getPartialDerivative(stateDimension);
final double gDot = isForward ? gLastDerivative : -gLastDerivative;
final double[] gGradientState = Arrays.copyOfRange(g.getGradient(), 0, stateDimension);
final RealVector lhs = MatrixUtils.createRealVector(deltaDerivatives);
final RealVector rhs = MatrixUtils.createRealVector(gGradientState).mapMultiply(1. / gDot);
return lhs.outerProduct(rhs);
}
/**
* Shift state using first order Taylor expansion for position.
* @param stateAtSwitch state
* @param dt time shift
* @return new state
*/
private SpacecraftState shift(final SpacecraftState stateAtSwitch, final double dt) {
final PVCoordinates pvCoordinates = stateAtSwitch.getPVCoordinates();
final AbsoluteDate shiftedDate = stateAtSwitch.getDate().shiftedBy(dt);
final TimeStampedPVCoordinates shiftedPV = new TimeStampedPVCoordinates(shiftedDate,
pvCoordinates.getPosition().add(pvCoordinates.getVelocity().scalarMultiply(dt)), pvCoordinates.getVelocity());
return buildCartesianState(stateAtSwitch, shiftedPV);
}
/**
* Build state given template and new position-velocity vector.
* @param templateState template state
* @param pvCoordinates position-velocity vector
* @return new state
*/
private SpacecraftState buildCartesianState(final SpacecraftState templateState,
final TimeStampedPVCoordinates pvCoordinates) {
final SpacecraftState state;
final Frame frame = templateState.getFrame();
if (templateState.isOrbitDefined()) {
final Orbit templateOrbit = templateState.getOrbit();
final CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, frame, templateOrbit.getMu());
final Attitude attitude = attitudeProvider.getAttitude(orbit, pvCoordinates.getDate(), frame);
state = new SpacecraftState(orbit, attitude);
} else {
final AbsolutePVCoordinates absolutePVCoordinates = new AbsolutePVCoordinates(frame, pvCoordinates);
final Attitude attitude = attitudeProvider.getAttitude(absolutePVCoordinates,
pvCoordinates.getDate(), frame);
state = new SpacecraftState(absolutePVCoordinates, attitude);
}
return state.withMass(templateState.getMass())
.withAdditionalData(templateState.getAdditionalDataValues())
.withAdditionalStatesDerivatives(templateState.getAdditionalStatesDerivatives());
}
/**
* Evaluate event function in Taylor algebra.
* @param state state
* @return g in Taylor algebra
*/
private Gradient evaluateG(final SpacecraftState state) {
final int stateDimension = matricesHarvester.getStateDimension();
Gradient time = Gradient.variable(stateDimension + 1, stateDimension, 0);
if (!isForward) {
time = time.negate();
}
final GradientField field = time.getField();
final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateGradient(field,
state, attitudeProvider);
return switchFieldDetector.g(fieldState.shiftedBy(time));
}
}