LofOffsetPointing.java
/* Copyright 2002-2024 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 java.util.ArrayList;
import java.util.List;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.geometry.euclidean.threed.FieldLine;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Line;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.FieldRotation;
import org.orekit.bodies.BodyShape;
import org.orekit.bodies.FieldGeodeticPoint;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.FieldStaticTransform;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.StaticTransform;
import org.orekit.frames.Transform;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.FieldTimeInterpolator;
import org.orekit.time.TimeInterpolator;
import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.Constants;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
/**
* This class provides a default attitude provider.
* <p>
* The attitude pointing law is defined by an attitude provider and
* the satellite axis vector chosen for pointing.
* </p>
* @author Véronique Pommier-Maurussane
*/
public class LofOffsetPointing extends GroundPointing {
/** Rotation from local orbital frame. */
private final AttitudeProvider attitudeLaw;
/** Body shape. */
private final BodyShape shape;
/** Chosen satellite axis for pointing, given in satellite frame. */
private final Vector3D satPointingVector;
/** Creates new instance.
* @param inertialFrame frame in which orbital velocities are computed
* @param shape Body shape
* @param attLaw Attitude law
* @param satPointingVector satellite vector defining the pointing direction
* @since 7.1
*/
public LofOffsetPointing(final Frame inertialFrame, final BodyShape shape,
final AttitudeProvider attLaw, final Vector3D satPointingVector) {
super(inertialFrame, shape.getBodyFrame());
this.shape = shape;
this.attitudeLaw = attLaw;
this.satPointingVector = satPointingVector;
}
/** {@inheritDoc} */
@Override
public Attitude getAttitude(final PVCoordinatesProvider pvProv,
final AbsoluteDate date, final Frame frame) {
return attitudeLaw.getAttitude(pvProv, date, frame);
}
/** {@inheritDoc} */
@Override
public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date, final Frame frame) {
return attitudeLaw.getAttitude(pvProv, date, frame);
}
/** {@inheritDoc} */
@Override
public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv,
final AbsoluteDate date, final Frame frame) {
return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
}
/** {@inheritDoc} */
@Override
public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date, final Frame frame) {
return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
}
/** {@inheritDoc} */
@Override
public TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv,
final AbsoluteDate date, final Frame frame) {
// sample intersection points in current date neighborhood
final double h = 0.1;
final List<TimeStampedPVCoordinates> sample = new ArrayList<>();
Transform centralRefToBody = null;
for (int i = -1; i < 2; ++i) {
final AbsoluteDate shifted = date.shiftedBy(i * h);
// transform from specified reference frame to spacecraft frame
final StaticTransform refToSc = StaticTransform.of(shifted, pvProv.getPosition(shifted, frame).negate(),
attitudeLaw.getAttitudeRotation(pvProv, shifted, frame));
// transform from specified reference frame to body frame
final StaticTransform refToBody;
if (i == 0) {
refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
} else {
refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
}
sample.add(losIntersectionWithBody(StaticTransform.compose(shifted, refToSc.getInverse(), refToBody)));
}
// create interpolator
final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
// use interpolation to compute properly the time-derivatives
final TimeStampedPVCoordinates targetBody =
interpolator.interpolate(date, sample);
// convert back to caller specified frame
return centralRefToBody.getInverse().transformPVCoordinates(targetBody);
}
/** {@inheritDoc} */
@Override
protected Vector3D getTargetPosition(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {
// transform from specified reference frame to spacecraft frame
final StaticTransform refToSc = StaticTransform.of(date, pvProv.getPosition(date, frame).negate(),
attitudeLaw.getAttitudeRotation(pvProv, date, frame));
// transform from specified reference frame to body frame
final StaticTransform refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);
final Vector3D targetBody = losIntersectionWithBody(StaticTransform.compose(date, refToSc.getInverse(), refToBody)).getPosition();
// convert back to caller specified frame
return refToBody.getInverse().transformPosition(targetBody);
}
/** {@inheritDoc} */
@Override
public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date,
final Frame frame) {
// sample intersection points in current date neighborhood
final double h = 0.1;
final List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<>();
FieldTransform<T> centralRefToBody = null;
for (int i = -1; i < 2; ++i) {
final FieldAbsoluteDate<T> shifted = date.shiftedBy(i * h);
// transform from specified reference frame to spacecraft frame
final FieldStaticTransform<T> refToSc = FieldStaticTransform.of(shifted,
pvProv.getPVCoordinates(shifted, frame).getPosition().negate(), attitudeLaw.getAttitudeRotation(pvProv, shifted, frame));
// transform from specified reference frame to body frame
final FieldStaticTransform<T> refToBody;
if (i == 0) {
refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
} else {
refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
}
sample.add(losIntersectionWithBody(FieldStaticTransform.compose(shifted, refToSc.getInverse(), refToBody)));
}
// create interpolator
final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_P);
// use interpolation to compute properly the time-derivatives
final TimeStampedFieldPVCoordinates<T> targetBody =
interpolator.interpolate(date, sample);
// convert back to caller specified frame
return centralRefToBody.getInverse().transformPVCoordinates(targetBody);
}
/** {@inheritDoc} */
@Override
protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getTargetPosition(final FieldPVCoordinatesProvider<T> pvProv,
final FieldAbsoluteDate<T> date,
final Frame frame) {
// transform from specified reference frame to spacecraft frame
final FieldStaticTransform<T> refToSc = FieldStaticTransform.of(date, pvProv.getPosition(date, frame).negate(),
attitudeLaw.getAttitudeRotation(pvProv, date, frame));
// transform from specified reference frame to body frame
final FieldStaticTransform<T> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);
final FieldVector3D<T> targetBody = losIntersectionWithBody(FieldStaticTransform.compose(date, refToSc.getInverse(), refToBody)).getPosition();
// convert back to caller specified frame
return refToBody.getInverse().transformPosition(targetBody);
}
/** Compute line of sight intersection with body.
* @param scToBody transform from spacecraft frame to body frame
* @return intersection point in body frame (only the position is set!)
*/
private TimeStampedPVCoordinates losIntersectionWithBody(final StaticTransform scToBody) {
// compute satellite pointing axis and position/velocity in body frame
final Vector3D pointingBodyFrame = scToBody.transformVector(satPointingVector);
final Vector3D pBodyFrame = scToBody.transformPosition(Vector3D.ZERO);
// Line from satellite following pointing direction
// we use arbitrarily the Earth radius as a scaling factor, it could be anything else
final Line pointingLine = new Line(pBodyFrame,
pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
pointingBodyFrame),
1.0e-10);
// Intersection with body shape
final GeodeticPoint gpIntersection =
shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), scToBody.getDate());
final Vector3D pIntersection =
(gpIntersection == null) ? null : shape.transform(gpIntersection);
// Check there is an intersection and it is not in the reverse pointing direction
if (pIntersection == null ||
Vector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame) < 0) {
throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
}
return new TimeStampedPVCoordinates(scToBody.getDate(),
pIntersection, Vector3D.ZERO, Vector3D.ZERO);
}
/** Compute line of sight intersection with body.
* @param scToBody transform from spacecraft frame to body frame
* @param <T> type of the field elements
* @return intersection point in body frame (only the position is set!)
*/
private <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> losIntersectionWithBody(final FieldStaticTransform<T> scToBody) {
// compute satellite pointing axis and position/velocity in body frame
final FieldVector3D<T> pointingBodyFrame = scToBody.transformVector(satPointingVector);
final FieldVector3D<T> pBodyFrame = scToBody.transformPosition(Vector3D.ZERO);
// Line from satellite following pointing direction
// we use arbitrarily the Earth radius as a scaling factor, it could be anything else
final FieldLine<T> pointingLine = new FieldLine<>(pBodyFrame,
pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
pointingBodyFrame),
1.0e-10);
// Intersection with body shape
final FieldGeodeticPoint<T> gpIntersection =
shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), new FieldAbsoluteDate<>(pBodyFrame.getX().getField(), scToBody.getDate()));
final FieldVector3D<T> pIntersection =
(gpIntersection == null) ? null : shape.transform(gpIntersection);
// Check there is an intersection and it is not in the reverse pointing direction
if (pIntersection == null ||
FieldVector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame).getReal() < 0) {
throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
}
final FieldVector3D<T> zero = FieldVector3D.getZero(pBodyFrame.getX().getField());
return new TimeStampedFieldPVCoordinates<>(scToBody.getDate(),
pIntersection, zero, zero);
}
}