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