List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D FieldVector3D
public FieldVector3D(final T[] v) throws DimensionMismatchException
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** {@inheritDoc} */ 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 { // get the position in body frame final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D()); // compute gradient and Hessian final GradientHessian gh = gradientHessian(date, positionBody); // gradient of the non-central part of the gravity field final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray(); // Hessian of the non-central part of the gravity field final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false); final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix()); final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot); // distribute all partial derivatives in a compact acceleration vector final int parameters = mass.getFreeParameters(); final int order = mass.getOrder(); 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 (i.e. gradient of field) derivatives[0] = gInertial[i];/*from www . jav a2 s . c o m*/ // next three elements are one row of the Jacobian of acceleration (i.e. Hessian of field) derivatives[1] = hInertial.getEntry(i, 0); derivatives[2] = hInertial.getEntry(i, 1); derivatives[3] = hInertial.getEntry(i, 2); // next elements (three or four depending on mass being used or not) are left as 0 accDer[i] = new DerivativeStructure(parameters, order, derivatives); } return new FieldVector3D<DerivativeStructure>(accDer); }
From source file:org.orekit.propagation.numerical.Jacobianizer.java
/** Compute acceleration and derivatives with respect to state. * @param date current date/* ww w .j a v a 2 s. co m*/ * @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.TimeStampedFieldPVCoordinates.java
/** Interpolate position-velocity. * <p>// www.jav a 2 s.c o m * The interpolated instance is created by polynomial Hermite interpolation * ensuring velocity remains the exact derivative of position. * </p> * <p> * Note that even if first time derivatives (velocities) * 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 positions. * </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 * @param <T> the type of the field elements * @return a new position-velocity, interpolated at specified date */ @SuppressWarnings("unchecked") public static <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> interpolate( final AbsoluteDate date, final CartesianDerivativesFilter filter, final Collection<TimeStampedFieldPVCoordinates<T>> sample) { // get field properties final T prototype = sample.iterator().next().getPosition().getX(); final T zero = prototype.getField().getZero(); // set up an interpolator taking derivatives into account final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>(); // add sample points switch (filter) { case USE_P: // populate sample with position data, ignoring velocity for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) { final FieldVector3D<T> position = datedPV.getPosition(); interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)), position.toArray()); } break; case USE_PV: // populate sample with position and velocity data for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) { final FieldVector3D<T> position = datedPV.getPosition(); final FieldVector3D<T> velocity = datedPV.getVelocity(); interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)), position.toArray(), velocity.toArray()); } break; case USE_PVA: // populate sample with position, velocity and acceleration data for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) { final FieldVector3D<T> position = datedPV.getPosition(); final FieldVector3D<T> velocity = datedPV.getVelocity(); final FieldVector3D<T> acceleration = datedPV.getAcceleration(); interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)), position.toArray(), velocity.toArray(), acceleration.toArray()); } break; default: // this should never happen throw new OrekitInternalError(null); } // interpolate final T[][] p = interpolator.derivatives(zero, 2); // build a new interpolated instance return new TimeStampedFieldPVCoordinates<T>(date, new FieldVector3D<T>(p[0]), new FieldVector3D<T>(p[1]), new FieldVector3D<T>(p[2])); }