List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldRotation toRotation
public Rotation toRotation()
From source file:org.orekit.propagation.numerical.Jacobianizer.java
/** Compute acceleration and derivatives with respect to state. * @param date current date/*from ww w . jav a2s . com*/ * @param frame inertial reference frame for state (both orbit and attitude) * @param position position of spacecraft in reference frame * @param velocity velocity of spacecraft in reference frame * @param rotation orientation (attitude) of the spacecraft with respect to reference frame * @param mass spacecraft mass * @return acceleration with derivatives * @exception OrekitException if the underlying force models cannot compute the acceleration */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass) throws OrekitException { final int parameters = mass.getFreeParameters(); final int order = mass.getOrder(); // estimate the scalar velocity step, assuming energy conservation // and hence differentiating equation V = sqrt(mu (2/r - 1/a)) final Vector3D p0 = position.toVector3D(); final Vector3D v0 = velocity.toVector3D(); final double r2 = p0.getNormSq(); final double hVel = mu * hPos / (v0.getNorm() * r2); // estimate mass step, applying the same relative value as position final double hMass = mass.getValue() * hPos / FastMath.sqrt(r2); // compute nominal acceleration final AccelerationRetriever nominal = new AccelerationRetriever(); computeShiftedAcceleration(nominal, date, frame, p0, v0, rotation.toRotation(), mass.getValue()); final double[] a0 = nominal.getAcceleration().toArray(); // compute accelerations with shifted states final AccelerationRetriever shifted = new AccelerationRetriever(); // shift position by hPos alon x, y and z computeShiftedAcceleration(shifted, date, frame, shift(position, 0, hPos), v0, shift(rotation, 0, hPos), shift(mass, 0, hPos)); final double[] derPx = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, shift(position, 1, hPos), v0, shift(rotation, 1, hPos), shift(mass, 1, hPos)); final double[] derPy = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, shift(position, 2, hPos), v0, shift(rotation, 2, hPos), shift(mass, 2, hPos)); final double[] derPz = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); // shift velocity by hVel alon x, y and z computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 3, hVel), shift(rotation, 3, hVel), shift(mass, 3, hVel)); final double[] derVx = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 4, hVel), shift(rotation, 4, hVel), shift(mass, 4, hVel)); final double[] derVy = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 5, hVel), shift(rotation, 5, hVel), shift(mass, 5, hVel)); final double[] derVz = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); final double[] derM; if (parameters < 7) { derM = null; } else { // shift mass by hMass computeShiftedAcceleration(shifted, date, frame, p0, v0, shift(rotation, 6, hMass), shift(mass, 6, hMass)); derM = new Vector3D(1 / hMass, shifted.getAcceleration(), -1 / hMass, nominal.getAcceleration()) .toArray(); } final double[] derivatives = new double[1 + parameters]; final DerivativeStructure[] accDer = new DerivativeStructure[3]; for (int i = 0; i < 3; ++i) { // first element is value of acceleration derivatives[0] = a0[i]; // next three elements are derivatives with respect to position derivatives[1] = derPx[i]; derivatives[2] = derPy[i]; derivatives[3] = derPz[i]; // next three elements are derivatives with respect to velocity derivatives[4] = derVx[i]; derivatives[5] = derVy[i]; derivatives[6] = derVz[i]; if (derM != null) { derivatives[7] = derM[i]; } accDer[i] = new DerivativeStructure(parameters, order, derivatives); } return new FieldVector3D<DerivativeStructure>(accDer); }
From source file:org.orekit.utils.FieldAngularCoordinatesTest.java
@Test public void testToAC() { Random random = new Random(0xc9b4cf6c371108e0l); for (int i = 0; i < 100; ++i) { FieldRotation<DerivativeStructure> r = randomRotation(random); FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3); FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3); FieldAngularCoordinates<DerivativeStructure> acds = new FieldAngularCoordinates<DerivativeStructure>(r, o, a);/*from w ww .j av a 2 s. co m*/ AngularCoordinates ac = acds.toAngularCoordinates(); Assert.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15); Assert.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15); } }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java
@Test public void testToAC() { Random random = new Random(0xc9b4cf6c371108e0l); for (int i = 0; i < 100; ++i) { FieldRotation<DerivativeStructure> r = randomRotation(random); FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3); FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3); TimeStampedFieldAngularCoordinates<DerivativeStructure> acds = new TimeStampedFieldAngularCoordinates<DerivativeStructure>( AbsoluteDate.J2000_EPOCH, r, o, a); AngularCoordinates ac = acds.toAngularCoordinates(); Assert.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15); Assert.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15); Assert.assertEquals(0, FieldVector3D.distance(a, ac.getRotationAcceleration()).getReal(), 1.0e-15); }/*from www . ja v a 2 s .c o m*/ }