Example usage for org.apache.commons.math3.geometry.euclidean.threed FieldRotation distance

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.threed FieldRotation distance.

Prototype

public static <T extends RealFieldElement<T>> T distance(final FieldRotation<T> r1, final FieldRotation<T> r2) 

Source Link

Document

Compute the distance between two rotations.

Usage

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
public void testZeroRate() throws OrekitException {
    FieldAngularCoordinates<DerivativeStructure> angularCoordinates = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(0.48, 0.64, 0.36, 0.48, false), createVector(0, 0, 0, 4), createVector(0, 0, 0, 4));
    Assert.assertEquals(createVector(0, 0, 0, 4), angularCoordinates.getRotationRate());
    double dt = 10.0;
    FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(0.0, shifted.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
    Assert.assertEquals(0.0, shifted.getRotationRate().getNorm().getReal(), 1.0e-15);
    Assert.assertEquals(0.0,/*from  w w w. j a v a2s.  c  o m*/
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-15);
}

---

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
public void testShift() throws OrekitException {
    double rate = 2 * FastMath.PI / (12 * 60);
    FieldAngularCoordinates<DerivativeStructure> angularCoordinates = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(1, 0, 0, 0, false),
            new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4));
    Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
    double dt = 10.0;
    double alpha = rate * dt;
    FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
    Assert.assertEquals(alpha,/*w w  w  .ja  v a 2 s .c o  m*/
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> xSat = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
    Assert.assertEquals(0.0,
            xSat.subtract(createVector(FastMath.cos(alpha), FastMath.sin(alpha), 0, 4)).getNorm().getReal(),
            1.0e-10);
    FieldVector3D<DerivativeStructure> ySat = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
    Assert.assertEquals(0.0,
            ySat.subtract(createVector(-FastMath.sin(alpha), FastMath.cos(alpha), 0, 4)).getNorm().getReal(),
            1.0e-10);
    FieldVector3D<DerivativeStructure> zSat = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
    Assert.assertEquals(0.0, zSat.subtract(createVector(0, 0, 1, 4)).getNorm().getReal(), 1.0e-10);

}

---

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
public void testSpin() throws OrekitException {
    double rate = 2 * FastMath.PI / (12 * 60);
    FieldAngularCoordinates<DerivativeStructure> angularCoordinates = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(0.48, 0.64, 0.36, 0.48, false),
            new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4));
    Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
    double dt = 10.0;
    FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
    Assert.assertEquals(rate * dt,//from w  ww.java2  s.co m
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation()
            .applyInverseTo(createVector(1, 0, 0, 4));
    FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation()
            .applyInverseTo(createVector(0, 1, 0, 4));
    FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation()
            .applyInverseTo(createVector(0, 0, 1, 4));
    FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation()
            .applyInverseTo(createVector(1, 0, 0, 4));
    FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation()
            .applyInverseTo(createVector(0, 1, 0, 4));
    FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation()
            .applyInverseTo(createVector(0, 0, 1, 4));
    Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(),
            1.0e-10);
    Assert.assertEquals(FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(),
            1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
    Assert.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(),
            1.0e-10);
    Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(),
            1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
    Assert.assertEquals(1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates
            .estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
    Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(),
            1.0e-10);

    FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(),
            angularCoordinates.getRotation(), dt);
    Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);

}

---

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
public void testNoCommute() {
    FieldAngularCoordinates<DerivativeStructure> ac1 = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(0.48, 0.64, 0.36, 0.48, false), createVector(0, 0, 0, 4), createVector(0, 0, 0, 4));
    FieldAngularCoordinates<DerivativeStructure> ac2 = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(0.36, -0.48, 0.48, 0.64, false), createVector(0, 0, 0, 4), createVector(0, 0, 0, 4));

    FieldAngularCoordinates<DerivativeStructure> add12 = ac1.addOffset(ac2);
    FieldAngularCoordinates<DerivativeStructure> add21 = ac2.addOffset(ac1);

    // the rotations are really different from each other
    Assert.assertEquals(2.574, FieldRotation.distance(add12.getRotation(), add21.getRotation()).getReal(),
            1.0e-3);/*from w  ww  . j  av  a 2s .  co  m*/

}

