Examples with Rotation org.hipparchus.geometry.euclidean.threed.Rotation used on opensource projects

Example 16 with Rotation

use of org.hipparchus.geometry.euclidean.threed.Rotation in project Orekit by CS-SI.

the class TimeStampedAngularCoordinatesTest method testInterpolationWithoutAcceleration.

@Test
public void testInterpolationWithoutAcceleration() throws OrekitException {
    AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
    double alpha0 = 0.5 * FastMath.PI;
    double omega = 0.05 * FastMath.PI;
    final TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(date, new Rotation(Vector3D.PLUS_K, alpha0, RotationConvention.VECTOR_OPERATOR), new Vector3D(omega, Vector3D.MINUS_K), Vector3D.ZERO);
    double[] errors = interpolationErrors(reference, 1.0);
    Assert.assertEquals(0.0, errors[0], 1.4e-15);
    Assert.assertEquals(0.0, errors[1], 3.0e-15);
    Assert.assertEquals(0.0, errors[2], 3.0e-14);
}

Example 17 with Rotation

use of org.hipparchus.geometry.euclidean.threed.Rotation in project Orekit by CS-SI.

the class TimeStampedAngularCoordinatesTest method interpolationErrors.

private double[] interpolationErrors(final TimeStampedAngularCoordinates reference, double dt) throws OrekitException {
    final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {

        public int getDimension() {
            return 4;
        }

        public double[] computeDerivatives(final double t, final double[] q) {
            final double omegaX = reference.getRotationRate().getX() + t * reference.getRotationAcceleration().getX();
            final double omegaY = reference.getRotationRate().getY() + t * reference.getRotationAcceleration().getY();
            final double omegaZ = reference.getRotationRate().getZ() + t * reference.getRotationAcceleration().getZ();
            return new double[] { 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ), 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ), 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ), 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ) };
        }
    };
    final List<TimeStampedAngularCoordinates> complete = new ArrayList<TimeStampedAngularCoordinates>();
    ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
    integrator.addStepHandler(new StepNormalizer(dt / 2000, new ODEFixedStepHandler() {

        public void handleStep(ODEStateAndDerivative state, boolean isLast) {
            final double t = state.getTime();
            final double[] q = state.getPrimaryState();
            complete.add(new TimeStampedAngularCoordinates(reference.getDate().shiftedBy(t), new Rotation(q[0], q[1], q[2], q[3], true), new Vector3D(1, reference.getRotationRate(), t, reference.getRotationAcceleration()), reference.getRotationAcceleration()));
        }
    }));
    double[] y = new double[] { reference.getRotation().getQ0(), reference.getRotation().getQ1(), reference.getRotation().getQ2(), reference.getRotation().getQ3() };
    integrator.integrate(ode, new ODEState(0, y), dt);
    List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
    sample.add(complete.get(0));
    sample.add(complete.get(complete.size() / 2));
    sample.add(complete.get(complete.size() - 1));
    double maxRotationError = 0;
    double maxRateError = 0;
    double maxAccelerationError = 0;
    for (TimeStampedAngularCoordinates acRef : complete) {
        TimeStampedAngularCoordinates interpolated = TimeStampedAngularCoordinates.interpolate(acRef.getDate(), AngularDerivativesFilter.USE_RRA, sample);
        double rotationError = Rotation.distance(acRef.getRotation(), interpolated.getRotation());
        double rateError = Vector3D.distance(acRef.getRotationRate(), interpolated.getRotationRate());
        double accelerationError = Vector3D.distance(acRef.getRotationAcceleration(), interpolated.getRotationAcceleration());
        maxRotationError = FastMath.max(maxRotationError, rotationError);
        maxRateError = FastMath.max(maxRateError, rateError);
        maxAccelerationError = FastMath.max(maxAccelerationError, accelerationError);
    }
    return new double[] { maxRotationError, maxRateError, maxAccelerationError };
}

