1   /* Copyright 2002-2021 CS GROUP
2    * Licensed to CS GROUP (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.forces.gravity;
18  
19  import org.hipparchus.analysis.differentiation.Gradient;
20  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21  import org.hipparchus.geometry.euclidean.threed.Vector3D;
22  import org.orekit.bodies.CelestialBody;
23  import org.orekit.propagation.SpacecraftState;
24  
25  /** Third body attraction force model.
26   * This class is a copy of {@link ThirdBodyAttraction} class.
27   * The computation of derivatives of the acceleration
28   * w.r.t. the Epoch has been added.
29   *
30   * @author Fabien Maussion
31   * @author Véronique Pommier-Maurussane
32   * @since 10.2
33   */
34  public class ThirdBodyAttractionEpoch extends ThirdBodyAttraction {
35  
36      /** The body to consider. */
37      private final CelestialBody body;
38  
39      /** Simple constructor.
40       * @param body the third body to consider
41       * (ex: {@link org.orekit.bodies.CelestialBodyFactory#getSun()} or
42       * {@link org.orekit.bodies.CelestialBodyFactory#getMoon()})
43       */
44      public ThirdBodyAttractionEpoch(final CelestialBody body) {
45          super(body);
46          this.body = body;
47      }
48  
49      /** Compute acceleration.
50       * @param s current state information: date, kinematics, attitude
51       * @param parameters values of the force model parameters
52       * @return acceleration in same frame as state
53       */
54      private FieldVector3D<Gradient> accelerationToEpoch(final SpacecraftState s, final double[] parameters) {
55  
56          final double gm = parameters[0];
57  
58          // compute bodies separation vectors and squared norm
59          final Vector3D centralToBody = body.getPVCoordinates(s.getDate(), s.getFrame()).getPosition();
60  
61          // Spacecraft Position
62          final double rx = centralToBody.getX();
63          final double ry = centralToBody.getY();
64          final double rz = centralToBody.getZ();
65  
66          final int freeParameters = 3;
67          final Gradient fpx = Gradient.variable(freeParameters, 0, rx);
68          final Gradient fpy = Gradient.variable(freeParameters, 1, ry);
69          final Gradient fpz = Gradient.variable(freeParameters, 2, rz);
70  
71          final FieldVector3D<Gradient> centralToBodyFV = new FieldVector3D<>(new Gradient[] {fpx, fpy, fpz});
72  
73  
74          final Gradient                r2Central = centralToBodyFV.getNormSq();
75          final FieldVector3D<Gradient> satToBody = centralToBodyFV.subtract(s.getPVCoordinates().getPosition());
76          final Gradient                r2Sat     = satToBody.getNormSq();
77  
78          return new FieldVector3D<>(gm, satToBody.scalarMultiply(r2Sat.multiply(r2Sat.sqrt()).reciprocal()),
79                                    -gm, centralToBodyFV.scalarMultiply(r2Central.multiply(r2Central.sqrt()).reciprocal()));
80      }
81  
82      /** Compute derivatives of the state w.r.t epoch.
83       * @param s current state information: date, kinematics, attitude
84       * @param parameters values of the force model parameters
85       * @return derivatives
86       */
87      public double[] getDerivativesToEpoch(final SpacecraftState s, final double[] parameters) {
88  
89          final FieldVector3D<Gradient> acc = accelerationToEpoch(s, parameters);
90          final Vector3D centralToBodyVelocity = body.getPVCoordinates(s.getDate(), s.getFrame()).getVelocity();
91  
92          final double[] dAccxdR1i = acc.getX().getGradient();
93          final double[] dAccydR1i = acc.getY().getGradient();
94          final double[] dAcczdR1i = acc.getZ().getGradient();
95          final double[] v = centralToBodyVelocity.toArray();
96  
97          return new double[] {dAccxdR1i[0] * v[0] + dAccxdR1i[0] * v[1] + dAccxdR1i[0] * v[2],
98              dAccydR1i[0] * v[0] + dAccydR1i[0] * v[1] + dAccydR1i[0] * v[2],
99              dAcczdR1i[0] * v[0] + dAcczdR1i[0] * v[1] + dAcczdR1i[0] * v[2]};
100     }
101 
102 }