1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.RealFieldElement;
22 import org.hipparchus.analysis.differentiation.DSFactory;
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.exception.LocalizedCoreFormats;
26 import org.hipparchus.exception.MathIllegalArgumentException;
27 import org.hipparchus.exception.MathRuntimeException;
28 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
29 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
30 import org.hipparchus.geometry.euclidean.threed.Rotation;
31 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
32 import org.hipparchus.geometry.euclidean.threed.Vector3D;
33 import org.hipparchus.linear.DecompositionSolver;
34 import org.hipparchus.linear.MatrixUtils;
35 import org.hipparchus.linear.QRDecomposition;
36 import org.hipparchus.linear.RealMatrix;
37 import org.hipparchus.linear.RealVector;
38 import org.hipparchus.util.FastMath;
39 import org.hipparchus.util.MathArrays;
40 import org.orekit.errors.OrekitException;
41 import org.orekit.errors.OrekitMessages;
42 import org.orekit.time.TimeShiftable;
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57 public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {
58
59
60
61
62 public static final AngularCoordinates IDENTITY =
63 new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
64
65
66 private static final long serialVersionUID = 20140414L;
67
68
69 private final Rotation rotation;
70
71
72 private final Vector3D rotationRate;
73
74
75 private final Vector3D rotationAcceleration;
76
77
78
79
80 public AngularCoordinates() {
81 this(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
82 }
83
84
85
86
87
88 public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
89 this(rotation, rotationRate, Vector3D.ZERO);
90 }
91
92
93
94
95
96
97 public AngularCoordinates(final Rotation rotation,
98 final Vector3D rotationRate, final Vector3D rotationAcceleration) {
99 this.rotation = rotation;
100 this.rotationRate = rotationRate;
101 this.rotationAcceleration = rotationAcceleration;
102 }
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124 public AngularCoordinates(final PVCoordinates#PVCoordinates">PVCoordinates u1, final PVCoordinates u2,
125 final PVCoordinates#PVCoordinates">PVCoordinates v1, final PVCoordinates v2,
126 final double tolerance) {
127
128 try {
129
130 rotation = new Rotation(u1.getPosition(), u2.getPosition(),
131 v1.getPosition(), v2.getPosition());
132
133
134
135
136 final Vector3D ru1Dot = rotation.applyTo(u1.getVelocity());
137 final Vector3D ru2Dot = rotation.applyTo(u2.getVelocity());
138 rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
139 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
140 tolerance);
141
142
143
144
145 final Vector3D ru1DotDot = rotation.applyTo(u1.getAcceleration());
146 final Vector3D oDotv1 = Vector3D.crossProduct(rotationRate, v1.getVelocity());
147 final Vector3D oov1 = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v1.getPosition()));
148 final Vector3D c1 = new Vector3D(1, ru1DotDot, -2, oDotv1, -1, oov1, -1, v1.getAcceleration());
149 final Vector3D ru2DotDot = rotation.applyTo(u2.getAcceleration());
150 final Vector3D oDotv2 = Vector3D.crossProduct(rotationRate, v2.getVelocity());
151 final Vector3D oov2 = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v2.getPosition()));
152 final Vector3D c2 = new Vector3D(1, ru2DotDot, -2, oDotv2, -1, oov2, -1, v2.getAcceleration());
153 rotationAcceleration = inverseCrossProducts(v1.getPosition(), c1, v2.getPosition(), c2, tolerance);
154
155 } catch (MathRuntimeException mrte) {
156 throw new OrekitException(mrte);
157 }
158
159 }
160
161
162
163
164
165
166
167
168
169
170
171
172
173 public AngularCoordinates(final PVCoordinatesl#PVCoordinates">PVCoordinates u, final PVCoordinates v) {
174 this(new FieldRotation<>(u.toDerivativeStructureVector(2),
175 v.toDerivativeStructureVector(2)));
176 }
177
178
179
180
181
182
183
184
185 public AngularCoordinates(final FieldRotation<DerivativeStructure> r) {
186
187 final double q0 = r.getQ0().getReal();
188 final double q1 = r.getQ1().getReal();
189 final double q2 = r.getQ2().getReal();
190 final double q3 = r.getQ3().getReal();
191
192 rotation = new Rotation(q0, q1, q2, q3, false);
193 if (r.getQ0().getOrder() >= 1) {
194 final double q0Dot = r.getQ0().getPartialDerivative(1);
195 final double q1Dot = r.getQ1().getPartialDerivative(1);
196 final double q2Dot = r.getQ2().getPartialDerivative(1);
197 final double q3Dot = r.getQ3().getPartialDerivative(1);
198 rotationRate =
199 new Vector3D(2 * MathArrays.linearCombination(-q1, q0Dot, q0, q1Dot, q3, q2Dot, -q2, q3Dot),
200 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot, q0, q2Dot, q1, q3Dot),
201 2 * MathArrays.linearCombination(-q3, q0Dot, q2, q1Dot, -q1, q2Dot, q0, q3Dot));
202 if (r.getQ0().getOrder() >= 2) {
203 final double q0DotDot = r.getQ0().getPartialDerivative(2);
204 final double q1DotDot = r.getQ1().getPartialDerivative(2);
205 final double q2DotDot = r.getQ2().getPartialDerivative(2);
206 final double q3DotDot = r.getQ3().getPartialDerivative(2);
207 rotationAcceleration =
208 new Vector3D(2 * MathArrays.linearCombination(-q1, q0DotDot, q0, q1DotDot, q3, q2DotDot, -q2, q3DotDot),
209 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot, q0, q2DotDot, q1, q3DotDot),
210 2 * MathArrays.linearCombination(-q3, q0DotDot, q2, q1DotDot, -q1, q2DotDot, q0, q3DotDot));
211 } else {
212 rotationAcceleration = Vector3D.ZERO;
213 }
214 } else {
215 rotationRate = Vector3D.ZERO;
216 rotationAcceleration = Vector3D.ZERO;
217 }
218
219 }
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238 private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1,
239 final Vector3D v2, final Vector3D c2,
240 final double tolerance)
241 throws MathIllegalArgumentException {
242
243 final double v12 = v1.getNormSq();
244 final double v1n = FastMath.sqrt(v12);
245 final double v22 = v2.getNormSq();
246 final double v2n = FastMath.sqrt(v22);
247 final double threshold = tolerance * FastMath.max(v1n, v2n);
248
249 Vector3D omega;
250
251 try {
252
253 final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
254 m.setEntry(0, 1, v1.getZ());
255 m.setEntry(0, 2, -v1.getY());
256 m.setEntry(1, 0, -v1.getZ());
257 m.setEntry(1, 2, v1.getX());
258 m.setEntry(2, 0, v1.getY());
259 m.setEntry(2, 1, -v1.getX());
260 m.setEntry(3, 1, v2.getZ());
261 m.setEntry(3, 2, -v2.getY());
262 m.setEntry(4, 0, -v2.getZ());
263 m.setEntry(4, 2, v2.getX());
264 m.setEntry(5, 0, v2.getY());
265 m.setEntry(5, 1, -v2.getX());
266
267 final RealVector rhs = MatrixUtils.createRealVector(new double[] {
268 c1.getX(), c1.getY(), c1.getZ(),
269 c2.getX(), c2.getY(), c2.getZ()
270 });
271
272
273 final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
274 final RealVector v = solver.solve(rhs);
275 omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));
276
277 } catch (MathIllegalArgumentException miae) {
278 if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {
279
280
281 final double c12 = c1.getNormSq();
282 final double c1n = FastMath.sqrt(c12);
283 final double c22 = c2.getNormSq();
284 final double c2n = FastMath.sqrt(c22);
285
286 if (c1n <= threshold && c2n <= threshold) {
287
288 return Vector3D.ZERO;
289 } else if (v1n <= threshold && c1n >= threshold) {
290
291 throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c1n, 0, true);
292 } else if (v2n <= threshold && c2n >= threshold) {
293
294 throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c2n, 0, true);
295 } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
296
297
298 omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
299 } else {
300 throw miae;
301 }
302 } else {
303 throw miae;
304 }
305
306 }
307
308
309 final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
310 if (d1 > threshold) {
311 throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d1, 0, true);
312 }
313
314 final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
315 if (d2 > threshold) {
316 throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d2, 0, true);
317 }
318
319 return omega;
320
321 }
322
323
324
325
326
327
328
329
330
331 public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) {
332
333
334 final double q0 = rotation.getQ0();
335 final double q1 = rotation.getQ1();
336 final double q2 = rotation.getQ2();
337 final double q3 = rotation.getQ3();
338
339
340 final double oX = rotationRate.getX();
341 final double oY = rotationRate.getY();
342 final double oZ = rotationRate.getZ();
343 final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
344 final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY, q2, oZ);
345 final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX, q0, oY, -q1, oZ);
346 final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
347
348
349 final double oXDot = rotationAcceleration.getX();
350 final double oYDot = rotationAcceleration.getY();
351 final double oZDot = rotationAcceleration.getZ();
352 final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
353 q1, q2, q3, q1Dot, q2Dot, q3Dot
354 }, new double[] {
355 oXDot, oYDot, oZDot, oX, oY, oZ
356 });
357 final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] {
358 q0, q2, -q3, q0Dot, q2Dot, -q3Dot
359 }, new double[] {
360 oXDot, oZDot, oYDot, oX, oZ, oY
361 });
362 final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] {
363 q0, q3, -q1, q0Dot, q3Dot, -q1Dot
364 }, new double[] {
365 oYDot, oXDot, oZDot, oY, oX, oZ
366 });
367 final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] {
368 q0, q1, -q2, q0Dot, q1Dot, -q2Dot
369 }, new double[] {
370 oZDot, oYDot, oXDot, oZ, oY, oX
371 });
372
373 final DSFactory factory;
374 final DerivativeStructure q0DS;
375 final DerivativeStructure q1DS;
376 final DerivativeStructure q2DS;
377 final DerivativeStructure q3DS;
378 switch(order) {
379 case 0 :
380 factory = new DSFactory(1, order);
381 q0DS = factory.build(q0);
382 q1DS = factory.build(q1);
383 q2DS = factory.build(q2);
384 q3DS = factory.build(q3);
385 break;
386 case 1 :
387 factory = new DSFactory(1, order);
388 q0DS = factory.build(q0, q0Dot);
389 q1DS = factory.build(q1, q1Dot);
390 q2DS = factory.build(q2, q2Dot);
391 q3DS = factory.build(q3, q3Dot);
392 break;
393 case 2 :
394 factory = new DSFactory(1, order);
395 q0DS = factory.build(q0, q0Dot, q0DotDot);
396 q1DS = factory.build(q1, q1Dot, q1DotDot);
397 q2DS = factory.build(q2, q2Dot, q2DotDot);
398 q3DS = factory.build(q3, q3Dot, q3DotDot);
399 break;
400 default :
401 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
402 }
403
404 return new FieldRotation<>(q0DS, q1DS, q2DS, q3DS, false);
405
406 }
407
408
409
410
411
412
413
414
415 public FieldRotation<UnivariateDerivative1> toUnivariateDerivative1Rotation() {
416
417
418 final double q0 = rotation.getQ0();
419 final double q1 = rotation.getQ1();
420 final double q2 = rotation.getQ2();
421 final double q3 = rotation.getQ3();
422
423
424 final double oX = rotationRate.getX();
425 final double oY = rotationRate.getY();
426 final double oZ = rotationRate.getZ();
427 final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
428 final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY, q2, oZ);
429 final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX, q0, oY, -q1, oZ);
430 final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
431
432 final UnivariateDerivative1 q0UD = new UnivariateDerivative1(q0, q0Dot);
433 final UnivariateDerivative1 q1UD = new UnivariateDerivative1(q1, q1Dot);
434 final UnivariateDerivative1 q2UD = new UnivariateDerivative1(q2, q2Dot);
435 final UnivariateDerivative1 q3UD = new UnivariateDerivative1(q3, q3Dot);
436
437 return new FieldRotation<>(q0UD, q1UD, q2UD, q3UD, false);
438
439 }
440
441
442
443
444
445
446
447
448
449 public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
450 final Rotation evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
451 return new Vector3D(evolution.getAngle() / dt, evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
452 }
453
454
455
456
457
458
459 public AngularCoordinates revert() {
460 return new AngularCoordinates(rotation.revert(),
461 rotation.applyInverseTo(rotationRate).negate(),
462 rotation.applyInverseTo(rotationAcceleration).negate());
463 }
464
465
466
467
468
469
470
471
472
473
474
475 public AngularCoordinates shiftedBy(final double dt) {
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491 final double rate = rotationRate.getNorm();
492 final Rotation rateContribution = (rate == 0.0) ?
493 Rotation.IDENTITY :
494 new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);
495
496
497 final AngularCoordinates linearPart =
498 new AngularCoordinates(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR), rotationRate);
499
500 final double acc = rotationAcceleration.getNorm();
501 if (acc == 0.0) {
502
503 return linearPart;
504 }
505
506
507
508
509
510 final AngularCoordinates quadraticContribution =
511 new AngularCoordinates(new Rotation(rotationAcceleration,
512 0.5 * acc * dt * dt,
513 RotationConvention.FRAME_TRANSFORM),
514 new Vector3D(dt, rotationAcceleration),
515 rotationAcceleration);
516
517
518
519
520
521 return quadraticContribution.addOffset(linearPart);
522
523 }
524
525
526
527
528 public Rotation getRotation() {
529 return rotation;
530 }
531
532
533
534
535 public Vector3D getRotationRate() {
536 return rotationRate;
537 }
538
539
540
541
542 public Vector3D getRotationAcceleration() {
543 return rotationAcceleration;
544 }
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564 public AngularCoordinatesarCoordinates">AngularCoordinates addOffset(final AngularCoordinates offset) {
565 final Vector3D rOmega = rotation.applyTo(offset.rotationRate);
566 final Vector3D rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
567 return new AngularCoordinates(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
568 rotationRate.add(rOmega),
569 new Vector3D( 1.0, rotationAcceleration,
570 1.0, rOmegaDot,
571 -1.0, Vector3D.crossProduct(rotationRate, rOmega)));
572 }
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592 public AngularCoordinatesrdinates">AngularCoordinates subtractOffset(final AngularCoordinates offset) {
593 return addOffset(offset.revert());
594 }
595
596
597
598
599
600 public PVCoordinatesoordinates">PVCoordinates applyTo(final PVCoordinates pv) {
601
602 final Vector3D transformedP = rotation.applyTo(pv.getPosition());
603 final Vector3D crossP = Vector3D.crossProduct(rotationRate, transformedP);
604 final Vector3D transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
605 final Vector3D crossV = Vector3D.crossProduct(rotationRate, transformedV);
606 final Vector3D crossCrossP = Vector3D.crossProduct(rotationRate, crossP);
607 final Vector3D crossDotP = Vector3D.crossProduct(rotationAcceleration, transformedP);
608 final Vector3D transformedA = new Vector3D( 1, rotation.applyTo(pv.getAcceleration()),
609 -2, crossV,
610 -1, crossCrossP,
611 -1, crossDotP);
612
613 return new PVCoordinates(transformedP, transformedV, transformedA);
614
615 }
616
617
618
619
620
621 public TimeStampedPVCoordinateseStampedPVCoordinates">TimeStampedPVCoordinates applyTo(final TimeStampedPVCoordinates pv) {
622
623 final Vector3D transformedP = getRotation().applyTo(pv.getPosition());
624 final Vector3D crossP = Vector3D.crossProduct(getRotationRate(), transformedP);
625 final Vector3D transformedV = getRotation().applyTo(pv.getVelocity()).subtract(crossP);
626 final Vector3D crossV = Vector3D.crossProduct(getRotationRate(), transformedV);
627 final Vector3D crossCrossP = Vector3D.crossProduct(getRotationRate(), crossP);
628 final Vector3D crossDotP = Vector3D.crossProduct(getRotationAcceleration(), transformedP);
629 final Vector3D transformedA = new Vector3D( 1, getRotation().applyTo(pv.getAcceleration()),
630 -2, crossV,
631 -1, crossCrossP,
632 -1, crossDotP);
633
634 return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV, transformedA);
635
636 }
637
638
639
640
641
642
643
644 public <T extends RealFieldElement<T>> FieldPVCoordinates<T> applyTo(final FieldPVCoordinates<T> pv) {
645
646 final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
647 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(rotationRate, transformedP);
648 final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
649 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(rotationRate, transformedV);
650 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(rotationRate, crossP);
651 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
652 final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
653 -2, crossV,
654 -1, crossCrossP,
655 -1, crossDotP);
656
657 return new FieldPVCoordinates<>(transformedP, transformedV, transformedA);
658
659 }
660
661
662
663
664
665
666
667 public <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedFieldPVCoordinates<T> pv) {
668
669 final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
670 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(rotationRate, transformedP);
671 final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
672 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(rotationRate, transformedV);
673 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(rotationRate, crossP);
674 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
675 final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
676 -2, crossV,
677 -1, crossCrossP,
678 -1, crossDotP);
679
680 return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV, transformedA);
681
682 }
683
684
685
686
687
688
689
690
691
692
693
694 public double[][] getModifiedRodrigues(final double sign) {
695
696 final double q0 = sign * getRotation().getQ0();
697 final double q1 = sign * getRotation().getQ1();
698 final double q2 = sign * getRotation().getQ2();
699 final double q3 = sign * getRotation().getQ3();
700 final double oX = getRotationRate().getX();
701 final double oY = getRotationRate().getY();
702 final double oZ = getRotationRate().getZ();
703 final double oXDot = getRotationAcceleration().getX();
704 final double oYDot = getRotationAcceleration().getY();
705 final double oZDot = getRotationAcceleration().getZ();
706
707
708 final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
709 final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY, q2, oZ);
710 final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX, q0, oY, -q1, oZ);
711 final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
712
713
714 final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
715 q1, q2, q3, q1Dot, q2Dot, q3Dot
716 }, new double[] {
717 oXDot, oYDot, oZDot, oX, oY, oZ
718 });
719 final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] {
720 q0, q2, -q3, q0Dot, q2Dot, -q3Dot
721 }, new double[] {
722 oXDot, oZDot, oYDot, oX, oZ, oY
723 });
724 final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] {
725 q0, q3, -q1, q0Dot, q3Dot, -q1Dot
726 }, new double[] {
727 oYDot, oXDot, oZDot, oY, oX, oZ
728 });
729 final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] {
730 q0, q1, -q2, q0Dot, q1Dot, -q2Dot
731 }, new double[] {
732 oZDot, oYDot, oXDot, oZ, oY, oX
733 });
734
735
736
737
738
739 final double inv = 1.0 / (1.0 + q0);
740 final double mTwoInvQ0Dot = -2 * inv * q0Dot;
741
742 final double r1 = inv * q1;
743 final double r2 = inv * q2;
744 final double r3 = inv * q3;
745
746 final double mInvR1 = -inv * r1;
747 final double mInvR2 = -inv * r2;
748 final double mInvR3 = -inv * r3;
749
750 final double r1Dot = MathArrays.linearCombination(inv, q1Dot, mInvR1, q0Dot);
751 final double r2Dot = MathArrays.linearCombination(inv, q2Dot, mInvR2, q0Dot);
752 final double r3Dot = MathArrays.linearCombination(inv, q3Dot, mInvR3, q0Dot);
753
754 final double r1DotDot = MathArrays.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
755 final double r2DotDot = MathArrays.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
756 final double r3DotDot = MathArrays.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);
757
758 return new double[][] {
759 {
760 r1, r2, r3
761 }, {
762 r1Dot, r2Dot, r3Dot
763 }, {
764 r1DotDot, r2DotDot, r3DotDot
765 }
766 };
767
768 }
769
770
771
772
773
774
775 public static AngularCoordinates createFromModifiedRodrigues(final double[][] r) {
776
777
778 final double rSquared = r[0][0] * r[0][0] + r[0][1] * r[0][1] + r[0][2] * r[0][2];
779 final double oPQ0 = 2 / (1 + rSquared);
780 final double q0 = oPQ0 - 1;
781 final double q1 = oPQ0 * r[0][0];
782 final double q2 = oPQ0 * r[0][1];
783 final double q3 = oPQ0 * r[0][2];
784
785
786 final double oPQ02 = oPQ0 * oPQ0;
787 final double q0Dot = -oPQ02 * MathArrays.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1], r[0][2], r[1][2]);
788 final double q1Dot = oPQ0 * r[1][0] + r[0][0] * q0Dot;
789 final double q2Dot = oPQ0 * r[1][1] + r[0][1] * q0Dot;
790 final double q3Dot = oPQ0 * r[1][2] + r[0][2] * q0Dot;
791 final double oX = 2 * MathArrays.linearCombination(-q1, q0Dot, q0, q1Dot, q3, q2Dot, -q2, q3Dot);
792 final double oY = 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot, q0, q2Dot, q1, q3Dot);
793 final double oZ = 2 * MathArrays.linearCombination(-q3, q0Dot, q2, q1Dot, -q1, q2Dot, q0, q3Dot);
794
795
796 final double q0DotDot = (1 - q0) / oPQ0 * q0Dot * q0Dot -
797 oPQ02 * MathArrays.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]) -
798 (q1Dot * q1Dot + q2Dot * q2Dot + q3Dot * q3Dot);
799 final double q1DotDot = MathArrays.linearCombination(oPQ0, r[2][0], 2 * r[1][0], q0Dot, r[0][0], q0DotDot);
800 final double q2DotDot = MathArrays.linearCombination(oPQ0, r[2][1], 2 * r[1][1], q0Dot, r[0][1], q0DotDot);
801 final double q3DotDot = MathArrays.linearCombination(oPQ0, r[2][2], 2 * r[1][2], q0Dot, r[0][2], q0DotDot);
802 final double oXDot = 2 * MathArrays.linearCombination(-q1, q0DotDot, q0, q1DotDot, q3, q2DotDot, -q2, q3DotDot);
803 final double oYDot = 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot, q0, q2DotDot, q1, q3DotDot);
804 final double oZDot = 2 * MathArrays.linearCombination(-q3, q0DotDot, q2, q1DotDot, -q1, q2DotDot, q0, q3DotDot);
805
806 return new AngularCoordinates(new Rotation(q0, q1, q2, q3, false),
807 new Vector3D(oX, oY, oZ),
808 new Vector3D(oXDot, oYDot, oZDot));
809
810 }
811
812 }