1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.orekit.frames;
19
20 import org.hamcrest.CoreMatchers;
21 import org.hamcrest.MatcherAssert;
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.complex.Complex;
25 import org.hipparchus.complex.ComplexField;
26 import org.hipparchus.geometry.euclidean.threed.FieldLine;
27 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
28 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
30 import org.hipparchus.geometry.euclidean.threed.Rotation;
31 import org.hipparchus.geometry.euclidean.threed.Vector3D;
32 import org.hipparchus.util.Binary64;
33 import org.hipparchus.util.Binary64Field;
34 import org.hipparchus.util.FastMath;
35 import org.junit.jupiter.api.Assertions;
36 import org.junit.jupiter.api.Test;
37 import org.orekit.OrekitMatchers;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.time.FieldAbsoluteDate;
40
41
42
43
44
45
46 public class FieldStaticTransformTest {
47
48
49 @Test
50 public void testSimpleComposition() {
51 doTestSimpleComposition(Binary64Field.getInstance());
52 }
53
54 private <T extends CalculusFieldElement<T>> void doTestSimpleComposition(Field<T> field) {
55
56
57
58
59 FieldRotation<T> fieldRotation = new FieldRotation<>(
60 FieldVector3D.getPlusK(field), field.getZero().newInstance(0.5 * FastMath.PI),
61 RotationConvention.VECTOR_OPERATOR);
62 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
63
64
65
66
67 FieldStaticTransform<T> fieldTransform = FieldStaticTransform.compose(
68 date,
69 FieldStaticTransform.of(date, fieldRotation),
70 FieldStaticTransform.of(date, FieldVector3D.getPlusI(field)));
71
72
73 StaticTransform transform = StaticTransform.compose(date.toAbsoluteDate(),
74 StaticTransform.of(date.toAbsoluteDate(), fieldRotation.toRotation()),
75 StaticTransform.of(date.toAbsoluteDate(), Vector3D.PLUS_I));
76 FieldStaticTransform<T> fieldTransform2 = FieldStaticTransform.of(date, transform);
77
78
79 verifyTransform(field, fieldRotation, fieldTransform);
80 verifyTransform(field, fieldRotation, fieldTransform2);
81 }
82
83
84 private <T extends CalculusFieldElement<T>> void verifyTransform(final Field<T> field,
85 final FieldRotation<T> rotation,
86 final FieldStaticTransform<T> transform) {
87
88 final T zero = field.getZero();
89 final T one = field.getOne();
90
91
92 FieldStaticTransform<T> identity = FieldStaticTransform
93 .compose(new FieldAbsoluteDate<>(field, transform.getDate()), transform, transform.getInverse());
94
95
96 double tol = 1e-15;
97 FieldVector3D<T> u = transform.transformPosition(new FieldVector3D<>(one, one, one));
98 FieldVector3D<T> v = new FieldVector3D<>(zero, one, one);
99 MatcherAssert.assertThat(u.toVector3D(), OrekitMatchers.vectorCloseTo(v.toVector3D(), tol));
100 FieldVector3D<T> w = transform.transformVector(new FieldVector3D<>(zero.newInstance(1), zero.newInstance(2), zero.newInstance(3)));
101 FieldVector3D<T> x = new FieldVector3D<>(zero.newInstance(-2), zero.newInstance(1), zero.newInstance(3));
102 MatcherAssert.assertThat(w.toVector3D(), OrekitMatchers.vectorCloseTo(x.toVector3D(), tol));
103 MatcherAssert.assertThat(transform.getTranslation().toVector3D(),
104 OrekitMatchers.vectorCloseTo(Vector3D.MINUS_J, tol));
105 MatcherAssert.assertThat(transform.getRotation().getAngle().getReal(),
106 CoreMatchers.is(rotation.getAngle().getReal()));
107 MatcherAssert.assertThat(transform.getRotation().getAxis(RotationConvention.VECTOR_OPERATOR).toVector3D(),
108 CoreMatchers.is(rotation.getAxis(RotationConvention.VECTOR_OPERATOR).toVector3D()));
109 MatcherAssert.assertThat(
110 identity.transformPosition(u).toVector3D(),
111 OrekitMatchers.vectorCloseTo(u.toVector3D(), tol));
112 MatcherAssert.assertThat(
113 identity.transformVector(u).toVector3D(),
114 OrekitMatchers.vectorCloseTo(u.toVector3D(), tol));
115
116 FieldVector3D<T> p1 = new FieldVector3D<>(zero.newInstance(42.1e6), zero.newInstance(42.1e6), zero.newInstance(42.1e6));
117 FieldVector3D<T> d = new FieldVector3D<>(zero.newInstance(-42e6), zero.newInstance(42e6), zero.newInstance(-42e6));
118 FieldLine<T> line = new FieldLine<>(p1, p1.add(d), 0);
119 FieldLine<T> actualLine = transform.transformLine(line);
120 MatcherAssert.assertThat(
121 actualLine.getDirection().toVector3D(),
122 OrekitMatchers.vectorCloseTo(transform.transformVector(d).normalize().toVector3D(), 44));
123
124 FieldVector3D<T> expectedOrigin = new FieldVector3D<>(
125 zero.newInstance(-56133332.666666666), zero.newInstance(28066666.333333333), zero.newInstance(28066666.333333333));
126 MatcherAssert.assertThat(
127 actualLine.getOrigin().toVector3D(),
128 OrekitMatchers.vectorCloseTo(expectedOrigin.toVector3D(), 33));
129 MatcherAssert.assertThat(
130 actualLine.getTolerance(),
131 CoreMatchers.is(line.getTolerance()));
132 }
133
134 @Test
135 void testOf() {
136
137 final AbsoluteDate expectedDate = AbsoluteDate.ARBITRARY_EPOCH;
138 final Vector3D expectedTranslation = new Vector3D(1., 2., 3.);
139 final Rotation rotation = new Rotation(Vector3D.MINUS_J, Vector3D.PLUS_I);
140 final ComplexField field = ComplexField.getInstance();
141 final Complex imaginaryComplex = Complex.I;
142 final FieldAbsoluteDate<Complex> expectedFieldDate = new FieldAbsoluteDate<>(field, expectedDate)
143 .shiftedBy(imaginaryComplex);
144 final FieldVector3D<Complex> fieldTranslation = new FieldVector3D<>(field, expectedTranslation);
145 final FieldRotation<Complex> fieldRotation = new FieldRotation<>(field, rotation);
146
147 final FieldStaticTransform<Complex> staticTransform = FieldStaticTransform.of(expectedFieldDate,
148 fieldTranslation, fieldRotation);
149
150 Assertions.assertEquals(expectedDate, staticTransform.getDate());
151 final FieldAbsoluteDate<Complex> actualFieldDate = staticTransform.getFieldDate();
152 Assertions.assertEquals(staticTransform.getDate(), actualFieldDate.toAbsoluteDate());
153 Assertions.assertEquals(Complex.ZERO, actualFieldDate.durationFrom(expectedFieldDate));
154 Assertions.assertEquals(expectedTranslation, staticTransform.getTranslation().toVector3D());
155 Assertions.assertEquals(0., Rotation.distance(fieldRotation.toRotation(),
156 staticTransform.getRotation().toRotation()));
157 }
158
159 @Test
160 void testGetStaticInverse() {
161
162 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
163 final Vector3D translation = new Vector3D(1., 2., 3.);
164 final Rotation rotation = new Rotation(Vector3D.MINUS_J, Vector3D.PLUS_I);
165 final ComplexField field = ComplexField.getInstance();
166 final FieldAbsoluteDate<Complex> fieldDate = new FieldAbsoluteDate<>(field, date);
167 final FieldVector3D<Complex> fieldTranslation = new FieldVector3D<>(field, translation);
168 final FieldRotation<Complex> fieldRotation = new FieldRotation<>(field, rotation);
169 final FieldStaticTransform<Complex> staticTransform = FieldStaticTransform.of(fieldDate, fieldTranslation,
170 fieldRotation);
171
172 final FieldStaticTransform<Complex> actualInverseStaticTransform = staticTransform.getStaticInverse();
173
174 final FieldStaticTransform<Complex> expectedInverseStaticTransform = staticTransform.getInverse();
175 Assertions.assertEquals(expectedInverseStaticTransform.getDate(), actualInverseStaticTransform.getDate());
176 Assertions.assertEquals(expectedInverseStaticTransform.getFieldDate(),
177 actualInverseStaticTransform.getFieldDate());
178 Assertions.assertEquals(expectedInverseStaticTransform.getTranslation().toVector3D(),
179 actualInverseStaticTransform.getTranslation().toVector3D());
180 Assertions.assertEquals(0., Rotation.distance(expectedInverseStaticTransform.getRotation().toRotation(),
181 actualInverseStaticTransform.getRotation().toRotation()));
182 }
183
184 @Test
185 void testGetFieldDate() {
186
187 final AbsoluteDate arbitraryEpoch = AbsoluteDate.ARBITRARY_EPOCH;
188 final TestFieldStaticTransform testFieldStaticTransform = new TestFieldStaticTransform(arbitraryEpoch);
189
190 final FieldAbsoluteDate<Complex> actualFieldDate = testFieldStaticTransform.getFieldDate();
191
192 Assertions.assertEquals(testFieldStaticTransform.getDate(), actualFieldDate.toAbsoluteDate());
193 }
194
195 @Test
196 void testGetIdentity() {
197
198 final Field<Binary64> field = Binary64Field.getInstance();
199 final FieldStaticTransform<Binary64> identity = FieldStaticTransform.getIdentity(field);
200 final Vector3D vector3D = new Vector3D(3., 2.,1.);
201 final FieldVector3D<Binary64> fieldVector3D = new FieldVector3D<>(field, vector3D);
202
203 Assertions.assertEquals(identity.getFieldDate().toAbsoluteDate(), identity.getDate());
204 Assertions.assertEquals(identity.transformVector(vector3D), identity.getRotation().applyTo(vector3D));
205 Assertions.assertEquals(identity.transformPosition(vector3D),
206 identity.getRotation().applyTo(vector3D).add(identity.getTranslation().toVector3D()));
207 Assertions.assertEquals(identity.transformVector(fieldVector3D), identity.getRotation().applyTo(fieldVector3D));
208 Assertions.assertEquals(identity.transformPosition(fieldVector3D),
209 identity.getRotation().applyTo(fieldVector3D).add(identity.getTranslation()));
210 Assertions.assertEquals(identity, identity.getInverse());
211 Assertions.assertEquals(identity, identity.getStaticInverse());
212 }
213
214 private static class TestFieldStaticTransform implements FieldStaticTransform<Complex> {
215
216 private final AbsoluteDate date;
217
218 TestFieldStaticTransform(final AbsoluteDate date) {
219 this.date = date;
220 }
221
222 @Override
223 public FieldVector3D<Complex> getTranslation() {
224 return FieldVector3D.getPlusI(ComplexField.getInstance());
225 }
226
227 @Override
228 public FieldRotation<Complex> getRotation() {
229 return null;
230 }
231
232 @Override
233 public FieldStaticTransform<Complex> getInverse() {
234 return null;
235 }
236
237 @Override
238 public AbsoluteDate getDate() {
239 return date;
240 }
241 }
242
243 }