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

Example 76 with Rotation

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

the class FixedRateTest method doTestSpin.

private <T extends RealFieldElement<T>> void doTestSpin(final Field<T> field) throws OrekitException {
    final T zero = field.getZero();
    FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(1970, 01, 01), new TimeComponents(3, 25, 45.6789), TimeScalesFactory.getUTC());
    final T rate = zero.add(2 * FastMath.PI / (12 * 60));
    AttitudeProvider law = new FixedRate(new Attitude(date.toAbsoluteDate(), FramesFactory.getEME2000(), new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate.getReal(), Vector3D.PLUS_K), Vector3D.ZERO));
    FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7178000.0), zero.add(1.e-4), zero.add(FastMath.toRadians(50.)), zero.add(FastMath.toRadians(10.)), zero.add(FastMath.toRadians(20.)), zero.add(FastMath.toRadians(30.)), PositionAngle.MEAN, FramesFactory.getEME2000(), date, 3.986004415e14);
    FieldPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit, law);
    T h = zero.add(0.01);
    FieldSpacecraftState<T> sMinus = propagator.propagate(date.shiftedBy(h.negate()));
    FieldSpacecraftState<T> s0 = propagator.propagate(date);
    FieldSpacecraftState<T> sPlus = propagator.propagate(date.shiftedBy(h));
    // check spin is consistent with attitude evolution
    double errorAngleMinus = FieldRotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(), s0.getAttitude().getRotation()).getReal();
    double evolutionAngleMinus = FieldRotation.distance(sMinus.getAttitude().getRotation(), s0.getAttitude().getRotation()).getReal();
    Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
    double errorAnglePlus = FieldRotation.distance(s0.getAttitude().getRotation(), sPlus.shiftedBy(h.negate()).getAttitude().getRotation()).getReal();
    double evolutionAnglePlus = FieldRotation.distance(s0.getAttitude().getRotation(), sPlus.getAttitude().getRotation()).getReal();
    Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
    FieldVector3D<T> spin0 = s0.getAttitude().getSpin();
    FieldVector3D<T> reference = FieldAngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(), sPlus.getAttitude().getRotation(), h.multiply(2));
    Assert.assertEquals(0.0, spin0.subtract(reference).getNorm().getReal(), 1.0e-14);
}

Example 77 with Rotation

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

the class TimeStampedAngularCoordinates method interpolate.

/**
 * Interpolate angular coordinates.
 * <p>
 * The interpolated instance is created by polynomial Hermite interpolation
 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
 * </p>
 * <p>
 * This method is based on Sergei Tanygin's paper <a
 * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
 * vector as described in Malcolm D. Shuster's paper <a
 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
 * Survey of Attitude Representations</a>. This change avoids the singularity at π.
 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
 * when this singularity is detected. Another change is that the mean linear motion
 * is first removed before interpolation and added back after interpolation. This allows
 * to use interpolation even when the sample covers much more than one turn and even
 * when sample points are separated by more than one turn.
 * </p>
 * <p>
 * Note that even if first and second time derivatives (rotation rates and acceleration)
 * from sample can be ignored, the interpolated instance always includes
 * interpolated derivatives. This feature can be used explicitly to
 * compute these derivatives when it would be too complex to compute them
 * from an analytical formula: just compute a few sample points from the
 * explicit formula and set the derivatives to zero in these sample points,
 * then use interpolation to add derivatives consistent with the rotations.
 * </p>
 * @param date interpolation date
 * @param filter filter for derivatives from the sample to use in interpolation
 * @param sample sample points on which interpolation should be done
 * @return a new position-velocity, interpolated at specified date
 * @exception OrekitException if the number of point is too small for interpolating
 */
public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedAngularCoordinates> sample) throws OrekitException {
    // set up safety elements for 2π singularity avoidance
    final double epsilon = 2 * FastMath.PI / sample.size();
    final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
    // set up a linear model canceling mean rotation rate
    final Vector3D meanRate;
    if (filter != AngularDerivativesFilter.USE_R) {
        Vector3D sum = Vector3D.ZERO;
        for (final TimeStampedAngularCoordinates datedAC : sample) {
            sum = sum.add(datedAC.getRotationRate());
        }
        meanRate = new Vector3D(1.0 / sample.size(), sum);
    } else {
        if (sample.size() < 2) {
            throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size());
        }
        Vector3D sum = Vector3D.ZERO;
        TimeStampedAngularCoordinates previous = null;
        for (final TimeStampedAngularCoordinates datedAC : sample) {
            if (previous != null) {
                sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.date)));
            }
            previous = datedAC;
        }
        meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
    }
    TimeStampedAngularCoordinates offset = new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);
    boolean restart = true;
    for (int i = 0; restart && i < sample.size() + 2; ++i) {
        // offset adaptation parameters
        restart = false;
        // set up an interpolator taking derivatives into account
        final HermiteInterpolator interpolator = new HermiteInterpolator();
        // add sample points
        double sign = +1.0;
        Rotation previous = Rotation.IDENTITY;
        for (final TimeStampedAngularCoordinates ac : sample) {
            // remove linear offset from the current coordinates
            final double dt = ac.date.durationFrom(date);
            final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));
            // make sure all interpolated points will be on the same branch
            final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), fixed.getRotation().getQ1(), previous.getQ1(), fixed.getRotation().getQ2(), previous.getQ2(), fixed.getRotation().getQ3(), previous.getQ3());
            sign = FastMath.copySign(1.0, dot * sign);
            previous = fixed.getRotation();
            // check modified Rodrigues vector singularity
            if (fixed.getRotation().getQ0() * sign < threshold) {
                // the sample point is close to a modified Rodrigues vector singularity
                // we need to change the linear offset model to avoid this
                restart = true;
                break;
            }
            final double[][] rodrigues = fixed.getModifiedRodrigues(sign);
            switch(filter) {
                case USE_RRA:
                    // populate sample with rotation, rotation rate and acceleration data
                    interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
                    break;
                case USE_RR:
                    // populate sample with rotation and rotation rate data
                    interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
                    break;
                case USE_R:
                    // populate sample with rotation data only
                    interpolator.addSamplePoint(dt, rodrigues[0]);
                    break;
                default:
                    // this should never happen
                    throw new OrekitInternalError(null);
            }
        }
        if (restart) {
            // interpolation failed, some intermediate rotation was too close to 2π
            // we need to offset all rotations to avoid the singularity
            offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon, RotationConvention.VECTOR_OPERATOR), Vector3D.ZERO, Vector3D.ZERO));
        } else {
            // interpolation succeeded with the current offset
            final double[][] p = interpolator.derivatives(0.0, 2);
            final AngularCoordinates ac = createFromModifiedRodrigues(p);
            return new TimeStampedAngularCoordinates(offset.getDate(), ac.getRotation(), ac.getRotationRate(), ac.getRotationAcceleration()).addOffset(offset);
        }
    }
    // this should never happen
    throw new OrekitInternalError(null);
}

