YawCompensation

/* Copyright 2002-2025 CS GROUP
 * 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.Field;
import org.hipparchus.CalculusFieldElement;
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.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;


/**
 * This class handles yaw compensation attitude provider.

 * <p>
 * Yaw compensation is mainly used for Earth observation satellites. As a
 * satellites moves along its track, the image of ground points move on
 * the focal point of the optical sensor. This motion is a combination of
 * the satellite motion, but also on the Earth rotation and on the current
 * attitude (in particular if the pointing includes Roll or Pitch offset).
 * In order to reduce geometrical distortion, the yaw angle is changed a
 * little from the simple ground pointing attitude such that the apparent
 * motion of ground points is along a prescribed axis (orthogonal to the
 * optical sensors rows), taking into account all effects.
 * </p>
 * <p>
 * This attitude is implemented as a wrapper on top of an underlying ground
 * pointing law that defines the roll and pitch angles.
 * </p>
 * <p>
 * Instances of this class are guaranteed to be immutable.
 * </p>
 * @see     GroundPointing
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class YawCompensation extends GroundPointingAttitudeModifier implements AttitudeProviderModifier {

    /** J axis. */
    private static final PVCoordinates PLUS_J =
            new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);

    /** K axis. */
    private static final PVCoordinates PLUS_K =
            new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);

    /** Creates a new instance.
     * @param inertialFrame frame in which orbital velocities are computed
     * @param groundPointingLaw ground pointing attitude provider without yaw compensation
     * @since 7.1
     */
    public YawCompensation(final Frame inertialFrame, final GroundPointing groundPointingLaw) {
        super(inertialFrame, groundPointingLaw.getBodyFrame(), groundPointingLaw);
    }

    /** {@inheritDoc} */
    @Override
    public Attitude getAttitude(final PVCoordinatesProvider pvProv,
                                final AbsoluteDate date, final Frame frame) {

        final Transform bodyToRef = getBodyFrame().getTransformTo(frame, date);

        // compute sliding target ground point
        final PVCoordinates slidingRef  = getTargetPV(pvProv, date, frame);
        final PVCoordinates slidingBody = bodyToRef.getInverse().transformPVCoordinates(slidingRef);

        // compute relative position of sliding ground point with respect to satellite
        final PVCoordinates relativePosition =
                new PVCoordinates(pvProv.getPVCoordinates(date, frame), slidingRef);

        // compute relative velocity of fixed ground point with respect to sliding ground point
        // the velocity part of the transformPVCoordinates for the sliding point ps
        // from body frame to reference frame is:
        //     d(ps_ref)/dt = r(d(ps_body)/dt + dq/dt) - Ω ⨯ ps_ref
        // where r is the rotation part of the transform, Ω is the corresponding
        // angular rate, and dq/dt is the derivative of the translation part of the
        // transform (the translation itself, without derivative, is hidden in the
        // ps_ref part in the cross product).
        // The sliding point ps is co-located to a fixed ground point pf (i.e. they have the
        // same position at time of computation), but this fixed point as zero velocity
        // with respect to the ground. So the velocity part of the transformPVCoordinates
        // for this fixed point can be computed using the same formula as above:
        // from body frame to reference frame is:
        //     d(pf_ref)/dt = r(0 + dq/dt) - Ω ⨯ pf_ref
        // so remembering that the two points are at the same location at computation time,
        // i.e. that at t=0 pf_ref=ps_ref, the relative velocity between the fixed point
        // and the sliding point is given by the simple expression:
        //     d(ps_ref)/dt - d(pf_ref)/dt = r(d(ps_body)/dt)
        // the acceleration is computed by differentiating the expression, which gives:
        //    d²(ps_ref)/dt² - d²(pf_ref)/dt² = r(d²(ps_body)/dt²) - Ω ⨯ [d(ps_ref)/dt - d(pf_ref)/dt]
        final Vector3D v = bodyToRef.getRotation().applyTo(slidingBody.getVelocity());
        final Vector3D a = new Vector3D(+1, bodyToRef.getRotation().applyTo(slidingBody.getAcceleration()),
                                        -1, Vector3D.crossProduct(bodyToRef.getRotationRate(), v));
        final PVCoordinates relativeVelocity = new PVCoordinates(v, a, Vector3D.ZERO);

        final PVCoordinates relativeNormal =
                PVCoordinates.crossProduct(relativePosition, relativeVelocity).normalize();

        // attitude definition :
        //  . Z satellite axis points to sliding target
        //  . target relative velocity is in (Z, X) plane, in the -X half plane part
        return new Attitude(frame,
                            new TimeStampedAngularCoordinates(date,
                                                              relativePosition.normalize(),
                                                              relativeNormal.normalize(),
                                                              PLUS_K, PLUS_J,
                                                              1.0e-9));

    }

    /** {@inheritDoc} */
    @Override
    public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
                                                                        final FieldAbsoluteDate<T> date, final Frame frame) {

        final Field<T>              field = date.getField();
        final FieldVector3D<T>      zero  = FieldVector3D.getZero(field);
        final FieldPVCoordinates<T> plusJ = new FieldPVCoordinates<>(FieldVector3D.getPlusJ(field), zero, zero);
        final FieldPVCoordinates<T> plusK = new FieldPVCoordinates<>(FieldVector3D.getPlusK(field), zero, zero);

        final FieldTransform<T> bodyToRef = getBodyFrame().getTransformTo(frame, date);

        // compute sliding target ground point
        final FieldPVCoordinates<T> slidingRef  = getTargetPV(pvProv, date, frame);
        final FieldPVCoordinates<T> slidingBody = bodyToRef.getInverse().transformPVCoordinates(slidingRef);

        // compute relative position of sliding ground point with respect to satellite
        final FieldPVCoordinates<T> relativePosition =
                new FieldPVCoordinates<>(pvProv.getPVCoordinates(date, frame), slidingRef);

        // compute relative velocity of fixed ground point with respect to sliding ground point
        // the velocity part of the transformPVCoordinates for the sliding point ps
        // from body frame to reference frame is:
        //     d(ps_ref)/dt = r(d(ps_body)/dt + dq/dt) - Ω ⨯ ps_ref
        // where r is the rotation part of the transform, Ω is the corresponding
        // angular rate, and dq/dt is the derivative of the translation part of the
        // transform (the translation itself, without derivative, is hidden in the
        // ps_ref part in the cross product).
        // The sliding point ps is co-located to a fixed ground point pf (i.e. they have the
        // same position at time of computation), but this fixed point as zero velocity
        // with respect to the ground. So the velocity part of the transformPVCoordinates
        // for this fixed point can be computed using the same formula as above:
        // from body frame to reference frame is:
        //     d(pf_ref)/dt = r(0 + dq/dt) - Ω ⨯ pf_ref
        // so remembering that the two points are at the same location at computation time,
        // i.e. that at t=0 pf_ref=ps_ref, the relative velocity between the fixed point
        // and the sliding point is given by the simple expression:
        //     d(ps_ref)/dt - d(pf_ref)/dt = r(d(ps_body)/dt)
        // the acceleration is computed by differentiating the expression, which gives:
        //    d²(ps_ref)/dt² - d²(pf_ref)/dt² = r(d²(ps_body)/dt²) - Ω ⨯ [d(ps_ref)/dt - d(pf_ref)/dt]
        final FieldVector3D<T> v = bodyToRef.getRotation().applyTo(slidingBody.getVelocity());
        final FieldVector3D<T> a = new FieldVector3D<>(+1, bodyToRef.getRotation().applyTo(slidingBody.getAcceleration()),
                                                       -1, FieldVector3D.crossProduct(bodyToRef.getRotationRate(), v));
        final FieldPVCoordinates<T> relativeVelocity = new FieldPVCoordinates<>(v, a, FieldVector3D.getZero(date.getField()));

        final FieldPVCoordinates<T> relativeNormal =
                        relativePosition.crossProduct(relativeVelocity).normalize();

        // attitude definition :
        //  . Z satellite axis points to sliding target
        //  . target relative velocity is in (Z, X) plane, in the -X half plane part
        return new FieldAttitude<>(frame,
                                   new TimeStampedFieldAngularCoordinates<>(date,
                                                                            relativePosition.normalize(),
                                                                            relativeNormal.normalize(),
                                                                            plusK, plusJ,
                                                                            1.0e-9));

    }

    /** Compute the yaw compensation angle at date.
     * @param pvProv provider for PV coordinates
     * @param date date at which compensation is requested
     * @param frame reference frame from which attitude is computed
     * @return yaw compensation angle for orbit.
     */
    public double getYawAngle(final PVCoordinatesProvider pvProv,
                              final AbsoluteDate date, final Frame frame) {
        final Rotation rBase        = getBaseState(pvProv, date, frame).getRotation();
        final Rotation rCompensated = getAttitude(pvProv, date, frame).getRotation();
        final Rotation compensation = rCompensated.compose(rBase.revert(), RotationConvention.VECTOR_OPERATOR);
        return -compensation.applyTo(Vector3D.PLUS_I).getAlpha();
    }

    /** Compute the yaw compensation angle at date.
     * @param pvProv provider for PV coordinates
     * @param date date at which compensation is requested
     * @param frame reference frame from which attitude is computed
     * @param <T> type of the field elements
     * @return yaw compensation angle for orbit.
     * @since 9.0
     */
    public <T extends CalculusFieldElement<T>> T getYawAngle(final FieldPVCoordinatesProvider<T> pvProv,
                                                         final FieldAbsoluteDate<T> date,
                                                         final Frame frame) {
        final FieldRotation<T> rBase        = getBaseState(pvProv, date, frame).getRotation();
        final FieldRotation<T> rCompensated = getAttitude(pvProv, date, frame).getRotation();
        final FieldRotation<T> compensation = rCompensated.compose(rBase.revert(), RotationConvention.VECTOR_OPERATOR);
        return compensation.applyTo(Vector3D.PLUS_I).getAlpha().negate();
    }

}