List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D getZ
public T getZ()
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** Transform a Cartesian point to a surface-relative point. * @param point Cartesian point//from www . j a va2 s . c om * @param frame frame in which Cartesian point is expressed * @param date date of the computation (used for frames conversions) * @return point at the same location but as a surface-relative point, * using time as the single derivation parameter * @exception OrekitException if point cannot be converted to body frame */ public FieldGeodeticPoint<DerivativeStructure> transform(final PVCoordinates point, final Frame frame, final AbsoluteDate date) throws OrekitException { // transform point to body frame final Transform toBody = frame.getTransformTo(bodyFrame, date); final PVCoordinates pointInBodyFrame = toBody.transformPVCoordinates(point); final FieldVector3D<DerivativeStructure> p = pointInBodyFrame.toDerivativeStructureVector(2); final DerivativeStructure pr2 = p.getX().multiply(p.getX()).add(p.getY().multiply(p.getY())); final DerivativeStructure pr = pr2.sqrt(); final DerivativeStructure pz = p.getZ(); // project point on the ellipsoid surface final TimeStampedPVCoordinates groundPoint = projectToGround( new TimeStampedPVCoordinates(date, pointInBodyFrame), bodyFrame); final FieldVector3D<DerivativeStructure> gp = groundPoint.toDerivativeStructureVector(2); final DerivativeStructure gpr2 = gp.getX().multiply(gp.getX()).add(gp.getY().multiply(gp.getY())); final DerivativeStructure gpr = gpr2.sqrt(); final DerivativeStructure gpz = gp.getZ(); // relative position of test point with respect to its ellipse sub-point final DerivativeStructure dr = pr.subtract(gpr); final DerivativeStructure dz = pz.subtract(gpz); final double insideIfNegative = g2 * (pr2.getReal() - ae2) + pz.getReal() * pz.getReal(); return new FieldGeodeticPoint<DerivativeStructure>(DerivativeStructure.atan2(gpz, gpr.multiply(g2)), DerivativeStructure.atan2(p.getY(), p.getX()), DerivativeStructure.hypot(dr, dz).copySign(insideIfNegative)); }
From source file:org.orekit.forces.AbstractForceModelTest.java
protected void checkParameterDerivative(SpacecraftState state, ForceModel forceModel, String name, double hFactor, double tol) throws OrekitException { try {//from www . j av a2 s.c om forceModel.accelerationDerivatives(state, "not a parameter"); Assert.fail("an exception should have been thrown"); } catch (UnknownParameterException upe) { // expected } catch (OrekitException oe) { // expected Assert.assertEquals(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, oe.getSpecifier()); } FieldVector3D<DerivativeStructure> accDer = forceModel.accelerationDerivatives(state, name); Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1), accDer.getY().getPartialDerivative(1), accDer.getZ().getPartialDerivative(1)); AccelerationRetriever accelerationRetriever = new AccelerationRetriever(); double p0 = forceModel.getParameter(name); double hParam = hFactor * forceModel.getParameter(name); forceModel.setParameter(name, p0 - 1 * hParam); Assert.assertEquals(p0 - 1 * hParam, forceModel.getParameter(name), 1.0e-10); forceModel.addContribution(state, accelerationRetriever); final Vector3D gammaM1h = accelerationRetriever.getAcceleration(); forceModel.setParameter(name, p0 + 1 * hParam); Assert.assertEquals(p0 + 1 * hParam, forceModel.getParameter(name), 1.0e-10); forceModel.addContribution(state, accelerationRetriever); final Vector3D gammaP1h = accelerationRetriever.getAcceleration(); final Vector3D reference = new Vector3D(1 / (2 * hParam), gammaP1h.subtract(gammaM1h)); final Vector3D delta = derivative.subtract(reference); Assert.assertEquals(0, delta.getNorm(), tol * reference.getNorm()); }
From source file:org.orekit.propagation.analytical.EcksteinHechlerPropagator.java
/** Convert circular parameters <em>with derivatives</em> to Cartesian coordinates. * @param date date of the orbital parameters * @param parameters circular parameters (a, ex, ey, i, raan, alphaM) * @return Cartesian coordinates consistent with values and derivatives */// w w w. j av a 2 s .c o m private TimeStampedPVCoordinates toCartesian(final AbsoluteDate date, final DerivativeStructure[] parameters) { // evaluate coordinates in the orbit canonical reference frame final DerivativeStructure cosOmega = parameters[4].cos(); final DerivativeStructure sinOmega = parameters[4].sin(); final DerivativeStructure cosI = parameters[3].cos(); final DerivativeStructure sinI = parameters[3].sin(); final DerivativeStructure alphaE = meanToEccentric(parameters[5], parameters[1], parameters[2]); final DerivativeStructure cosAE = alphaE.cos(); final DerivativeStructure sinAE = alphaE.sin(); final DerivativeStructure ex2 = parameters[1].multiply(parameters[1]); final DerivativeStructure ey2 = parameters[2].multiply(parameters[2]); final DerivativeStructure exy = parameters[1].multiply(parameters[2]); final DerivativeStructure q = ex2.add(ey2).subtract(1).negate().sqrt(); final DerivativeStructure beta = q.add(1).reciprocal(); final DerivativeStructure bx2 = beta.multiply(ex2); final DerivativeStructure by2 = beta.multiply(ey2); final DerivativeStructure bxy = beta.multiply(exy); final DerivativeStructure u = bxy.multiply(sinAE) .subtract(parameters[1].add(by2.subtract(1).multiply(cosAE))); final DerivativeStructure v = bxy.multiply(cosAE) .subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE))); final DerivativeStructure x = parameters[0].multiply(u); final DerivativeStructure y = parameters[0].multiply(v); // canonical orbit reference frame final FieldVector3D<DerivativeStructure> p = new FieldVector3D<DerivativeStructure>( x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))), x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))), y.multiply(sinI)); // dispatch derivatives final Vector3D p0 = new Vector3D(p.getX().getValue(), p.getY().getValue(), p.getZ().getValue()); final Vector3D p1 = new Vector3D(p.getX().getPartialDerivative(1), p.getY().getPartialDerivative(1), p.getZ().getPartialDerivative(1)); final Vector3D p2 = new Vector3D(p.getX().getPartialDerivative(2), p.getY().getPartialDerivative(2), p.getZ().getPartialDerivative(2)); return new TimeStampedPVCoordinates(date, p0, p1, p2); }
From source file:org.orekit.propagation.events.ElevationExtremumDetector.java
/** Compute the value of the detection function. * <p>// ww w .j a va2s .co m * The value is the spacecraft elevation first time derivative. * </p> * @param s the current state information: date, kinematics, attitude * @return spacecraft elevation first time derivative * @exception OrekitException if some specific error occurs */ public double g(final SpacecraftState s) throws OrekitException { // get position, velocity acceleration of spacecraft in topocentric frame final Transform inertToTopo = s.getFrame().getTransformTo(topo, s.getDate()); final TimeStampedPVCoordinates pvTopo = inertToTopo.transformPVCoordinates(s.getPVCoordinates()); // convert the coordinates to DerivativeStructure based vector // instead of having vector position, then vector velocity then vector acceleration // we get one vector and each coordinate is a DerivativeStructure containing // value, first time derivative (we don't need second time derivative here) final FieldVector3D<DerivativeStructure> pvDS = pvTopo.toDerivativeStructureVector(1); // compute elevation and its first time derivative final DerivativeStructure elevation = pvDS.getZ().divide(pvDS.getNorm()).asin(); // return elevation first time derivative return elevation.getPartialDerivative(1); }
From source file:org.orekit.propagation.numerical.Jacobianizer.java
/** Shift a vector. * @param nominal nominal vector/* w w w . j a v a 2 s .c om*/ * @param index index of the variable with respect to which we shift * @param h shift step * @return shifted vector */ private Vector3D shift(final FieldVector3D<DerivativeStructure> nominal, final int index, final double h) { final double[] delta = new double[nominal.getX().getFreeParameters()]; delta[index] = h; return new Vector3D(nominal.getX().taylor(delta), nominal.getY().taylor(delta), nominal.getZ().taylor(delta)); }
From source file:org.orekit.propagation.numerical.PartialDerivativesEquations.java
/** {@inheritDoc} */ public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) throws OrekitException { final int dim = 3; // Lazy initialization if (dirty) {/*from w w w. j a v a2 s .c o m*/ // if step has not been set by user, set a default value if (Double.isNaN(hPos)) { hPos = FastMath.sqrt(Precision.EPSILON) * s.getPVCoordinates().getPosition().getNorm(); } // set up derivatives providers derivativesProviders.clear(); derivativesProviders.addAll(propagator.getForceModels()); derivativesProviders.add(propagator.getNewtonianAttractionForceModel()); // check all parameters are handled by at least one Jacobian provider for (final ParameterConfiguration param : selectedParameters) { final String parameterName = param.getParameterName(); boolean found = false; for (final ForceModel provider : derivativesProviders) { if (provider.isSupported(parameterName)) { param.setProvider(provider); found = true; } } if (!found) { // build the list of supported parameters, avoiding duplication final SortedSet<String> set = new TreeSet<String>(); for (final ForceModel provider : derivativesProviders) { for (final String forceModelParameter : provider.getParametersNames()) { set.add(forceModelParameter); } } final StringBuilder builder = new StringBuilder(); for (final String forceModelParameter : set) { if (builder.length() > 0) { builder.append(", "); } builder.append(forceModelParameter); } throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, parameterName, builder.toString()); } } // check the numbers of parameters and matrix size agree if (selectedParameters.size() != paramDim) { throw new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH, paramDim, selectedParameters.size()); } dAccdParam = new double[dim]; dAccdPos = new double[dim][dim]; dAccdVel = new double[dim][dim]; dAccdM = (stateDim > 6) ? new double[dim] : null; dirty = false; } // initialize acceleration Jacobians to zero for (final double[] row : dAccdPos) { Arrays.fill(row, 0.0); } for (final double[] row : dAccdVel) { Arrays.fill(row, 0.0); } if (dAccdM != null) { Arrays.fill(dAccdM, 0.0); } // prepare derivation variables, 3 for position, 3 for velocity and optionally 1 for mass final int nbVars = (dAccdM == null) ? 6 : 7; // position corresponds three free parameters final Vector3D position = s.getPVCoordinates().getPosition(); final FieldVector3D<DerivativeStructure> dsP = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 0, position.getX()), new DerivativeStructure(nbVars, 1, 1, position.getY()), new DerivativeStructure(nbVars, 1, 2, position.getZ())); // velocity corresponds three free parameters final Vector3D velocity = s.getPVCoordinates().getVelocity(); final FieldVector3D<DerivativeStructure> dsV = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 3, velocity.getX()), new DerivativeStructure(nbVars, 1, 4, velocity.getY()), new DerivativeStructure(nbVars, 1, 5, velocity.getZ())); // mass corresponds either to a constant or to one free parameter final DerivativeStructure dsM = (dAccdM == null) ? new DerivativeStructure(nbVars, 1, s.getMass()) : new DerivativeStructure(nbVars, 1, 6, s.getMass()); // we should compute attitude partial derivatives with respect to position/velocity // see issue #200 final Rotation rotation = s.getAttitude().getRotation(); final FieldRotation<DerivativeStructure> dsR = new FieldRotation<DerivativeStructure>( new DerivativeStructure(nbVars, 1, rotation.getQ0()), new DerivativeStructure(nbVars, 1, rotation.getQ1()), new DerivativeStructure(nbVars, 1, rotation.getQ2()), new DerivativeStructure(nbVars, 1, rotation.getQ3()), false); // compute acceleration Jacobians for (final ForceModel derivativesProvider : derivativesProviders) { final FieldVector3D<DerivativeStructure> acceleration = derivativesProvider .accelerationDerivatives(s.getDate(), s.getFrame(), dsP, dsV, dsR, dsM); addToRow(acceleration.getX(), 0); addToRow(acceleration.getY(), 1); addToRow(acceleration.getZ(), 2); } // the variational equations of the complete state Jacobian matrix have the // following form for 7x7, i.e. when mass partial derivatives are also considered // (when mass is not considered, only the A, B, D and E matrices are used along // with their derivatives): // [ | | ] [ | | ] [ | | ] // [ Adot | Bdot | Cdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ A | B | C ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ | | ] [ | | ] [ | | ] // [ Ddot | Edot | Fdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ D | E | F ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ Gdot | Hdot | Idot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ G | H | I ] // The A, B, D and E sub-matrices and their derivatives (Adot ...) are 3x3 matrices, // the C and F sub-matrices and their derivatives (Cdot ...) are 3x1 matrices, // the G and H sub-matrices and their derivatives (Gdot ...) are 1x3 matrices, // the I sub-matrix and its derivative (Idot) is a 1x1 matrix. // The expanded multiplication above can be rewritten to take into account // the fixed values found in the sub-matrices in the left factor. This leads to: // [ Adot ] = [ D ] // [ Bdot ] = [ E ] // [ Cdot ] = [ F ] // [ Ddot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ D ] + [ dAcc/dm ] * [ G ] // [ Edot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ E ] + [ dAcc/dm ] * [ H ] // [ Fdot ] = [ dAcc/dPos ] * [ C ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dm ] * [ I ] // [ Gdot ] = [ 0 ] // [ Hdot ] = [ 0 ] // [ Idot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (A, B, ... I) matrices into the single dimension array p and of the mapping of the // (Adot, Bdot, ... Idot) matrices into the single dimension array pDot. // copy D, E and F into Adot, Bdot and Cdot final double[] p = s.getAdditionalState(getName()); System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim); // compute Ddot, Edot and Fdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; for (int j = 0; j < stateDim; ++j) { pDot[(dim + i) * stateDim + j] = dAdPi[0] * p[j] + dAdPi[1] * p[j + stateDim] + dAdPi[2] * p[j + 2 * stateDim] + dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[j + 6 * stateDim]); } } if (dAccdM != null) { // set Gdot, Hdot and Idot to 0 Arrays.fill(pDot, 6 * stateDim, 7 * stateDim, 0.0); } for (int k = 0; k < paramDim; ++k) { // compute the acceleration gradient with respect to current parameter final ParameterConfiguration param = selectedParameters.get(k); final ForceModel provider = param.getProvider(); final FieldVector3D<DerivativeStructure> accDer = provider.accelerationDerivatives(s, param.getParameterName()); dAccdParam[0] = accDer.getX().getPartialDerivative(1); dAccdParam[1] = accDer.getY().getPartialDerivative(1); dAccdParam[2] = accDer.getZ().getPartialDerivative(1); // the variational equations of the parameters Jacobian matrix are computed // one column at a time, they have the following form: // [ ] [ | | ] [ ] [ ] // [ Jdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ J ] [ dVel/dParam = 0 ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ ] [ | | ] [ ] [ ] // [ Kdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ K ] + [ dAcc/dParam ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ Ldot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ L ] [ dmDot/dParam = 0 ] // The J and K sub-columns and their derivatives (Jdot ...) are 3 elements columns, // the L sub-colums and its derivative (Ldot) are 1 elements columns. // The expanded multiplication and addition above can be rewritten to take into // account the fixed values found in the sub-matrices in the left factor. This leads to: // [ Jdot ] = [ K ] // [ Kdot ] = [ dAcc/dPos ] * [ J ] + [ dAcc/dVel ] * [ K ] + [ dAcc/dm ] * [ L ] + [ dAcc/dParam ] // [ Ldot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (J, K, L) columns into the single dimension array p and of the mapping of the // (Jdot, Kdot, Ldot) columns into the single dimension array pDot. // copy K into Jdot final int columnTop = stateDim * stateDim + k; pDot[columnTop] = p[columnTop + 3 * paramDim]; pDot[columnTop + paramDim] = p[columnTop + 4 * paramDim]; pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim]; // compute Kdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; pDot[columnTop + (dim + i) * paramDim] = dAccdParam[i] + dAdPi[0] * p[columnTop] + dAdPi[1] * p[columnTop + paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] + dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[columnTop + 6 * paramDim]); } if (dAccdM != null) { // set Ldot to 0 pDot[columnTop + 6 * paramDim] = 0; } } // these equations have no effect of the main state itself return null; }
From source file:org.orekit.utils.FieldPVCoordinatesTest.java
@Test @Deprecated // to be removed when FieldPVCoordinates.interpolate is removed public void testInterpolatePolynomialPV() { Random random = new Random(0xae7771c9933407bdl); AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH; for (int i = 0; i < 20; ++i) { PolynomialFunction px = randomPolynomial(5, random); PolynomialFunction py = randomPolynomial(5, random); PolynomialFunction pz = randomPolynomial(5, random); PolynomialFunction pxDot = px.polynomialDerivative(); PolynomialFunction pyDot = py.polynomialDerivative(); PolynomialFunction pzDot = pz.polynomialDerivative(); List<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { FieldVector3D<DerivativeStructure> position = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, 0, px.value(dt)), new DerivativeStructure(3, 1, 1, py.value(dt)), new DerivativeStructure(3, 1, 2, pz.value(dt))); FieldVector3D<DerivativeStructure> velocity = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, pxDot.value(dt)), new DerivativeStructure(3, 1, pyDot.value(dt)), new DerivativeStructure(3, 1, pzDot.value(dt))); sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt), new FieldPVCoordinates<DerivativeStructure>(position, velocity))); }/* w ww . j av a 2 s. c om*/ for (double dt = 0; dt < 1.0; dt += 0.01) { FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates .interpolate(t0.shiftedBy(dt), true, sample); FieldVector3D<DerivativeStructure> p = interpolated.getPosition(); FieldVector3D<DerivativeStructure> v = interpolated.getVelocity(); Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(1, p.getX().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getY().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(1, p.getY().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getY().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getZ().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getZ().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(1, p.getZ().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-15 * v.getNorm().getValue()); } } }
From source file:org.orekit.utils.PVCoordinates.java
/** Builds a PVCoordinates triplet from a {@link FieldVector3D}<{@link DerivativeStructure}>. * <p>//from ww w . j a v a 2 s.c om * The vector components must have time as their only derivation parameter and * have consistent derivation orders. * </p> * @param p vector with time-derivatives embedded within the coordinates */ public PVCoordinates(final FieldVector3D<DerivativeStructure> p) { position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal()); if (p.getX().getOrder() >= 1) { velocity = new Vector3D(p.getX().getPartialDerivative(1), p.getY().getPartialDerivative(1), p.getZ().getPartialDerivative(1)); if (p.getX().getOrder() >= 2) { acceleration = new Vector3D(p.getX().getPartialDerivative(2), p.getY().getPartialDerivative(2), p.getZ().getPartialDerivative(2)); } else { acceleration = Vector3D.ZERO; } } else { velocity = Vector3D.ZERO; acceleration = Vector3D.ZERO; } }
From source file:org.orekit.utils.PVCoordinatesDSTest.java
@Test public void testInterpolatePolynomialPV() { Random random = new Random(0xae7771c9933407bdl); AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH; for (int i = 0; i < 20; ++i) { PolynomialFunction px = randomPolynomial(5, random); PolynomialFunction py = randomPolynomial(5, random); PolynomialFunction pz = randomPolynomial(5, random); PolynomialFunction pxDot = px.polynomialDerivative(); PolynomialFunction pyDot = py.polynomialDerivative(); PolynomialFunction pzDot = pz.polynomialDerivative(); List<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { FieldVector3D<DerivativeStructure> position = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, 0, px.value(dt)), new DerivativeStructure(3, 1, 1, py.value(dt)), new DerivativeStructure(3, 1, 2, pz.value(dt))); FieldVector3D<DerivativeStructure> velocity = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, pxDot.value(dt)), new DerivativeStructure(3, 1, pyDot.value(dt)), new DerivativeStructure(3, 1, pzDot.value(dt))); sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt), new FieldPVCoordinates<DerivativeStructure>(position, velocity))); }//from w w w . j av a 2s .c o m for (double dt = 0; dt < 1.0; dt += 0.01) { FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates .interpolate(t0.shiftedBy(dt), true, sample); FieldVector3D<DerivativeStructure> p = interpolated.getPosition(); FieldVector3D<DerivativeStructure> v = interpolated.getVelocity(); Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(1, p.getX().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getY().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(1, p.getY().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getY().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getZ().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getZ().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(1, p.getZ().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-15 * v.getNorm().getValue()); } } }
From source file:org.orekit.utils.PVCoordinatesDSTest.java
@Test public void testInterpolatePolynomialPositionOnly() { Random random = new Random(0x88740a12e4299003l); AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH; for (int i = 0; i < 20; ++i) { PolynomialFunction px = randomPolynomial(5, random); PolynomialFunction py = randomPolynomial(5, random); PolynomialFunction pz = randomPolynomial(5, random); PolynomialFunction pxDot = px.polynomialDerivative(); PolynomialFunction pyDot = py.polynomialDerivative(); PolynomialFunction pzDot = pz.polynomialDerivative(); List<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>>(); for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) { FieldVector3D<DerivativeStructure> position = createVector(px.value(dt), py.value(dt), pz.value(dt), 6);/*from w w w . j a v a2s. c o m*/ sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt), new FieldPVCoordinates<DerivativeStructure>(position, createVector(0, 0, 0, 6)))); } for (double dt = 0; dt < 1.0; dt += 0.01) { FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates .interpolate(t0.shiftedBy(dt), false, sample); FieldVector3D<DerivativeStructure> p = interpolated.getPosition(); FieldVector3D<DerivativeStructure> v = interpolated.getVelocity(); Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-14 * p.getNorm().getValue()); Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-14 * p.getNorm().getValue()); Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-14 * p.getNorm().getValue()); Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-14 * v.getNorm().getValue()); Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-14 * v.getNorm().getValue()); Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-14 * v.getNorm().getValue()); } } }