PolynomialRotation.java
/* Copyright 2013-2022 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.rugged.los;
import java.util.stream.Stream;
import org.hipparchus.Field;
import org.hipparchus.analysis.differentiation.Derivative;
import org.hipparchus.analysis.polynomials.PolynomialFunction;
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.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.orekit.rugged.utils.DerivativeGenerator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterObserver;
/** {@link LOSTransform LOS transform} based on a rotation with polynomial angle.
* @author Luc Maisonobe
* @see LOSBuilder
*/
public class PolynomialRotation implements LOSTransform {
/** Parameters scaling factor.
* <p>
* We use a power of 2 to avoid numeric noise introduction
* in the multiplications/divisions sequences.
* </p>
*/
private final double SCALE = FastMath.scalb(1.0, -20);
/** Rotation axis. */
private final Vector3D axis;
/** Rotation angle polynomial. */
private PolynomialFunction angle;
/** Rotation axis and derivatives. */
private FieldVector3D<?> axisDS;
/** Rotation angle polynomial and derivatives. */
private Derivative<?>[] angleDS;
/** Reference date for polynomial evaluation. */
private final AbsoluteDate referenceDate;
/** Drivers for rotation angle polynomial coefficients. */
private final ParameterDriver[] coefficientsDrivers;
/** Simple constructor.
* <p>
* The angle of the rotation is evaluated as a polynomial in t,
* where t is the duration in seconds between evaluation date and
* reference date. The parameters are the polynomial coefficients,
* with the constant term at index 0.
* </p>
* @param name name of the rotation (used for estimated parameters identification)
* @param axis rotation axis
* @param referenceDate reference date for the polynomial angle
* @param angleCoeffs polynomial coefficients of the polynomial angle,
* with the constant term at index 0
*/
public PolynomialRotation(final String name,
final Vector3D axis,
final AbsoluteDate referenceDate,
final double... angleCoeffs) {
this.axis = axis;
this.referenceDate = referenceDate;
this.coefficientsDrivers = new ParameterDriver[angleCoeffs.length];
final ParameterObserver resettingObserver = new ParameterObserver() {
@Override
public void valueChanged(final double previousValue, final ParameterDriver driver) {
// reset rotations to null, they will be evaluated lazily if needed
angle = null;
axisDS = null;
angleDS = null;
}
};
for (int i = 0; i < angleCoeffs.length; ++i) {
coefficientsDrivers[i] = new ParameterDriver(name + "[" + i + "]", angleCoeffs[i], SCALE,
Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
coefficientsDrivers[i].addObserver(resettingObserver);
}
}
/** Simple constructor.
* <p>
* The angle of the rotation is evaluated as a polynomial in t,
* where t is the duration in seconds between evaluation date and
* reference date. The parameters are the polynomial coefficients,
* with the constant term at index 0.
* </p>
* @param name name of the rotation (used for estimated parameters identification)
* @param axis rotation axis
* @param referenceDate reference date for the polynomial angle
* @param angle polynomial angle
*/
public PolynomialRotation(final String name,
final Vector3D axis,
final AbsoluteDate referenceDate,
final PolynomialFunction angle) {
this(name, axis, referenceDate, angle.getCoefficients());
}
/** {@inheritDoc}
* @since 2.0
*/
@Override
public Stream<ParameterDriver> getParametersDrivers() {
return Stream.of(coefficientsDrivers);
}
/** {@inheritDoc} */
@Override
public Vector3D transformLOS(final int i, final Vector3D los, final AbsoluteDate date) {
if (angle == null) {
// lazy evaluation of the rotation
final double[] coefficients = new double[coefficientsDrivers.length];
for (int k = 0; k < coefficients.length; ++k) {
coefficients[k] = coefficientsDrivers[k].getValue();
}
angle = new PolynomialFunction(coefficients);
}
return new Rotation(axis,
angle.value(date.durationFrom(referenceDate)),
RotationConvention.VECTOR_OPERATOR).applyTo(los);
}
/** {@inheritDoc} */
@SuppressWarnings("unchecked")
@Override
public <T extends Derivative<T>> FieldVector3D<T> transformLOS(final int i, final FieldVector3D<T> los,
final AbsoluteDate date,
final DerivativeGenerator<T> generator) {
final Field<T> field = generator.getField();
final FieldVector3D<T> axisD;
final T[] angleD;
if (axisDS == null || !axisDS.getX().getField().equals(field)) {
// lazy evaluation of the rotation
axisD = new FieldVector3D<>(generator.constant(axis.getX()),
generator.constant(axis.getY()),
generator.constant(axis.getZ()));
angleD = MathArrays.buildArray(field, coefficientsDrivers.length);
for (int k = 0; k < angleD.length; ++k) {
angleD[k] = generator.variable(coefficientsDrivers[k]);
}
// cache evaluated rotation parameters
axisDS = axisD;
angleDS = angleD;
} else {
// reuse cached values
axisD = (FieldVector3D<T>) axisDS;
angleD = (T[]) angleDS;
}
// evaluate polynomial, with all its partial derivatives
final double t = date.durationFrom(referenceDate);
T alpha = field.getZero();
for (int k = angleDS.length - 1; k >= 0; --k) {
alpha = alpha.multiply(t).add(angleD[k]);
}
return new FieldRotation<>(axisD, alpha, RotationConvention.VECTOR_OPERATOR).applyTo(los);
}
}