AlignedAndConstrained.java
/* Copyright 2002-2024 Luc Maisonobe
* 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.attitudes;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
import org.hipparchus.analysis.differentiation.UnivariateDerivative2Field;
import org.hipparchus.geometry.euclidean.threed.FieldRotation;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.orekit.annotation.DefaultDataContext;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.AngularCoordinates;
import org.orekit.utils.ExtendedPositionProvider;
import org.orekit.utils.FieldAngularCoordinates;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import java.util.HashMap;
import java.util.Map;
/**
* Attitude provider with one satellite vector aligned and another one constrained to two targets.
* @author Luc Maisonobe
* @since 12.2
*/
public class AlignedAndConstrained implements AttitudeProvider
{
/** Satellite vector for primary target. */
private final FieldVector3D<UnivariateDerivative2> primarySat;
/** Primary target. */
private final TargetProvider primaryTarget;
/** Satellite vector for secondary target. */
private final FieldVector3D<UnivariateDerivative2> secondarySat;
/** Secondary target. */
private final TargetProvider secondaryTarget;
/** Sun model. */
private final ExtendedPositionProvider sun;
/** Earth model. */
private final OneAxisEllipsoid earth;
/** Reference inertial frame. */
private final Frame inertialFrame;
/** Cached field-based satellite vectors. */
private final transient Map<Field<? extends CalculusFieldElement<?>>, Cache<? extends CalculusFieldElement<?>>>
cachedSatelliteVectors;
/**
* Simple constructor.
* @param primarySat satellite vector for primary target
* @param primaryTarget primary target
* @param secondarySat satellite vector for secondary target
* @param secondaryTarget secondary target
* @param inertialFrame reference inertial frame
* @param sun Sun model
* @param earth Earth model
* @since 13.0
*/
public AlignedAndConstrained(final Vector3D primarySat, final TargetProvider primaryTarget,
final Vector3D secondarySat, final TargetProvider secondaryTarget,
final Frame inertialFrame, final ExtendedPositionProvider sun,
final OneAxisEllipsoid earth) {
if (!inertialFrame.isPseudoInertial()) {
throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, inertialFrame.getName());
}
this.primarySat = new FieldVector3D<>(UnivariateDerivative2Field.getInstance(), primarySat);
this.primaryTarget = primaryTarget;
this.secondarySat = new FieldVector3D<>(UnivariateDerivative2Field.getInstance(), secondarySat);
this.secondaryTarget = secondaryTarget;
this.inertialFrame = inertialFrame;
this.sun = sun;
this.earth = earth;
this.cachedSatelliteVectors = new HashMap<>();
}
/**
* Constructor with default inertial frame.
* @param primarySat satellite vector for primary target
* @param primaryTarget primary target
* @param secondarySat satellite vector for secondary target
* @param secondaryTarget secondary target
* @param sun Sun model
* @param earth Earth model
*/
@DefaultDataContext
public AlignedAndConstrained(final Vector3D primarySat, final TargetProvider primaryTarget,
final Vector3D secondarySat, final TargetProvider secondaryTarget,
final ExtendedPositionProvider sun,
final OneAxisEllipsoid earth)
{
this(primarySat, primaryTarget, secondarySat, secondaryTarget, FramesFactory.getGCRF(), sun, earth);
}
/** {@inheritDoc} */
@Override
public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {
final TimeStampedPVCoordinates satPV = pvProv.getPVCoordinates(date, inertialFrame);
// compute targets references at the specified date
final Vector3D primaryDirection = primaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
final Vector3D secondaryDirection = secondaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
// compute transform from inertial frame to satellite frame
final Rotation rotation = new Rotation(primaryDirection, secondaryDirection, primarySat.toVector3D(),
secondarySat.toVector3D());
if (inertialFrame != frame) {
// prepend transform from specified frame to inertial frame
final Rotation prepended = frame.getStaticTransformTo(inertialFrame, date).getRotation();
return rotation.compose(prepended, RotationConvention.VECTOR_OPERATOR);
}
return rotation;
}
/** {@inheritDoc} */
@Override
public Attitude getAttitude(final PVCoordinatesProvider pvProv,
final AbsoluteDate date,
final Frame frame)
{
final TimeStampedPVCoordinates satPV = pvProv.getPVCoordinates(date, inertialFrame);
// compute targets references at the specified date
final FieldVector3D<UnivariateDerivative2> primaryDirection = primaryTarget.getDerivative2TargetDirection(sun,
earth, satPV, inertialFrame);
final FieldVector3D<UnivariateDerivative2> secondaryDirection = secondaryTarget.getDerivative2TargetDirection(sun,
earth, satPV, inertialFrame);
// compute transform from inertial frame to satellite frame
final FieldRotation<UnivariateDerivative2> inertToSatRotation =
new FieldRotation<>(primaryDirection, secondaryDirection, primarySat, secondarySat);
// build the angular coordinates
final AngularCoordinates angularCoordinates = new AngularCoordinates(inertToSatRotation);
final Attitude attitude = new Attitude(date, inertialFrame, angularCoordinates);
return attitude.withReferenceFrame(frame);
}
/** {@inheritDoc} */
@Override
public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date,
final Frame frame) {
final TimeStampedFieldPVCoordinates<T> satPV = pvProv.getPVCoordinates(date, inertialFrame);
// compute targets references at the specified date
final FieldVector3D<T> primaryDirection = primaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
final FieldVector3D<T> secondaryDirection = secondaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
// compute transform from inertial frame to satellite frame
final Field<T> field = date.getField();
final FieldRotation<T> rotation = new FieldRotation<>(primaryDirection, secondaryDirection,
new FieldVector3D<>(field, primarySat.toVector3D()), new FieldVector3D<>(field, secondarySat.toVector3D()));
if (inertialFrame != frame) {
// prepend transform from specified frame to inertial frame
final FieldRotation<T> prepended = frame.getStaticTransformTo(inertialFrame, date).getRotation();
return rotation.compose(prepended, RotationConvention.VECTOR_OPERATOR);
}
return rotation;
}
/** {@inheritDoc} */
@Override
public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date,
final Frame frame)
{
// get the satellite vectors for specified field
@SuppressWarnings("unchecked")
final Cache<T> satVectors =
(Cache<T>) cachedSatelliteVectors.computeIfAbsent(date.getField(),
f -> new Cache<>(date.getField(), primarySat, secondarySat));
final TimeStampedFieldPVCoordinates<T> satPV = pvProv.getPVCoordinates(date, inertialFrame);
// compute targets references at the specified date
final FieldVector3D<FieldUnivariateDerivative2<T>> primaryDirection = primaryTarget.getDerivative2TargetDirection(sun,
earth, satPV, inertialFrame);
final FieldVector3D<FieldUnivariateDerivative2<T>> secondaryDirection = secondaryTarget.getDerivative2TargetDirection(sun,
earth, satPV, inertialFrame);
// compute transform from inertial frame to satellite frame
final FieldRotation<FieldUnivariateDerivative2<T>> inertToSatRotation =
new FieldRotation<>(primaryDirection, secondaryDirection, satVectors.primarySat, satVectors.secondarySat);
// build the attitude
final FieldAngularCoordinates<T> angularCoordinates = new FieldAngularCoordinates<>(inertToSatRotation);
final FieldAttitude<T> attitude = new FieldAttitude<>(date, inertialFrame, angularCoordinates);
return attitude.withReferenceFrame(frame);
}
/** Container for cached satellite vectors. */
private static class Cache<T extends CalculusFieldElement<T>> {
/** Satellite vector for primary target. */
private final FieldVector3D<FieldUnivariateDerivative2<T>> primarySat;
/** Satellite vector for primary target. */
private final FieldVector3D<FieldUnivariateDerivative2<T>> secondarySat;
/** Simple constructor.
* @param field field to which the elements belong
* @param primarySat satellite vector for primary target
* @param secondarySat satellite vector for primary target
*/
Cache(final Field<T> field,
final FieldVector3D<UnivariateDerivative2> primarySat,
final FieldVector3D<UnivariateDerivative2> secondarySat) {
final FieldUnivariateDerivative2<T> zero =
new FieldUnivariateDerivative2<>(field.getZero(), field.getZero(), field.getZero());
this.primarySat = new FieldVector3D<>(zero.newInstance(primarySat.getX().getValue()),
zero.newInstance(primarySat.getY().getValue()),
zero.newInstance(primarySat.getZ().getValue()));
this.secondarySat = new FieldVector3D<>(zero.newInstance(secondarySat.getX().getValue()),
zero.newInstance(secondarySat.getY().getValue()),
zero.newInstance(secondarySat.getZ().getValue()));
}
}
}