1 /* Copyright 2022-2025 Luc Maisonobe
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.files.ccsds.ndm.adm;
18
19 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.util.FastMath;
23 import org.hipparchus.util.SinCos;
24 import org.orekit.errors.OrekitException;
25 import org.orekit.errors.OrekitMessages;
26
27 /** Utility to extract precession from the motion of a spin axis.
28 * <p>
29 * The precession is used in {@link AttitudeType#SPIN_NUTATION} and
30 * {@link AttitudeType#SPIN_NUTATION_MOMENTUM} attitude types. CCSDS
31 * calls it nutation, but it is really precession.
32 * </p>
33 * @author Luc Maisonobe
34 * @since 12.0
35 */
36 class PrecessionFinder {
37
38 /** Precession axis (axis of the cone described by spin). */
39 private final Vector3D axis;
40
41 /** Precession angle (fixed cone angle between precession axis and spin axis). */
42 private final double precessionAngle;
43
44 /** Angular velocity around the precession axis (rad/s). */
45 private final double angularVelocity;
46
47 /** Build from spin axis motion.
48 * <p>
49 * Note that the derivatives up to second order are really needed
50 * in order to retrieve the precession motion.
51 * </p>
52 * @param spin spin axis, including value, first and second derivative
53 */
54 PrecessionFinder(final FieldVector3D<UnivariateDerivative2> spin) {
55
56 // using a suitable coordinates frame, the spin axis can be considered to describe
57 // a cone of half aperture angle 0 ≤ η ≤ π around k axis, at angular rate ω ≥ 0
58 // with an initial position in the (+i,±k) half-plane. In this frame, the normalized
59 // direction of spin s = spin/||spin|| and its first and second time derivatives
60 // have coordinates:
61 // s(t): (sin η cos ω(t-t₀), sin η sin ω(t-t₀), cos η)
62 // ds/dt(t): (-ω sin η sin ω(t-t₀), ω sin η cos ω(t-t₀), 0)
63 // d²s/dt²(t): (-ω² sin η cos ω(t-t₀), -ω² sin η sin ω(t-t₀), 0)
64 // at initial time t = t₀ this leads to:
65 // s₀ = s(t₀): (sin η, 0, cos η)
66 // s₁ = ds/dt(t₀): (0, ω sin η, 0)
67 // s₂ = d²s/dt²(t₀): (-ω² sin η, 0, 0)
68 // however, only the spin vector itself is provided, we don't initially know the unit
69 // vectors (i, j, k) of this "suitable coordinates frame". We can however easily
70 // build another frame (u, v, w) from the normalized spin vector s as follows:
71 // u = s₀, v = s₁/||s₁||, w = u ⨯ v
72 // the coordinates of vectors u, v and w in the "suitable coordinates frame" are:
73 // u: ( sin η, 0, cos η)
74 // v: ( 0, 1, 0)
75 // w: (-cos η, 0, sin η)
76 // we can then deduce the following relations, which can be computed regardless
77 // of frames used to express the various vectors:
78 // s₁ · v = ω sin η = ||s₁||
79 // s₂ · w = ω² sin η cos η
80 // these relations can be solved for η and ω (we know that 0 ≤ η ≤ π and ω ≥ 0):
81 // η = atan2(||s₁||², s₂ · w)
82 // ω = ||s₁|| / sin η
83 // then the k axis, which is the precession axis, can be deduced as:
84 // k = cos η u + sin η w
85
86 final UnivariateDerivative2 nS = spin.getNorm();
87 if (nS.getValue() == 0) {
88 // special case, no motion at all, set up arbitrary values
89 axis = Vector3D.PLUS_K;
90 precessionAngle = 0.0;
91 angularVelocity = 0.0;
92 } else {
93
94 // build the derivatives vectors
95 final FieldVector3D<UnivariateDerivative2> s = spin.scalarMultiply(nS.reciprocal());
96 final Vector3D s0 = new Vector3D(s.getX().getValue(),
97 s.getY().getValue(),
98 s.getZ().getValue());
99 final Vector3D s1 = new Vector3D(s.getX().getFirstDerivative(),
100 s.getY().getFirstDerivative(),
101 s.getZ().getFirstDerivative());
102 final Vector3D s2 = new Vector3D(s.getX().getSecondDerivative(),
103 s.getY().getSecondDerivative(),
104 s.getZ().getSecondDerivative());
105
106 final double nV2 = s1.getNormSq();
107 if (nV2 == 0.0) {
108 // special case: we have a fixed spin vector
109 axis = s0;
110 precessionAngle = 0.0;
111 angularVelocity = 0.0;
112 } else {
113
114 // check second derivatives were provided ; we do it on spin rather than s2
115 // and use test against exact 0.0 because the normalization process changes
116 // the derivatives and what we really want to check are missing derivatives
117 if (new Vector3D(spin.getX().getSecondDerivative(),
118 spin.getY().getSecondDerivative(),
119 spin.getZ().getSecondDerivative()).getNormSq() == 0) {
120 throw new OrekitException(OrekitMessages.CANNOT_ESTIMATE_PRECESSION_WITHOUT_PROPER_DERIVATIVES);
121 }
122
123 // build the unit vectors
124 final double nV = FastMath.sqrt(nV2);
125 final Vector3D v = s1.scalarMultiply(1.0 / nV);
126 final Vector3D w = Vector3D.crossProduct(s0, v);
127
128 // compute precession model
129 precessionAngle = FastMath.atan2(nV2, Vector3D.dotProduct(s2, w));
130 final SinCos sc = FastMath.sinCos(precessionAngle);
131 angularVelocity = nV / sc.sin();
132 axis = new Vector3D(sc.cos(), s0, sc.sin(), w);
133
134 }
135 }
136
137 }
138
139 /** Get the precession axis.
140 * @return precession axis
141 */
142 public Vector3D getAxis() {
143 return axis;
144 }
145
146 /** Get the precession angle.
147 * @return fixed cone angle between precession axis an spin axis (rad)
148 */
149 public double getPrecessionAngle() {
150 return precessionAngle;
151 }
152
153 /** Get the angular velocity around precession axis.
154 * @return angular velocity around precession axis (rad/s)
155 */
156 public double getAngularVelocity() {
157 return angularVelocity;
158 }
159
160 }