1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 package org.hipparchus.geometry.euclidean.threed;
24
25 import org.hipparchus.analysis.differentiation.DSFactory;
26 import org.hipparchus.analysis.differentiation.DerivativeStructure;
27 import org.hipparchus.exception.MathIllegalArgumentException;
28 import org.hipparchus.exception.MathIllegalStateException;
29 import org.hipparchus.exception.MathRuntimeException;
30 import org.hipparchus.linear.MatrixUtils;
31 import org.hipparchus.linear.RealMatrix;
32 import org.hipparchus.random.UnitSphereRandomVectorGenerator;
33 import org.hipparchus.random.Well1024a;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.MathUtils;
36 import org.junit.Assert;
37 import org.junit.Test;
38
39
40 public class FieldRotationDSTest {
41
42 @Test
43 public void testIdentity() {
44
45 FieldRotation<DerivativeStructure> r = createRotation(1, 0, 0, 0, false);
46 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
47 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
48 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
49 checkAngle(r.getAngle(), 0);
50
51 r = createRotation(-1, 0, 0, 0, false);
52 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
53 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
54 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
55 checkAngle(r.getAngle(), 0);
56
57 r = createRotation(42, 0, 0, 0, true);
58 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
59 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
60 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
61 checkAngle(r.getAngle(), 0);
62
63 }
64
65 @Test
66 public void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
67
68 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10),
69 createAngle(2 * FastMath.PI / 3) ,
70 RotationConvention.VECTOR_OPERATOR);
71 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
72 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1));
73 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0));
74 double s = 1 / FastMath.sqrt(3);
75 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector( s, s, s));
76 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-s, -s, -s));
77 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
78
79 try {
80 new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0),
81 createAngle(2 * FastMath.PI / 3),
82 RotationConvention.VECTOR_OPERATOR);
83 Assert.fail("an exception should have been thrown");
84 } catch (MathIllegalArgumentException e) {
85 }
86
87 r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1),
88 createAngle(1.5 * FastMath.PI),
89 RotationConvention.VECTOR_OPERATOR);
90 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, -1));
91 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, +1));
92 checkAngle(r.getAngle(), 0.5 * FastMath.PI);
93
94 r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0),
95 createAngle(FastMath.PI),
96 RotationConvention.VECTOR_OPERATOR);
97 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, +1, 0));
98 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, -1, 0));
99 checkAngle(r.getAngle(), FastMath.PI);
100
101 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
102 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
103
104 }
105
106 @Test
107 public void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
108
109 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10),
110 createAngle(2 * FastMath.PI / 3) ,
111 RotationConvention.FRAME_TRANSFORM);
112 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
113 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
114 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
115 double s = 1 / FastMath.sqrt(3);
116 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector( s, s, s));
117 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(-s, -s, -s));
118 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
119
120 try {
121 new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0),
122 createAngle(2 * FastMath.PI / 3),
123 RotationConvention.FRAME_TRANSFORM);
124 Assert.fail("an exception should have been thrown");
125 } catch (MathIllegalArgumentException e) {
126 }
127
128 r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1),
129 createAngle(1.5 * FastMath.PI),
130 RotationConvention.FRAME_TRANSFORM);
131 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, -1));
132 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, +1));
133 checkAngle(r.getAngle(), 0.5 * FastMath.PI);
134
135 r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0),
136 createAngle(FastMath.PI),
137 RotationConvention.FRAME_TRANSFORM);
138 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, +1, 0));
139 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, -1, 0));
140 checkAngle(r.getAngle(), FastMath.PI);
141
142 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
143 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
144
145 }
146
147 @Test
148 public void testRevert() {
149 double a = 0.001;
150 double b = 0.36;
151 double c = 0.48;
152 double d = 0.8;
153 FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
154 double a2 = a * a;
155 double b2 = b * b;
156 double c2 = c * c;
157 double d2 = d * d;
158 double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
159 Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
160 Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
161 Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
162 Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
163 Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
164 Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
165 Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
166 Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
167 Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
168 Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
169 Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
170 Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
171 Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
172 Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
173 Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
174 Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
175 FieldRotation<DerivativeStructure> reverted = r.revert();
176 FieldRotation<DerivativeStructure> rrT = r.applyTo(reverted);
177 checkRotationDS(rrT, 1, 0, 0, 0);
178 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
179 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
180 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
181 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
182 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
183 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
184 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
185 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
186 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
187 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
188 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
189 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
190 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
191 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
192 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
193 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
194 FieldRotation<DerivativeStructure> rTr = reverted.applyTo(r);
195 checkRotationDS(rTr, 1, 0, 0, 0);
196 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
197 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
198 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
199 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
200 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
201 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
202 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
203 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
204 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
205 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
206 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
207 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
208 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
209 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
210 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
211 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
212 Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
213 Assert.assertEquals(-1,
214 FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
215 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
216 1.0e-15);
217 }
218
219 @Test
220 public void testRevertVectorOperator() {
221 double a = 0.001;
222 double b = 0.36;
223 double c = 0.48;
224 double d = 0.8;
225 FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
226 double a2 = a * a;
227 double b2 = b * b;
228 double c2 = c * c;
229 double d2 = d * d;
230 double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
231 Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
232 Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
233 Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
234 Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
235 Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
236 Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
237 Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
238 Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
239 Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
240 Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
241 Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
242 Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
243 Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
244 Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
245 Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
246 Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
247 FieldRotation<DerivativeStructure> reverted = r.revert();
248 FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
249 checkRotationDS(rrT, 1, 0, 0, 0);
250 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
251 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
252 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
253 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
254 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
255 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
256 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
257 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
258 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
259 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
260 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
261 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
262 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
263 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
264 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
265 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
266 FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
267 checkRotationDS(rTr, 1, 0, 0, 0);
268 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
269 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
270 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
271 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
272 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
273 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
274 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
275 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
276 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
277 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
278 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
279 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
280 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
281 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
282 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
283 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
284 Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
285 Assert.assertEquals(-1,
286 FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
287 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
288 1.0e-15);
289 }
290
291 @Test
292 public void testRevertFrameTransform() {
293 double a = 0.001;
294 double b = 0.36;
295 double c = 0.48;
296 double d = 0.8;
297 FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
298 double a2 = a * a;
299 double b2 = b * b;
300 double c2 = c * c;
301 double d2 = d * d;
302 double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
303 Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
304 Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
305 Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
306 Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
307 Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
308 Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
309 Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
310 Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
311 Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
312 Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
313 Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
314 Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
315 Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
316 Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
317 Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
318 Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
319 FieldRotation<DerivativeStructure> reverted = r.revert();
320 FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
321 checkRotationDS(rrT, 1, 0, 0, 0);
322 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
323 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
324 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
325 Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
326 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
327 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
328 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
329 Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
330 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
331 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
332 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
333 Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
334 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
335 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
336 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
337 Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
338 FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
339 checkRotationDS(rTr, 1, 0, 0, 0);
340 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
341 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
342 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
343 Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
344 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
345 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
346 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
347 Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
348 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
349 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
350 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
351 Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
352 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
353 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
354 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
355 Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
356 Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
357 Assert.assertEquals(-1,
358 FieldVector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
359 reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
360 1.0e-15);
361 }
362
363 @Test
364 public void testVectorOnePair() throws MathRuntimeException {
365
366 FieldVector3D<DerivativeStructure> u = createVector(3, 2, 1);
367 FieldVector3D<DerivativeStructure> v = createVector(-4, 2, 2);
368 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(u, v);
369 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
370
371 checkAngle(new FieldRotation<DerivativeStructure>(u, u.negate()).getAngle(), FastMath.PI);
372
373 try {
374 new FieldRotation<DerivativeStructure>(u, createVector(0, 0, 0));
375 Assert.fail("an exception should have been thrown");
376 } catch (MathRuntimeException e) {
377
378 }
379
380 }
381
382 @Test
383 public void testVectorTwoPairs() throws MathRuntimeException {
384
385 FieldVector3D<DerivativeStructure> u1 = createVector(3, 0, 0);
386 FieldVector3D<DerivativeStructure> u2 = createVector(0, 5, 0);
387 FieldVector3D<DerivativeStructure> v1 = createVector(0, 0, 2);
388 FieldVector3D<DerivativeStructure> v2 = createVector(-2, 0, 2);
389 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(u1, u2, v1, v2);
390 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
391 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0));
392
393 r = new FieldRotation<DerivativeStructure>(u1, u2, u1.negate(), u2.negate());
394 FieldVector3D<DerivativeStructure> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
395 if (FieldVector3D.dotProduct(axis, createVector(0, 0, 1)).getReal() > 0) {
396 checkVector(axis, createVector(0, 0, 1));
397 } else {
398 checkVector(axis, createVector(0, 0, -1));
399 }
400 checkAngle(r.getAngle(), FastMath.PI);
401
402 double sqrt = FastMath.sqrt(2) / 2;
403 r = new FieldRotation<DerivativeStructure>(createVector(1, 0, 0), createVector(0, 1, 0),
404 createVector(0.5, 0.5, sqrt),
405 createVector(0.5, 0.5, -sqrt));
406 checkRotationDS(r, sqrt, 0.5, 0.5, 0);
407
408 r = new FieldRotation<DerivativeStructure>(u1, u2, u1, FieldVector3D.crossProduct(u1, u2));
409 checkRotationDS(r, sqrt, -sqrt, 0, 0);
410
411 checkRotationDS(new FieldRotation<DerivativeStructure>(u1, u2, u1, u2), 1, 0, 0, 0);
412
413 try {
414 new FieldRotation<DerivativeStructure>(u1, u2, createVector(0, 0, 0), v2);
415 Assert.fail("an exception should have been thrown");
416 } catch (MathRuntimeException e) {
417
418 }
419
420 }
421
422 @Test
423 public void testMatrix()
424 throws MathIllegalArgumentException {
425
426 try {
427 createRotation(new double[][] {
428 { 0.0, 1.0, 0.0 },
429 { 1.0, 0.0, 0.0 }
430 }, 1.0e-7);
431 Assert.fail("Expecting MathIllegalArgumentException");
432 } catch (MathIllegalArgumentException nrme) {
433
434 }
435
436 try {
437 createRotation(new double[][] {
438 { 0.445888, 0.797184, -0.407040 },
439 { 0.821760, -0.184320, 0.539200 },
440 { -0.354816, 0.574912, 0.737280 }
441 }, 1.0e-7);
442 Assert.fail("Expecting MathIllegalArgumentException");
443 } catch (MathIllegalArgumentException nrme) {
444
445 }
446
447 try {
448 createRotation(new double[][] {
449 { 0.4, 0.8, -0.4 },
450 { -0.4, 0.6, 0.7 },
451 { 0.8, -0.2, 0.5 }
452 }, 1.0e-15);
453 Assert.fail("Expecting MathIllegalArgumentException");
454 } catch (MathIllegalArgumentException nrme) {
455
456 }
457
458 checkRotationDS(createRotation(new double[][] {
459 { 0.445888, 0.797184, -0.407040 },
460 { -0.354816, 0.574912, 0.737280 },
461 { 0.821760, -0.184320, 0.539200 }
462 }, 1.0e-10),
463 0.8, 0.288, 0.384, 0.36);
464
465 checkRotationDS(createRotation(new double[][] {
466 { 0.539200, 0.737280, 0.407040 },
467 { 0.184320, -0.574912, 0.797184 },
468 { 0.821760, -0.354816, -0.445888 }
469 }, 1.0e-10),
470 0.36, 0.8, 0.288, 0.384);
471
472 checkRotationDS(createRotation(new double[][] {
473 { -0.445888, 0.797184, -0.407040 },
474 { 0.354816, 0.574912, 0.737280 },
475 { 0.821760, 0.184320, -0.539200 }
476 }, 1.0e-10),
477 0.384, 0.36, 0.8, 0.288);
478
479 checkRotationDS(createRotation(new double[][] {
480 { -0.539200, 0.737280, 0.407040 },
481 { -0.184320, -0.574912, 0.797184 },
482 { 0.821760, 0.354816, 0.445888 }
483 }, 1.0e-10),
484 0.288, 0.384, 0.36, 0.8);
485
486 double[][] m1 = { { 0.0, 1.0, 0.0 },
487 { 0.0, 0.0, 1.0 },
488 { 1.0, 0.0, 0.0 } };
489 FieldRotation<DerivativeStructure> r = createRotation(m1, 1.0e-7);
490 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
491 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
492 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
493
494 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
495 { 0.48293, 0.78164, -0.39474 },
496 { 0.27296, 0.29396, 0.91602 } };
497 r = createRotation(m2, 1.0e-12);
498
499 DerivativeStructure[][] m3 = r.getMatrix();
500 double d00 = m2[0][0] - m3[0][0].getReal();
501 double d01 = m2[0][1] - m3[0][1].getReal();
502 double d02 = m2[0][2] - m3[0][2].getReal();
503 double d10 = m2[1][0] - m3[1][0].getReal();
504 double d11 = m2[1][1] - m3[1][1].getReal();
505 double d12 = m2[1][2] - m3[1][2].getReal();
506 double d20 = m2[2][0] - m3[2][0].getReal();
507 double d21 = m2[2][1] - m3[2][1].getReal();
508 double d22 = m2[2][2] - m3[2][2].getReal();
509
510 Assert.assertTrue(FastMath.abs(d00) < 6.0e-6);
511 Assert.assertTrue(FastMath.abs(d01) < 6.0e-6);
512 Assert.assertTrue(FastMath.abs(d02) < 6.0e-6);
513 Assert.assertTrue(FastMath.abs(d10) < 6.0e-6);
514 Assert.assertTrue(FastMath.abs(d11) < 6.0e-6);
515 Assert.assertTrue(FastMath.abs(d12) < 6.0e-6);
516 Assert.assertTrue(FastMath.abs(d20) < 6.0e-6);
517 Assert.assertTrue(FastMath.abs(d21) < 6.0e-6);
518 Assert.assertTrue(FastMath.abs(d22) < 6.0e-6);
519
520 Assert.assertTrue(FastMath.abs(d00) > 4.0e-7);
521 Assert.assertTrue(FastMath.abs(d01) > 4.0e-7);
522 Assert.assertTrue(FastMath.abs(d02) > 4.0e-7);
523 Assert.assertTrue(FastMath.abs(d10) > 4.0e-7);
524 Assert.assertTrue(FastMath.abs(d11) > 4.0e-7);
525 Assert.assertTrue(FastMath.abs(d12) > 4.0e-7);
526 Assert.assertTrue(FastMath.abs(d20) > 4.0e-7);
527 Assert.assertTrue(FastMath.abs(d21) > 4.0e-7);
528 Assert.assertTrue(FastMath.abs(d22) > 4.0e-7);
529
530 for (int i = 0; i < 3; ++i) {
531 for (int j = 0; j < 3; ++j) {
532 double m3tm3 = m3[i][0].getReal() * m3[j][0].getReal() +
533 m3[i][1].getReal() * m3[j][1].getReal() +
534 m3[i][2].getReal() * m3[j][2].getReal();
535 if (i == j) {
536 Assert.assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10);
537 } else {
538 Assert.assertTrue(FastMath.abs(m3tm3) < 1.0e-10);
539 }
540 }
541 }
542
543 checkVector(r.applyTo(createVector(1, 0, 0)),
544 new FieldVector3D<DerivativeStructure>(m3[0][0], m3[1][0], m3[2][0]));
545 checkVector(r.applyTo(createVector(0, 1, 0)),
546 new FieldVector3D<DerivativeStructure>(m3[0][1], m3[1][1], m3[2][1]));
547 checkVector(r.applyTo(createVector(0, 0, 1)),
548 new FieldVector3D<DerivativeStructure>(m3[0][2], m3[1][2], m3[2][2]));
549
550 double[][] m4 = { { 1.0, 0.0, 0.0 },
551 { 0.0, -1.0, 0.0 },
552 { 0.0, 0.0, -1.0 } };
553 r = createRotation(m4, 1.0e-7);
554 checkAngle(r.getAngle(), FastMath.PI);
555
556 try {
557 double[][] m5 = { { 0.0, 0.0, 1.0 },
558 { 0.0, 1.0, 0.0 },
559 { 1.0, 0.0, 0.0 } };
560 r = createRotation(m5, 1.0e-7);
561 Assert.fail("got " + r + ", should have caught an exception");
562 } catch (MathIllegalArgumentException e) {
563
564 }
565
566 }
567
568 @Test
569 public void testAngles()
570 throws MathIllegalStateException {
571
572 DSFactory factory = new DSFactory(3, 1);
573 for (RotationConvention convention : RotationConvention.values()) {
574 RotationOrder[] CardanOrders = {
575 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
576 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
577 };
578
579 for (int i = 0; i < CardanOrders.length; ++i) {
580 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
581 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
582 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
583 FieldRotation<DerivativeStructure> r =
584 new FieldRotation<DerivativeStructure>(CardanOrders[i],
585 convention,
586 factory.variable(0, alpha1),
587 factory.variable(1, alpha2),
588 factory.variable(2, alpha3));
589 DerivativeStructure[] angles = r.getAngles(CardanOrders[i], convention);
590 checkAngle(angles[0], alpha1);
591 checkAngle(angles[1], alpha2);
592 checkAngle(angles[2], alpha3);
593 }
594 }
595 }
596 }
597
598 RotationOrder[] EulerOrders = {
599 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
600 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
601 };
602
603 for (int i = 0; i < EulerOrders.length; ++i) {
604 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
605 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
606 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
607 FieldRotation<DerivativeStructure> r =
608 new FieldRotation<DerivativeStructure>(EulerOrders[i],
609 convention,
610 factory.variable(0, alpha1),
611 factory.variable(1, alpha2),
612 factory.variable(2, alpha3));
613 DerivativeStructure[] angles = r.getAngles(EulerOrders[i], convention);
614 checkAngle(angles[0], alpha1);
615 checkAngle(angles[1], alpha2);
616 checkAngle(angles[2], alpha3);
617 }
618 }
619 }
620 }
621 }
622
623 }
624
625 @Test
626 public void testSingularities() {
627
628 DSFactory factory = new DSFactory(3, 1);
629 for (RotationConvention convention : RotationConvention.values()) {
630 RotationOrder[] CardanOrders = {
631 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
632 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
633 };
634
635 double[] singularCardanAngle = {
636 -FastMath.PI / 2, -FastMath.PI / 2 + 1.0e-12, -FastMath.PI / 2 + 1.0e-10,
637 FastMath.PI / 2 - 1.0e-10, FastMath.PI / 2 - 1.0e-12, FastMath.PI / 2
638 };
639 for (int i = 0; i < CardanOrders.length; ++i) {
640 for (int j = 0; j < singularCardanAngle.length; ++j) {
641 FieldRotation<DerivativeStructure> r =
642 new FieldRotation<DerivativeStructure>(CardanOrders[i],
643 convention,
644 factory.variable(0, 0.1),
645 factory.variable(1, singularCardanAngle[j]),
646 factory.variable(2, 0.3));
647 Assert.assertEquals(singularCardanAngle[j], r.getAngles(CardanOrders[i], convention)[1].getReal(), 4.5e-16);
648 }
649 }
650
651 RotationOrder[] EulerOrders = {
652 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
653 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
654 };
655
656 double[] singularEulerAngle = { 0, 1.0e-12, 1.0e-10, FastMath.PI - 1.0e-10, FastMath.PI - 1.0e-12, FastMath.PI };
657 for (int i = 0; i < EulerOrders.length; ++i) {
658 for (int j = 0; j < singularEulerAngle.length; ++j) {
659 FieldRotation<DerivativeStructure> r =
660 new FieldRotation<DerivativeStructure>(EulerOrders[i],
661 convention,
662 factory.variable(0, 0.1),
663 factory.variable(1, singularEulerAngle[j]),
664 factory.variable(2, 0.3));
665 r.getAngles(EulerOrders[i], convention);
666 Assert.assertEquals(singularEulerAngle[j], r.getAngles(EulerOrders[i], convention)[1].getReal(), 1.0e-24);
667 }
668 }
669
670 }
671 }
672
673 @Test
674 public void testQuaternion() throws MathIllegalArgumentException {
675
676 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
677 createAngle(1.7),
678 RotationConvention.VECTOR_OPERATOR);
679 double n = 23.5;
680 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(r1.getQ0().multiply(n), r1.getQ1().multiply(n),
681 r1.getQ2().multiply(n), r1.getQ3().multiply(n),
682 true);
683 for (double x = -0.9; x < 0.9; x += 0.2) {
684 for (double y = -0.9; y < 0.9; y += 0.2) {
685 for (double z = -0.9; z < 0.9; z += 0.2) {
686 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
687 checkVector(r2.applyTo(u), r1.applyTo(u));
688 }
689 }
690 }
691
692 r1 = createRotation(0.288, 0.384, 0.36, 0.8, false);
693 checkRotationDS(r1,
694 -r1.getQ0().getReal(), -r1.getQ1().getReal(),
695 -r1.getQ2().getReal(), -r1.getQ3().getReal());
696 Assert.assertEquals(0.288, r1.toRotation().getQ0(), 1.0e-15);
697 Assert.assertEquals(0.384, r1.toRotation().getQ1(), 1.0e-15);
698 Assert.assertEquals(0.36, r1.toRotation().getQ2(), 1.0e-15);
699 Assert.assertEquals(0.8, r1.toRotation().getQ3(), 1.0e-15);
700
701 }
702
703 @Test
704 public void testApplyToRotation() throws MathIllegalArgumentException {
705
706 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
707 createAngle(1.7),
708 RotationConvention.VECTOR_OPERATOR);
709 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
710 createAngle(0.3),
711 RotationConvention.VECTOR_OPERATOR);
712 FieldRotation<DerivativeStructure> r3 = r2.applyTo(r1);
713 FieldRotation<DerivativeStructure> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
714 r1.getQ1().getReal(),
715 r1.getQ2().getReal(),
716 r1.getQ3().getReal(),
717 false));
718
719 for (double x = -0.9; x < 0.9; x += 0.2) {
720 for (double y = -0.9; y < 0.9; y += 0.2) {
721 for (double z = -0.9; z < 0.9; z += 0.2) {
722 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
723 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
724 checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
725 }
726 }
727 }
728
729 }
730
731 @Test
732 public void testComposeVectorOperator() throws MathIllegalArgumentException {
733
734 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
735 createAngle(1.7),
736 RotationConvention.VECTOR_OPERATOR);
737 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
738 createAngle(0.3),
739 RotationConvention.VECTOR_OPERATOR);
740 FieldRotation<DerivativeStructure> r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
741 FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
742 r1.getQ1().getReal(),
743 r1.getQ2().getReal(),
744 r1.getQ3().getReal(),
745 false),
746 RotationConvention.VECTOR_OPERATOR);
747
748 for (double x = -0.9; x < 0.9; x += 0.2) {
749 for (double y = -0.9; y < 0.9; y += 0.2) {
750 for (double z = -0.9; z < 0.9; z += 0.2) {
751 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
752 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
753 checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
754 }
755 }
756 }
757
758 }
759
760 @Test
761 public void testComposeFrameTransform() throws MathIllegalArgumentException {
762
763 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
764 createAngle(1.7),
765 RotationConvention.FRAME_TRANSFORM);
766 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
767 createAngle(0.3),
768 RotationConvention.FRAME_TRANSFORM);
769 FieldRotation<DerivativeStructure> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
770 FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
771 r1.getQ1().getReal(),
772 r1.getQ2().getReal(),
773 r1.getQ3().getReal(),
774 false),
775 RotationConvention.FRAME_TRANSFORM);
776
777 for (double x = -0.9; x < 0.9; x += 0.2) {
778 for (double y = -0.9; y < 0.9; y += 0.2) {
779 for (double z = -0.9; z < 0.9; z += 0.2) {
780 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
781 checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
782 checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
783 }
784 }
785 }
786
787 }
788
789 @Test
790 public void testApplyInverseToRotation() throws MathIllegalArgumentException {
791
792 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
793 createAngle(1.7),
794 RotationConvention.VECTOR_OPERATOR);
795 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
796 createAngle(0.3),
797 RotationConvention.VECTOR_OPERATOR);
798 FieldRotation<DerivativeStructure> r3 = r2.applyInverseTo(r1);
799 FieldRotation<DerivativeStructure> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
800 r1.getQ1().getReal(),
801 r1.getQ2().getReal(),
802 r1.getQ3().getReal(),
803 false));
804
805 for (double x = -0.9; x < 0.9; x += 0.2) {
806 for (double y = -0.9; y < 0.9; y += 0.2) {
807 for (double z = -0.9; z < 0.9; z += 0.2) {
808 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
809 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
810 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
811 }
812 }
813 }
814
815 }
816
817 @Test
818 public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
819
820 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
821 createAngle(1.7),
822 RotationConvention.VECTOR_OPERATOR);
823 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
824 createAngle(0.3),
825 RotationConvention.VECTOR_OPERATOR);
826 FieldRotation<DerivativeStructure> r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
827 FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
828 r1.getQ1().getReal(),
829 r1.getQ2().getReal(),
830 r1.getQ3().getReal(),
831 false),
832 RotationConvention.VECTOR_OPERATOR);
833
834 for (double x = -0.9; x < 0.9; x += 0.2) {
835 for (double y = -0.9; y < 0.9; y += 0.2) {
836 for (double z = -0.9; z < 0.9; z += 0.2) {
837 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
838 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
839 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
840 }
841 }
842 }
843
844 }
845
846 @Test
847 public void testComposeInverseframeTransform() throws MathIllegalArgumentException {
848
849 FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
850 createAngle(1.7),
851 RotationConvention.FRAME_TRANSFORM);
852 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
853 createAngle(0.3),
854 RotationConvention.FRAME_TRANSFORM);
855 FieldRotation<DerivativeStructure> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
856 FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
857 r1.getQ1().getReal(),
858 r1.getQ2().getReal(),
859 r1.getQ3().getReal(),
860 false),
861 RotationConvention.FRAME_TRANSFORM);
862
863 for (double x = -0.9; x < 0.9; x += 0.2) {
864 for (double y = -0.9; y < 0.9; y += 0.2) {
865 for (double z = -0.9; z < 0.9; z += 0.2) {
866 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
867 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
868 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
869 }
870 }
871 }
872
873 }
874
875 @Test
876 public void testDoubleVectors() throws MathIllegalArgumentException {
877
878 Well1024a random = new Well1024a(0x180b41cfeeffaf67l);
879 UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random);
880 for (int i = 0; i < 10; ++i) {
881 double[] unit = g.nextVector();
882 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(unit[0], unit[1], unit[2]),
883 createAngle(random.nextDouble()),
884 RotationConvention.VECTOR_OPERATOR);
885
886 for (double x = -0.9; x < 0.9; x += 0.2) {
887 for (double y = -0.9; y < 0.9; y += 0.2) {
888 for (double z = -0.9; z < 0.9; z += 0.2) {
889 FieldVector3D<DerivativeStructure> uds = createVector(x, y, z);
890 FieldVector3D<DerivativeStructure> ruds = r.applyTo(uds);
891 FieldVector3D<DerivativeStructure> rIuds = r.applyInverseTo(uds);
892 Vector3D u = new Vector3D(x, y, z);
893 FieldVector3D<DerivativeStructure> ru = r.applyTo(u);
894 FieldVector3D<DerivativeStructure> rIu = r.applyInverseTo(u);
895 DerivativeStructure[] ruArray = new DerivativeStructure[3];
896 r.applyTo(new double[] { x, y, z}, ruArray);
897 DerivativeStructure[] rIuArray = new DerivativeStructure[3];
898 r.applyInverseTo(new double[] { x, y, z}, rIuArray);
899 checkVector(ruds, ru);
900 checkVector(ruds, new FieldVector3D<DerivativeStructure>(ruArray));
901 checkVector(rIuds, rIu);
902 checkVector(rIuds, new FieldVector3D<DerivativeStructure>(rIuArray));
903 }
904 }
905 }
906 }
907
908 }
909
910 @Test
911 public void testDoubleRotations() throws MathIllegalArgumentException {
912
913 Well1024a random = new Well1024a(0x180b41cfeeffaf67l);
914 UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random);
915 DSFactory factory = new DSFactory(4, 1);
916 for (int i = 0; i < 10; ++i) {
917 double[] unit1 = g.nextVector();
918 Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]),
919 random.nextDouble(), RotationConvention.VECTOR_OPERATOR);
920 FieldRotation<DerivativeStructure> r1Prime = new FieldRotation<DerivativeStructure>(factory.variable(0, r1.getQ0()),
921 factory.variable(1, r1.getQ1()),
922 factory.variable(2, r1.getQ2()),
923 factory.variable(3, r1.getQ3()),
924 false);
925 double[] unit2 = g.nextVector();
926 FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(unit2[0], unit2[1], unit2[2]),
927 createAngle(random.nextDouble()),
928 RotationConvention.VECTOR_OPERATOR);
929
930 FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2);
931 FieldRotation<DerivativeStructure> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
932 FieldRotation<DerivativeStructure> rC = FieldRotation.applyInverseTo(r1, r2);
933 FieldRotation<DerivativeStructure> rD = r1Prime.composeInverse(r2, RotationConvention.VECTOR_OPERATOR);
934
935 for (double x = -0.9; x < 0.9; x += 0.2) {
936 for (double y = -0.9; y < 0.9; y += 0.2) {
937 for (double z = -0.9; z < 0.9; z += 0.2) {
938
939 FieldVector3D<DerivativeStructure> uds = createVector(x, y, z);
940 checkVector(r1Prime.applyTo(uds), FieldRotation.applyTo(r1, uds));
941 checkVector(r1Prime.applyInverseTo(uds), FieldRotation.applyInverseTo(r1, uds));
942 checkVector(rA.applyTo(uds), rB.applyTo(uds));
943 checkVector(rA.applyInverseTo(uds), rB.applyInverseTo(uds));
944 checkVector(rC.applyTo(uds), rD.applyTo(uds));
945 checkVector(rC.applyInverseTo(uds), rD.applyInverseTo(uds));
946
947 }
948 }
949 }
950 }
951
952 }
953
954 @Test
955 public void testDerivatives() {
956
957 double eps = 7.e-16;
958 double kx = 2;
959 double ky = -3;
960 double kz = 5;
961 double n2 = kx * kx + ky * ky + kz * kz;
962 double n = FastMath.sqrt(n2);
963 double theta = 1.7;
964 double cosTheta = FastMath.cos(theta);
965 double sinTheta = FastMath.sin(theta);
966 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(kx, ky, kz),
967 createAngle(theta),
968 RotationConvention.VECTOR_OPERATOR);
969 Vector3D a = new Vector3D(kx / n, ky / n, kz / n);
970
971
972 RealMatrix dadk = MatrixUtils.createRealMatrix(new double[][] {
973 { (ky * ky + kz * kz) / ( n * n2), -kx * ky / ( n * n2), -kx * kz / ( n * n2) },
974 { -kx * ky / ( n * n2), (kx * kx + kz * kz) / ( n * n2), -ky * kz / ( n * n2) },
975 { -kx * kz / ( n * n2), -ky * kz / ( n * n2), (kx * kx + ky * ky) / ( n * n2) }
976 });
977
978 for (double x = -0.9; x < 0.9; x += 0.2) {
979 for (double y = -0.9; y < 0.9; y += 0.2) {
980 for (double z = -0.9; z < 0.9; z += 0.2) {
981 Vector3D u = new Vector3D(x, y, z);
982 FieldVector3D<DerivativeStructure> v = r.applyTo(createVector(x, y, z));
983
984
985 double dot = Vector3D.dotProduct(u, a);
986 Vector3D cross = Vector3D.crossProduct(a, u);
987 double c1 = 1 - cosTheta;
988 double c2 = c1 * dot;
989 Vector3D rt = new Vector3D(cosTheta, u, c2, a, sinTheta, cross);
990 Assert.assertEquals(rt.getX(), v.getX().getReal(), eps);
991 Assert.assertEquals(rt.getY(), v.getY().getReal(), eps);
992 Assert.assertEquals(rt.getZ(), v.getZ().getReal(), eps);
993
994
995
996 RealMatrix dvda = MatrixUtils.createRealMatrix(new double[][] {
997 { c1 * x * a.getX() + c2, c1 * y * a.getX() + sinTheta * z, c1 * z * a.getX() - sinTheta * y },
998 { c1 * x * a.getY() - sinTheta * z, c1 * y * a.getY() + c2, c1 * z * a.getY() + sinTheta * x },
999 { c1 * x * a.getZ() + sinTheta * y, c1 * y * a.getZ() - sinTheta * x, c1 * z * a.getZ() + c2 }
1000 });
1001
1002
1003 RealMatrix dvdk = dvda.multiply(dadk);
1004
1005
1006 Assert.assertEquals(dvdk.getEntry(0, 0), v.getX().getPartialDerivative(1, 0, 0, 0), eps);
1007 Assert.assertEquals(dvdk.getEntry(0, 1), v.getX().getPartialDerivative(0, 1, 0, 0), eps);
1008 Assert.assertEquals(dvdk.getEntry(0, 2), v.getX().getPartialDerivative(0, 0, 1, 0), eps);
1009 Assert.assertEquals(dvdk.getEntry(1, 0), v.getY().getPartialDerivative(1, 0, 0, 0), eps);
1010 Assert.assertEquals(dvdk.getEntry(1, 1), v.getY().getPartialDerivative(0, 1, 0, 0), eps);
1011 Assert.assertEquals(dvdk.getEntry(1, 2), v.getY().getPartialDerivative(0, 0, 1, 0), eps);
1012 Assert.assertEquals(dvdk.getEntry(2, 0), v.getZ().getPartialDerivative(1, 0, 0, 0), eps);
1013 Assert.assertEquals(dvdk.getEntry(2, 1), v.getZ().getPartialDerivative(0, 1, 0, 0), eps);
1014 Assert.assertEquals(dvdk.getEntry(2, 2), v.getZ().getPartialDerivative(0, 0, 1, 0), eps);
1015
1016
1017
1018 Vector3D dvdTheta =
1019 new Vector3D(-sinTheta, u, sinTheta * dot, a, cosTheta, cross);
1020 Assert.assertEquals(dvdTheta.getX(), v.getX().getPartialDerivative(0, 0, 0, 1), eps);
1021 Assert.assertEquals(dvdTheta.getY(), v.getY().getPartialDerivative(0, 0, 0, 1), eps);
1022 Assert.assertEquals(dvdTheta.getZ(), v.getZ().getPartialDerivative(0, 0, 0, 1), eps);
1023
1024 }
1025 }
1026 }
1027 }
1028
1029 @Test
1030 public void testArray() throws MathIllegalArgumentException {
1031
1032 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(2, -3, 5),
1033 createAngle(1.7),
1034 RotationConvention.VECTOR_OPERATOR);
1035
1036 for (double x = -0.9; x < 0.9; x += 0.2) {
1037 for (double y = -0.9; y < 0.9; y += 0.2) {
1038 for (double z = -0.9; z < 0.9; z += 0.2) {
1039 FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
1040 FieldVector3D<DerivativeStructure> v = r.applyTo(u);
1041 DerivativeStructure[] out = new DerivativeStructure[3];
1042 r.applyTo(new DerivativeStructure[] { u.getX(), u.getY(), u.getZ() }, out);
1043 Assert.assertEquals(v.getX().getReal(), out[0].getReal(), 1.0e-10);
1044 Assert.assertEquals(v.getY().getReal(), out[1].getReal(), 1.0e-10);
1045 Assert.assertEquals(v.getZ().getReal(), out[2].getReal(), 1.0e-10);
1046 r.applyInverseTo(out, out);
1047 Assert.assertEquals(u.getX().getReal(), out[0].getReal(), 1.0e-10);
1048 Assert.assertEquals(u.getY().getReal(), out[1].getReal(), 1.0e-10);
1049 Assert.assertEquals(u.getZ().getReal(), out[2].getReal(), 1.0e-10);
1050 }
1051 }
1052 }
1053
1054 }
1055
1056 @Test
1057 public void testApplyInverseTo() throws MathIllegalArgumentException {
1058
1059 DerivativeStructure[] in = new DerivativeStructure[3];
1060 DerivativeStructure[] out = new DerivativeStructure[3];
1061 DerivativeStructure[] rebuilt = new DerivativeStructure[3];
1062 FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
1063 createAngle(1.7),
1064 RotationConvention.VECTOR_OPERATOR);
1065 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
1066 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
1067 FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
1068 FastMath.sin(lambda) * FastMath.cos(phi),
1069 FastMath.sin(phi));
1070 r.applyInverseTo(r.applyTo(u));
1071 checkVector(u, r.applyInverseTo(r.applyTo(u)));
1072 checkVector(u, r.applyTo(r.applyInverseTo(u)));
1073 in[0] = u.getX();
1074 in[1] = u.getY();
1075 in[2] = u.getZ();
1076 r.applyTo(in, out);
1077 r.applyInverseTo(out, rebuilt);
1078 Assert.assertEquals(in[0].getReal(), rebuilt[0].getReal(), 1.0e-12);
1079 Assert.assertEquals(in[1].getReal(), rebuilt[1].getReal(), 1.0e-12);
1080 Assert.assertEquals(in[2].getReal(), rebuilt[2].getReal(), 1.0e-12);
1081 }
1082 }
1083
1084 r = createRotation(1, 0, 0, 0, false);
1085 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
1086 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
1087 FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
1088 FastMath.sin(lambda) * FastMath.cos(phi),
1089 FastMath.sin(phi));
1090 checkVector(u, r.applyInverseTo(r.applyTo(u)));
1091 checkVector(u, r.applyTo(r.applyInverseTo(u)));
1092 }
1093 }
1094
1095 r = new FieldRotation<DerivativeStructure>(createVector(0, 0, 1),
1096 createAngle(FastMath.PI),
1097 RotationConvention.VECTOR_OPERATOR);
1098 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
1099 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
1100 FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
1101 FastMath.sin(lambda) * FastMath.cos(phi),
1102 FastMath.sin(phi));
1103 checkVector(u, r.applyInverseTo(r.applyTo(u)));
1104 checkVector(u, r.applyTo(r.applyInverseTo(u)));
1105 }
1106 }
1107
1108 }
1109
1110 @Test
1111 public void testIssue639() throws MathRuntimeException{
1112 FieldVector3D<DerivativeStructure> u1 = createVector(-1321008684645961.0 / 268435456.0,
1113 -5774608829631843.0 / 268435456.0,
1114 -3822921525525679.0 / 4294967296.0);
1115 FieldVector3D<DerivativeStructure> u2 =createVector( -5712344449280879.0 / 2097152.0,
1116 -2275058564560979.0 / 1048576.0,
1117 4423475992255071.0 / 65536.0);
1118 FieldRotation<DerivativeStructure> rot = new FieldRotation<DerivativeStructure>(u1, u2, createVector(1, 0, 0),createVector(0, 0, 1));
1119 Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0().getReal(), 1.0e-15);
1120 Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1().getReal(), 1.0e-15);
1121 Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2().getReal(), 1.0e-15);
1122 Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3().getReal(), 1.0e-15);
1123 }
1124
1125 @Test
1126 public void testIssue801() throws MathRuntimeException {
1127 FieldVector3D<DerivativeStructure> u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0);
1128 FieldVector3D<DerivativeStructure> u2 = createVector(0.0, 0.0, 1.0);
1129
1130 FieldVector3D<DerivativeStructure> v1 = createVector(0.9999999999999999, 0.0, 0.0);
1131 FieldVector3D<DerivativeStructure> v2 = createVector(0.0, 0.0, -1.0);
1132
1133 FieldRotation<DerivativeStructure> quat = new FieldRotation<DerivativeStructure>(u1, u2, v1, v2);
1134 double q2 = quat.getQ0().getReal() * quat.getQ0().getReal() +
1135 quat.getQ1().getReal() * quat.getQ1().getReal() +
1136 quat.getQ2().getReal() * quat.getQ2().getReal() +
1137 quat.getQ3().getReal() * quat.getQ3().getReal();
1138 Assert.assertEquals(1.0, q2, 1.0e-14);
1139 Assert.assertEquals(0.0, FieldVector3D.angle(v1, quat.applyTo(u1)).getReal(), 1.0e-14);
1140 Assert.assertEquals(0.0, FieldVector3D.angle(v2, quat.applyTo(u2)).getReal(), 1.0e-14);
1141
1142 }
1143
1144 private void checkAngle(DerivativeStructure a1, double a2) {
1145 Assert.assertEquals(a1.getReal(), MathUtils.normalizeAngle(a2, a1.getReal()), 1.0e-10);
1146 }
1147
1148 private void checkRotationDS(FieldRotation<DerivativeStructure> r, double q0, double q1, double q2, double q3) {
1149 FieldRotation<DerivativeStructure> rPrime = createRotation(q0, q1, q2, q3, false);
1150 Assert.assertEquals(0, FieldRotation.distance(r, rPrime).getReal(), 1.0e-12);
1151 }
1152
1153 private FieldRotation<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3,
1154 boolean needsNormalization) {
1155 DSFactory factory = new DSFactory(4, 1);
1156 return new FieldRotation<DerivativeStructure>(factory.variable(0, q0),
1157 factory.variable(1, q1),
1158 factory.variable(2, q2),
1159 factory.variable(3, q3),
1160 needsNormalization);
1161 }
1162
1163 private FieldRotation<DerivativeStructure> createRotation(double[][] m, double threshold) {
1164 DSFactory factory = new DSFactory(4, 1);
1165 DerivativeStructure[][] mds = new DerivativeStructure[m.length][m[0].length];
1166 int index = 0;
1167 for (int i = 0; i < m.length; ++i) {
1168 for (int j = 0; j < m[i].length; ++j) {
1169 mds[i][j] = factory.variable(index, m[i][j]);
1170 index = (index + 1) % 4;
1171 }
1172 }
1173 return new FieldRotation<DerivativeStructure>(mds, threshold);
1174 }
1175
1176 private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z) {
1177 DSFactory factory = new DSFactory(4, 1);
1178 return new FieldVector3D<DerivativeStructure>(factory.constant(x),
1179 factory.constant(y),
1180 factory.constant(z));
1181 }
1182
1183 private FieldVector3D<DerivativeStructure> createAxis(double x, double y, double z) {
1184 DSFactory factory = new DSFactory(4, 1);
1185 return new FieldVector3D<DerivativeStructure>(factory.variable(0, x),
1186 factory.variable(1, y),
1187 factory.variable(2, z));
1188 }
1189
1190 private DerivativeStructure createAngle(double alpha) {
1191 DSFactory factory = new DSFactory(4, 1);
1192 return factory.variable(3, alpha);
1193 }
1194
1195 private void checkVector(FieldVector3D<DerivativeStructure> u, FieldVector3D<DerivativeStructure> v) {
1196 Assert.assertEquals(u.getX().getReal(), v.getX().getReal(), 1.0e-12);
1197 Assert.assertEquals(u.getY().getReal(), v.getY().getReal(), 1.0e-12);
1198 Assert.assertEquals(u.getZ().getReal(), v.getZ().getReal(), 1.0e-12);
1199 }
1200
1201 }