1   /* Copyright 2013-2019 CS Systèmes d'Information
2    * Licensed to CS Systèmes d'Information (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.rugged.los;
18  
19  import java.util.stream.Stream;
20  
21  import org.hipparchus.analysis.differentiation.DerivativeStructure;
22  import org.hipparchus.analysis.polynomials.PolynomialFunction;
23  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
24  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25  import org.hipparchus.geometry.euclidean.threed.Rotation;
26  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
27  import org.hipparchus.geometry.euclidean.threed.Vector3D;
28  import org.hipparchus.util.FastMath;
29  import org.orekit.rugged.utils.DSGenerator;
30  import org.orekit.time.AbsoluteDate;
31  import org.orekit.utils.ParameterDriver;
32  import org.orekit.utils.ParameterObserver;
33  
34  /** {@link LOSTransform LOS transform} based on a rotation with polynomial angle.
35   * @author Luc Maisonobe
36   * @see LOSBuilder
37   */
38  public class PolynomialRotation implements LOSTransform {
39  
40      /** Parameters scaling factor.
41       * <p>
42       * We use a power of 2 to avoid numeric noise introduction
43       * in the multiplications/divisions sequences.
44       * </p>
45       */
46      private final double SCALE = FastMath.scalb(1.0, -20);
47  
48      /** Rotation axis. */
49      private final Vector3D axis;
50  
51      /** Rotation angle polynomial. */
52      private PolynomialFunction angle;
53  
54      /** Rotation axis and derivatives. */
55      private FieldVector3D<DerivativeStructure> axisDS;
56  
57      /** Rotation angle polynomial and derivatives. */
58      private DerivativeStructure[] angleDS;
59  
60      /** Reference date for polynomial evaluation. */
61      private final AbsoluteDate referenceDate;
62  
63      /** Drivers for rotation angle polynomial coefficients. */
64      private final ParameterDriver[] coefficientsDrivers;
65  
66      /** Simple constructor.
67       * <p>
68       * The angle of the rotation is evaluated as a polynomial in t,
69       * where t is the duration in seconds between evaluation date and
70       * reference date. The parameters are the polynomial coefficients,
71       * with the constant term at index 0.
72       * </p>
73       * @param name name of the rotation (used for estimated parameters identification)
74       * @param axis rotation axis
75       * @param referenceDate reference date for the polynomial angle
76       * @param angleCoeffs polynomial coefficients of the polynomial angle,
77       * with the constant term at index 0
78       */
79      public PolynomialRotation(final String name,
80                                final Vector3D axis,
81                                final AbsoluteDate referenceDate,
82                                final double... angleCoeffs) {
83          this.axis                = axis;
84          this.referenceDate       = referenceDate;
85          this.coefficientsDrivers = new ParameterDriver[angleCoeffs.length];
86          final ParameterObserver resettingObserver = new ParameterObserver() {
87              @Override
88              public void valueChanged(final double previousValue, final ParameterDriver driver) {
89                  // reset rotations to null, they will be evaluated lazily if needed
90                  angle   = null;
91                  axisDS  = null;
92                  angleDS = null;
93              }
94          };
95          for (int i = 0; i < angleCoeffs.length; ++i) {
96              coefficientsDrivers[i] = new ParameterDriver(name + "[" + i + "]", angleCoeffs[i], SCALE,
97                      Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
98              coefficientsDrivers[i].addObserver(resettingObserver);
99          }
100     }
101 
102     /** Simple constructor.
103      * <p>
104      * The angle of the rotation is evaluated as a polynomial in t,
105      * where t is the duration in seconds between evaluation date and
106      * reference date. The parameters are the polynomial coefficients,
107      * with the constant term at index 0.
108      * </p>
109      * @param name name of the rotation (used for estimated parameters identification)
110      * @param axis rotation axis
111      * @param referenceDate reference date for the polynomial angle
112      * @param angle polynomial angle
113      */
114     public PolynomialRotation(final String name,
115                               final Vector3D axis,
116                               final AbsoluteDate referenceDate,
117                               final PolynomialFunction angle) {
118         this(name, axis, referenceDate, angle.getCoefficients());
119     }
120 
121     /** {@inheritDoc}
122      * @since 2.0
123      */
124     @Override
125     public Stream<ParameterDriver> getParametersDrivers() {
126         return Stream.of(coefficientsDrivers);
127     }
128 
129     /** {@inheritDoc} */
130     @Override
131     public Vector3D transformLOS(final int i, final Vector3D los, final AbsoluteDate date) {
132         if (angle == null) {
133             // lazy evaluation of the rotation
134             final double[] coefficients = new double[coefficientsDrivers.length];
135             for (int k = 0; k < coefficients.length; ++k) {
136                 coefficients[k] = coefficientsDrivers[k].getValue();
137             }
138             angle = new PolynomialFunction(coefficients);
139         }
140         return new Rotation(axis,
141                             angle.value(date.durationFrom(referenceDate)),
142                             RotationConvention.VECTOR_OPERATOR).applyTo(los);
143     }
144 
145     /** {@inheritDoc} */
146     @Override
147     public FieldVector3D<DerivativeStructure> transformLOS(final int i, final FieldVector3D<DerivativeStructure> los,
148                                                            final AbsoluteDate date, final DSGenerator generator) {
149 
150         if (angleDS == null) {
151             // lazy evaluation of the rotation
152             axisDS = new FieldVector3D<DerivativeStructure>(generator.constant(axis.getX()),
153                                                             generator.constant(axis.getY()),
154                                                             generator.constant(axis.getZ()));
155             angleDS = new DerivativeStructure[coefficientsDrivers.length];
156             for (int k = 0; k < angleDS.length; ++k) {
157                 angleDS[k] = generator.variable(coefficientsDrivers[k]);
158             }
159         }
160         // evaluate polynomial, with all its partial derivatives
161         final double t = date.durationFrom(referenceDate);
162         DerivativeStructure alpha = axisDS.getX().getField().getZero();
163         for (int k = angleDS.length - 1; k >= 0; --k) {
164             alpha = alpha.multiply(t).add(angleDS[k]);
165         }
166 
167         return new FieldRotation<DerivativeStructure>(axisDS,
168                                                       alpha,
169                                                       RotationConvention.VECTOR_OPERATOR).applyTo(los);
170 
171     }
172 
173 }