---

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
public void testRoundTripNoOp() {
    Random random = new Random(0x1e610cfe89306669l);
    for (int i = 0; i < 100; ++i) {

        FieldRotation<DerivativeStructure> r1 = randomRotation(random);
        FieldVector3D<DerivativeStructure> o1 = randomVector(random, 1.0e-2);
        FieldVector3D<DerivativeStructure> a1 = randomVector(random, 1.0e-2);
        FieldAngularCoordinates<DerivativeStructure> ac1 = new FieldAngularCoordinates<DerivativeStructure>(r1,
                o1, a1);/*from   www  .j a  v  a2 s . c  o  m*/

        FieldRotation<DerivativeStructure> r2 = randomRotation(random);
        FieldVector3D<DerivativeStructure> o2 = randomVector(random, 1.0e-2);
        FieldVector3D<DerivativeStructure> a2 = randomVector(random, 1.0e-2);
        FieldAngularCoordinates<DerivativeStructure> ac2 = new FieldAngularCoordinates<DerivativeStructure>(r2,
                o2, a2);

        FieldAngularCoordinates<DerivativeStructure> roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
        Assert.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripSA.getRotation()).getReal(),
                1.0e-15);
        Assert.assertEquals(0.0,
                FieldVector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()).getReal(),
                2.0e-17);
        Assert.assertEquals(0.0, FieldVector3D
                .distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()).getReal(),
                2.0e-17);

        FieldAngularCoordinates<DerivativeStructure> roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
        Assert.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripAS.getRotation()).getReal(),
                1.0e-15);
        Assert.assertEquals(0.0,
                FieldVector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()).getReal(),
                2.0e-17);
        Assert.assertEquals(0.0, FieldVector3D
                .distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()).getReal(),
                2.0e-17);

    }
}

---

From source file:org.orekit.utils.FieldAngularCoordinatesTest.java

@Test
@Deprecated // to be removed when AngularCoordinates.interpolate is removed
public void testInterpolationSimple() throws OrekitException {
    AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
    double alpha0 = 0.5 * FastMath.PI;
    double omega = 0.5 * FastMath.PI;
    FieldAngularCoordinates<DerivativeStructure> reference = new FieldAngularCoordinates<DerivativeStructure>(
            createRotation(createVector(0, 0, 1, 4), alpha0),
            new FieldVector3D<DerivativeStructure>(omega, createVector(0, 0, -1, 4)), createVector(0, 0, 0, 4));

    List<Pair<AbsoluteDate, FieldAngularCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldAngularCoordinates<DerivativeStructure>>>();
    for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
        sample.add(new Pair<AbsoluteDate, FieldAngularCoordinates<DerivativeStructure>>(date.shiftedBy(dt),
                reference.shiftedBy(dt)));
    }//w  w w  . j a  va 2 s  .  co m

    for (double dt = 0; dt < 1.0; dt += 0.001) {
        FieldAngularCoordinates<DerivativeStructure> interpolated = FieldAngularCoordinates
                .interpolate(date.shiftedBy(dt), true, sample);
        FieldRotation<DerivativeStructure> r = interpolated.getRotation();
        FieldVector3D<DerivativeStructure> rate = interpolated.getRotationRate();
        FieldVector3D<DerivativeStructure> acceleration = interpolated.getRotationAcceleration();
        Assert.assertEquals(0.0, FieldRotation
                .distance(createRotation(createVector(0, 0, 1, 4), alpha0 + omega * dt), r).getReal(), 1.1e-15);
        Assert.assertEquals(0.0, FieldVector3D.distance(createVector(0, 0, -omega, 4), rate).getReal(),
                4.0e-15);
        Assert.assertEquals(0.0, FieldVector3D.distance(createVector(0, 0, 0, 4), acceleration).getReal(),
                3.2e-14);
    }

}

---

From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java

