Examples with AngularCoordinates org.orekit.utils.AngularCoordinates used on opensource projects

Example 6 with AngularCoordinates

use of org.orekit.utils.AngularCoordinates in project Orekit by CS-SI.

the class FramesFactoryTest method doTestDerivatives.

private void doTestDerivatives(AbsoluteDate ref, double duration, double step, boolean forbidInterpolation, double cartesianTolerance, double cartesianDotTolerance, double cartesianDotDotTolerance, double rodriguesTolerance, double rodriguesDotTolerance, double rodriguesDotDotTolerance) throws OrekitException {
    final DSFactory factory = new DSFactory(1, 2);
    final FieldAbsoluteDate<DerivativeStructure> refDS = new FieldAbsoluteDate<>(factory.getDerivativeField(), ref);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 60.0);
    for (final Predefined predefined : Predefined.values()) {
        final Frame frame = FramesFactory.getFrame(predefined);
        final Frame parent = frame.getParent();
        if (parent != null) {
            UnivariateDifferentiableVectorFunction dCartesian = differentiator.differentiate(new UnivariateVectorFunction() {

                @Override
                public double[] value(double t) {
                    try {
                        return forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getTranslation().toArray() : parent.getTransformTo(frame, ref.shiftedBy(t)).getTranslation().toArray();
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            UnivariateDifferentiableVectorFunction dOrientation = differentiator.differentiate(new UnivariateVectorFunction() {

                double sign = +1.0;

                Rotation previous = Rotation.IDENTITY;

                @Override
                public double[] value(double t) {
                    try {
                        AngularCoordinates ac = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getAngular() : parent.getTransformTo(frame, ref.shiftedBy(t)).getAngular();
                        final double dot = MathArrays.linearCombination(ac.getRotation().getQ0(), previous.getQ0(), ac.getRotation().getQ1(), previous.getQ1(), ac.getRotation().getQ2(), previous.getQ2(), ac.getRotation().getQ3(), previous.getQ3());
                        sign = FastMath.copySign(1.0, dot * sign);
                        previous = ac.getRotation();
                        return ac.getModifiedRodrigues(sign)[0];
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            double maxCartesianError = 0;
            double maxCartesianDotError = 0;
            double maxCartesianDotDotError = 0;
            double maxRodriguesError = 0;
            double maxRodriguesDotError = 0;
            double maxRodriguesDotDotError = 0;
            for (double dt = 0; dt < duration; dt += step) {
                final DerivativeStructure dtDS = factory.variable(0, dt);
                final FieldTransform<DerivativeStructure> tDS = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, refDS.shiftedBy(dtDS)) : parent.getTransformTo(frame, refDS.shiftedBy(dtDS));
                final DerivativeStructure[] refCart = dCartesian.value(dtDS);
                final DerivativeStructure[] fieldCart = tDS.getTranslation().toArray();
                for (int i = 0; i < 3; ++i) {
                    maxCartesianError = FastMath.max(maxCartesianError, FastMath.abs(refCart[i].getValue() - fieldCart[i].getValue()));
                    maxCartesianDotError = FastMath.max(maxCartesianDotError, FastMath.abs(refCart[i].getPartialDerivative(1) - fieldCart[i].getPartialDerivative(1)));
                    maxCartesianDotDotError = FastMath.max(maxCartesianDotDotError, FastMath.abs(refCart[i].getPartialDerivative(2) - fieldCart[i].getPartialDerivative(2)));
                }
                final DerivativeStructure[] refOr = dOrientation.value(dtDS);
                DerivativeStructure[] fieldOr = tDS.getAngular().getModifiedRodrigues(1.0)[0];
                final double dot = refOr[0].linearCombination(refOr, fieldOr).getReal();
                if (dot < 0 || Double.isNaN(dot)) {
                    fieldOr = tDS.getAngular().getModifiedRodrigues(-1.0)[0];
                }
                for (int i = 0; i < 3; ++i) {
                    maxRodriguesError = FastMath.max(maxRodriguesError, FastMath.abs(refOr[i].getValue() - fieldOr[i].getValue()));
                    maxRodriguesDotError = FastMath.max(maxRodriguesDotError, FastMath.abs(refOr[i].getPartialDerivative(1) - fieldOr[i].getPartialDerivative(1)));
                    maxRodriguesDotDotError = FastMath.max(maxRodriguesDotDotError, FastMath.abs(refOr[i].getPartialDerivative(2) - fieldOr[i].getPartialDerivative(2)));
                }
            }
            Assert.assertEquals(frame.getName(), 0, maxCartesianError, cartesianTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotError, cartesianDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotDotError, cartesianDotDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesError, rodriguesTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotError, rodriguesDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotDotError, rodriguesDotDotTolerance);
        }
    }
}

Example 7 with AngularCoordinates

use of org.orekit.utils.AngularCoordinates in project Orekit by CS-SI.

the class TIRFProviderTest method assertTransformEquals.

/**
 * Check the two {@link Transform}s are the same to within an absolute tolerance.
 */
private static void assertTransformEquals(Transform expected, Transform actual, double absTol) {
    final AngularCoordinates expectedAngular = expected.getAngular();
    final AngularCoordinates actualAngular = actual.getAngular();
    final Rotation expectedRotation = expectedAngular.getRotation();
    final Rotation actualRotation = actualAngular.getRotation();
    final PVCoordinates expectedPV = expected.getCartesian();
    final PVCoordinates actualPV = actual.getCartesian();
    // transform
    Assert.assertEquals(Rotation.distance(actualRotation, expectedRotation), 0, absTol);
    Assert.assertEquals(expectedAngular.getRotationRate(), actualAngular.getRotationRate());
    Assert.assertEquals(expectedPV.getPosition(), actualPV.getPosition());
    Assert.assertEquals(expectedPV.getVelocity(), actualPV.getVelocity());
}