Example 18 with Rotation

use of org.hipparchus.geometry.euclidean.threed.Rotation in project Orekit by CS-SI.

the class TimeStampedAngularCoordinatesTest method testInterpolationAroundPI.

@Test
public void testInterpolationAroundPI() throws OrekitException {
    List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
    // add angular coordinates at t0: 179.999 degrees rotation along X axis
    AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI());
    TimeStampedAngularCoordinates ac0 = new TimeStampedAngularCoordinates(t0, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(179.999), RotationConvention.VECTOR_OPERATOR), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO);
    sample.add(ac0);
    // add angular coordinates at t1: -179.999 degrees rotation (= 180.001 degrees) along X axis
    AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI());
    TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(t1, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(-179.999), RotationConvention.VECTOR_OPERATOR), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO);
    sample.add(ac1);
    // get interpolated angular coordinates at mid time between t0 and t1
    AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI());
    TimeStampedAngularCoordinates interpolated = TimeStampedAngularCoordinates.interpolate(t, AngularDerivativesFilter.USE_R, sample);
    Assert.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle(), 1.0e-12);
}

Example 19 with Rotation

use of org.hipparchus.geometry.euclidean.threed.Rotation in project Orekit by CS-SI.

the class AngularCoordinatesTest method testRodriguesSpecialCases.

@Test
public void testRodriguesSpecialCases() {
    // identity
    double[][] identity = new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO).getModifiedRodrigues(1.0);
    for (double[] row : identity) {
        for (double element : row) {
            Assert.assertEquals(0.0, element, Precision.SAFE_MIN);
        }
    }
    AngularCoordinates acId = AngularCoordinates.createFromModifiedRodrigues(identity);
    Assert.assertEquals(0.0, acId.getRotation().getAngle(), Precision.SAFE_MIN);
    Assert.assertEquals(0.0, acId.getRotationRate().getNorm(), Precision.SAFE_MIN);
    // PI angle rotation (which is singular for non-modified Rodrigues vector)
    RandomGenerator random = new Well1024a(0x2158523e6accb859l);
    for (int i = 0; i < 100; ++i) {
        Vector3D axis = randomVector(random, 1.0);
        AngularCoordinates original = new AngularCoordinates(new Rotation(axis, FastMath.PI, RotationConvention.VECTOR_OPERATOR), Vector3D.ZERO, Vector3D.ZERO);
        AngularCoordinates rebuilt = AngularCoordinates.createFromModifiedRodrigues(original.getModifiedRodrigues(1.0));
        Assert.assertEquals(FastMath.PI, rebuilt.getRotation().getAngle(), 1.0e-15);
        Assert.assertEquals(0.0, FastMath.sin(Vector3D.angle(axis, rebuilt.getRotation().getAxis(RotationConvention.VECTOR_OPERATOR))), 1.0e-15);
        Assert.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-16);
    }
}

Example 20 with Rotation

use of org.hipparchus.geometry.euclidean.threed.Rotation in project Orekit by CS-SI.

the class AngularCoordinatesTest method testRoundTripNoOp.

@Test
public void testRoundTripNoOp() {
    RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
    for (int i = 0; i < 100; ++i) {
        Rotation r1 = randomRotation(random);
        Vector3D o1 = randomVector(random, 1.0e-2);
        Vector3D oDot1 = randomVector(random, 1.0e-2);
        AngularCoordinates ac1 = new AngularCoordinates(r1, o1, oDot1);
        Rotation r2 = randomRotation(random);
        Vector3D o2 = randomVector(random, 1.0e-2);
        Vector3D oDot2 = randomVector(random, 1.0e-2);
        AngularCoordinates ac2 = new AngularCoordinates(r2, o2, oDot2);
        AngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
        Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 5.0e-16);
        Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);
        Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 2.0e-17);
        AngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
        Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 5.0e-16);
        Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17);
        Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17);
    }
}