@Test
public void testZeroRate() throws OrekitException {
    TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates = new TimeStampedFieldAngularCoordinates<DerivativeStructure>(
            AbsoluteDate.J2000_EPOCH, createRotation(0.48, 0.64, 0.36, 0.48, false), createVector(0, 0, 0, 4),
            createVector(0, 0, 0, 4));/*w w w.java  2s  . c o m*/
    Assert.assertEquals(createVector(0, 0, 0, 4), angularCoordinates.getRotationRate());
    double dt = 10.0;
    TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(0.0, shifted.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
    Assert.assertEquals(0.0, shifted.getRotationRate().getNorm().getReal(), 1.0e-15);
    Assert.assertEquals(0.0,
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-15);
}

---

From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java

@Test
public void testShift() throws OrekitException {
    double rate = 2 * FastMath.PI / (12 * 60);
    TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates = new TimeStampedFieldAngularCoordinates<DerivativeStructure>(
            AbsoluteDate.J2000_EPOCH, createRotation(1, 0, 0, 0, false),
            new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4));
    Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
    double dt = 10.0;
    double alpha = rate * dt;
    TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
    Assert.assertEquals(alpha,/* w  w  w . j  a  v a 2  s.  c o  m*/
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> xSat = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
    Assert.assertEquals(0.0,
            xSat.subtract(createVector(FastMath.cos(alpha), FastMath.sin(alpha), 0, 4)).getNorm().getReal(),
            1.0e-10);
    FieldVector3D<DerivativeStructure> ySat = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
    Assert.assertEquals(0.0,
            ySat.subtract(createVector(-FastMath.sin(alpha), FastMath.cos(alpha), 0, 4)).getNorm().getReal(),
            1.0e-10);
    FieldVector3D<DerivativeStructure> zSat = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
    Assert.assertEquals(0.0, zSat.subtract(createVector(0, 0, 1, 4)).getNorm().getReal(), 1.0e-10);

}

---

From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java

@Test
public void testSpin() throws OrekitException {
    double rate = 2 * FastMath.PI / (12 * 60);
    TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates = new TimeStampedFieldAngularCoordinates<DerivativeStructure>(
            AbsoluteDate.J2000_EPOCH, createRotation(0.48, 0.64, 0.36, 0.48, false),
            new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4));
    Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
    double dt = 10.0;
    TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
    Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
    Assert.assertEquals(rate * dt,//  w w  w . ja v  a2 s  .c  o m
            FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation()
            .applyInverseTo(createVector(1, 0, 0, 4));
    FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation()
            .applyInverseTo(createVector(0, 1, 0, 4));
    FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation()
            .applyInverseTo(createVector(0, 0, 1, 4));
    FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation()
            .applyInverseTo(createVector(1, 0, 0, 4));
    FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation()
            .applyInverseTo(createVector(0, 1, 0, 4));
    FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation()
            .applyInverseTo(createVector(0, 0, 1, 4));
    Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(),
            1.0e-10);
    Assert.assertEquals(FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(),
            1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
    Assert.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(),
            1.0e-10);
    Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(),
            1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
    Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
    Assert.assertEquals(1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);

    FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates
            .estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
    Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(),
            1.0e-10);

    FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(),
            angularCoordinates.getRotation(), dt);
    Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);

}

---

From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java

@Test
public void testNoCommute() {
    TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 = new TimeStampedFieldAngularCoordinates<DerivativeStructure>(
            AbsoluteDate.J2000_EPOCH, createRotation(0.48, 0.64, 0.36, 0.48, false), createVector(0, 0, 0, 4),
            createVector(0, 0, 0, 4));/*from  w  w  w. j a va 2  s  .c  o  m*/
    TimeStampedFieldAngularCoordinates<DerivativeStructure> ac2 = new TimeStampedFieldAngularCoordinates<DerivativeStructure>(
            AbsoluteDate.J2000_EPOCH, createRotation(0.36, -0.48, 0.48, 0.64, false), createVector(0, 0, 0, 4),
            createVector(0, 0, 0, 4));

    TimeStampedFieldAngularCoordinates<DerivativeStructure> add12 = ac1.addOffset(ac2);
    TimeStampedFieldAngularCoordinates<DerivativeStructure> add21 = ac2.addOffset(ac1);

    // the rotations are really different from each other
    Assert.assertEquals(2.574, FieldRotation.distance(add12.getRotation(), add21.getRotation()).getReal(),
            1.0e-3);

}

---

• «
• 1
• 2
• »