Example 78 with Rotation

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

the class AngularCoordinates method shiftedBy.

/**
 * Get a time-shifted state.
 * <p>
 * The state can be slightly shifted to close dates. This shift is based on
 * an approximate solution of the fixed acceleration motion. It is <em>not</em>
 * intended as a replacement for proper attitude propagation but should be
 * sufficient for either small time shifts or coarse accuracy.
 * </p>
 * @param dt time shift in seconds
 * @return a new state, shifted with respect to the instance (which is immutable)
 */
public AngularCoordinates shiftedBy(final double dt) {
    // the shiftedBy method is based on a local approximation.
    // It considers separately the contribution of the constant
    // rotation, the linear contribution or the rate and the
    // quadratic contribution of the acceleration. The rate
    // and acceleration contributions are small rotations as long
    // as the time shift is small, which is the crux of the algorithm.
    // Small rotations are almost commutative, so we append these small
    // contributions one after the other, as if they really occurred
    // successively, despite this is not what really happens.
    // compute the linear contribution first, ignoring acceleration
    // BEWARE: there is really a minus sign here, because if
    // the target frame rotates in one direction, the vectors in the origin
    // frame seem to rotate in the opposite direction
    final double rate = rotationRate.getNorm();
    final Rotation rateContribution = (rate == 0.0) ? Rotation.IDENTITY : new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);
    // append rotation and rate contribution
    final AngularCoordinates linearPart = new AngularCoordinates(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR), rotationRate);
    final double acc = rotationAcceleration.getNorm();
    if (acc == 0.0) {
        // no acceleration, the linear part is sufficient
        return linearPart;
    }
    // compute the quadratic contribution, ignoring initial rotation and rotation rate
    // BEWARE: there is really a minus sign here, because if
    // the target frame rotates in one direction, the vectors in the origin
    // frame seem to rotate in the opposite direction
    final AngularCoordinates quadraticContribution = new AngularCoordinates(new Rotation(rotationAcceleration, 0.5 * acc * dt * dt, RotationConvention.FRAME_TRANSFORM), new Vector3D(dt, rotationAcceleration), rotationAcceleration);
    // quadratic part after the linear part as a simple offset
    return quadraticContribution.addOffset(linearPart);
}

Example 79 with Rotation

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

the class SpinStabilized method getAttitude.

/**
 * {@inheritDoc}
 */
public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException {
    // get attitude from underlying non-rotating law
    final Attitude base = nonRotatingLaw.getAttitude(pvProv, date, frame);
    final Transform baseTransform = new Transform(date, base.getOrientation());
    // compute spin transform due to spin from reference to current date
    final Transform spinInfluence = new Transform(date, new Rotation(axis, rate * date.durationFrom(start), RotationConvention.FRAME_TRANSFORM), spin);
    // combine the two transforms
    final Transform combined = new Transform(date, baseTransform, spinInfluence);
    // build the attitude
    return new Attitude(date, frame, combined.getRotation(), combined.getRotationRate(), combined.getRotationAcceleration());
}

Example 80 with Rotation

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

the class YawCompensation method getYawAngle.

/**
 * Compute the yaw compensation angle at date.
 * @param pvProv provider for PV coordinates
 * @param date date at which compensation is requested
 * @param frame reference frame from which attitude is computed
 * @return yaw compensation angle for orbit.
 * @throws OrekitException if some specific error occurs
 */
public double getYawAngle(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException {
    final Rotation rBase = getBaseState(pvProv, date, frame).getRotation();
    final Rotation rCompensated = getAttitude(pvProv, date, frame).getRotation();
    final Rotation compensation = rCompensated.compose(rBase.revert(), RotationConvention.VECTOR_OPERATOR);
    return -compensation.applyTo(Vector3D.PLUS_I).getAlpha();
}