1 /* Copyright 2002-2026 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
18 package org.orekit.propagation.relative.clohessywiltshire;
19
20 import org.hipparchus.CalculusFieldElement;
21 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22 import org.hipparchus.linear.FieldMatrix;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.orekit.utils.FieldPVCoordinates;
25 import org.orekit.utils.TimeStampedFieldPVCoordinates;
26
27 /**
28 * This class stores the 4 sub-matrices of the 6x6 state transition matrix of the Clohessy-Wiltshire equations.
29 *
30 * @param <T> Any scalar field.
31 * @author Romain Cuvillon
32 * @since 14.0
33 */
34 public class FieldClohessyWiltshireMatrices<T extends CalculusFieldElement<T>> {
35
36 /**
37 * Time since epoch used in computing the matrices, in seconds.
38 */
39 private final T timeSinceEpoch;
40
41 /**
42 * Matrix that multiplies the position to compute the position.
43 */
44 private final FieldMatrix<T> phiRR;
45
46 /**
47 * Matrix that multiplies the velocity to compute the position.
48 */
49 private final FieldMatrix<T> phiRV;
50
51 /**
52 * Matrix that multiplies the position to compute the velocity.
53 */
54 private final FieldMatrix<T> phiVR;
55
56 /**
57 * Matrix that multiplies the velocity to compute the velocity.
58 */
59 private final FieldMatrix<T> phiVV;
60
61 /**
62 * Constructor of the 4 sub-matrices of the 6x6 state transition matrix of the Clohessy-Wiltshire equations.
63 * <blockquote>
64 * δ<b>r</b>(t) = φ<sub>rr</sub>(t) δ<b>r</b><sub>0</sub> + φ<sub>rv</sub>(t)
65 * δ<b>v</b><sub>0</sub>
66 * <br><br>
67 * δ<b>v</b>(t) = φ<sub>vr</sub>(t) δ<b>r</b><sub>0</sub> + φ<sub>vv</sub>(t)
68 * δ<b>v</b><sub>0</sub>
69 * </blockquote>
70 *
71 * @param timeSinceEpoch duration in seconds since epoch
72 * @param phiRR position to position transition sub-matrix
73 * @param phiRV velocity to position transition sub-matrix
74 * @param phiVR position to velocity transition sub-matrix
75 * @param phiVV velocity to velocity transition sub-matrix
76 */
77 public FieldClohessyWiltshireMatrices(final T timeSinceEpoch, final FieldMatrix<T> phiRR,
78 final FieldMatrix<T> phiRV, final FieldMatrix<T> phiVR,
79 final FieldMatrix<T> phiVV) {
80 this.timeSinceEpoch = timeSinceEpoch;
81 this.phiRR = phiRR;
82 this.phiRV = phiRV;
83 this.phiVR = phiVR;
84 this.phiVV = phiVV;
85 }
86
87 /**
88 * Get the sub-matrix of the transition matrix φ<sub>rr</sub>(t).
89 *
90 * @return φ<sub>rr</sub>(t) sub-matrix
91 */
92 public FieldMatrix<T> getPhiRR() {
93 return phiRR;
94 }
95
96 /**
97 * Get the sub-matrix of the transition matrix φ<sub>rv</sub>(t).
98 *
99 * @return φ<sub>rv</sub>(t) sub-matrix
100 */
101 public FieldMatrix<T> getPhiRV() {
102 return phiRV;
103 }
104
105 /**
106 * Get the sub-matrix of the transition matrix φ<sub>vr</sub>(t).
107 *
108 * @return φ<sub>vr</sub>(t) sub-matrix
109 */
110 public FieldMatrix<T> getPhiVR() {
111 return phiVR;
112 }
113
114 /**
115 * Get the sub-matrix of the transition matrix φ<sub>vv</sub>(t).
116 *
117 * @return φ<sub>vv</sub>(t) sub-matrix
118 */
119 public FieldMatrix<T> getPhiVV() {
120 return phiVV;
121 }
122
123 /**
124 * Transforms the input initial PVT expressed in the target's QSW LOF to the PVT at the time encoded in the
125 * Clohessy-Wiltshire matrices contained in the object.
126 * <p>pos(t) = Φrr * pos(0) + Φrv * vel(0)</p>
127 * <p>vel(t) = Φvr * pos(0) + Φvv * vel(0)</p>
128 *
129 * @param pvt input initial PVT expressed in the target's QSW LOF.
130 * @return PVT at the time encoded in the Clohessy-Wiltshire matrices, expressed in the target's QSW LOF.
131 */
132 public TimeStampedFieldPVCoordinates<T> transform(final TimeStampedFieldPVCoordinates<T> pvt) {
133 return new TimeStampedFieldPVCoordinates<>(pvt.getDate().shiftedBy(timeSinceEpoch),
134 transform(new FieldPVCoordinates<>(pvt.getPosition(),
135 pvt.getVelocity())));
136 }
137
138 /**
139 * Transforms the input initial PV expressed in the target's QSW LOF to the PV at the time encoded in the
140 * Clohessy-Wiltshire matrices contained in the object.
141 * <p>pos(t) = Φrr * pos(0) + Φrv * vel(0)</p>
142 * <p>vel(t) = Φvr * pos(0) + Φvv * vel(0)</p>
143 *
144 * @param pv input initial PV expressed in the target's QSW LOF.
145 * @return PV at the time encoded in the Clohessy-Wiltshire matrices, expressed in the target's QSW LOF.
146 */
147 public FieldPVCoordinates<T> transform(final FieldPVCoordinates<T> pv) {
148 // Convert Vector3D to RealMatrix
149 final FieldMatrix<T> pos0 = MatrixUtils.createColumnFieldMatrix(pv.getPosition().toArray());
150 final FieldMatrix<T> vel0 = MatrixUtils.createColumnFieldMatrix(pv.getVelocity().toArray());
151
152 // Transform the input PV using the current matrices
153 final FieldVector3D<T> pos1 =
154 new FieldVector3D<>(phiRR.multiply(pos0).add(phiRV.multiply(vel0)).getColumn(0));
155 final FieldVector3D<T> vel1 =
156 new FieldVector3D<>(phiVR.multiply(pos0).add(phiVV.multiply(vel0)).getColumn(0));
157
158 // Return transformed PV
159 return new FieldPVCoordinates<>(pos1, vel1);
160 }
161 }