List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D FieldVector3D
public FieldVector3D(final T x, final T y, final T z)
From source file:org.orekit.bodies.FieldGeodeticPoint.java
/** Get the direction above the point, expressed in parent shape frame. * <p>The zenith direction is defined as the normal to local horizontal plane.</p> * @return unit vector in the zenith direction * @see #getNadir()/*from www . j a va2s . co m*/ */ public FieldVector3D<T> getZenith() { if (zenith == null) { final T cosLat = latitude.cos(); final T sinLat = latitude.sin(); final T cosLon = longitude.cos(); final T sinLon = longitude.sin(); zenith = new FieldVector3D<T>(cosLon.multiply(cosLat), sinLon.multiply(cosLat), sinLat); } return zenith; }
From source file:org.orekit.bodies.FieldGeodeticPoint.java
/** Get the direction to the north of point, expressed in parent shape frame. * <p>The north direction is defined in the horizontal plane * (normal to zenith direction) and following the local meridian.</p> * @return unit vector in the north direction * @see #getSouth()// www . j a v a 2 s .c om */ public FieldVector3D<T> getNorth() { if (north == null) { final T cosLat = latitude.cos(); final T sinLat = latitude.sin(); final T cosLon = longitude.cos(); final T sinLon = longitude.sin(); north = new FieldVector3D<T>(cosLon.multiply(sinLat).negate(), sinLon.multiply(sinLat).negate(), cosLat); } return north; }
From source file:org.orekit.bodies.FieldGeodeticPoint.java
/** Get the direction to the east of point, expressed in parent shape frame. * <p>The east direction is defined in the horizontal plane * in order to complete direct triangle (east, north, zenith).</p> * @return unit vector in the east direction * @see #getWest()//from w ww . j a v a 2s. c o m */ public FieldVector3D<T> getEast() { if (east == null) { east = new FieldVector3D<T>(longitude.sin().negate(), longitude.cos(), longitude.getField().getZero()); } return east; }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** Transform a surface-relative point to a Cartesian point. * @param point surface-relative point, using time as the single derivation parameter * @return point at the same location but as a Cartesian point including derivatives */// w ww. j a v a 2 s.c o m public PVCoordinates transform(final FieldGeodeticPoint<DerivativeStructure> point) { final DerivativeStructure latitude = point.getLatitude(); final DerivativeStructure longitude = point.getLongitude(); final DerivativeStructure altitude = point.getAltitude(); final DerivativeStructure cLambda = longitude.cos(); final DerivativeStructure sLambda = longitude.sin(); final DerivativeStructure cPhi = latitude.cos(); final DerivativeStructure sPhi = latitude.sin(); final DerivativeStructure n = sPhi.multiply(sPhi).multiply(e2).subtract(1.0).negate().sqrt().reciprocal() .multiply(getA()); final DerivativeStructure r = n.add(altitude).multiply(cPhi); return new PVCoordinates(new FieldVector3D<DerivativeStructure>(r.multiply(cLambda), r.multiply(sLambda), sPhi.multiply(altitude.add(n.multiply(g2))))); }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> radiationPressureAcceleration(final AbsoluteDate date, final Frame frame, final Vector3D position, final Rotation rotation, final double mass, final Vector3D flux, final String paramName) throws OrekitException { if (flux.getNormSq() < Precision.SAFE_MIN) { // null illumination (we are probably in umbra) final DerivativeStructure zero = new DerivativeStructure(1, 1, 0.0); return new FieldVector3D<DerivativeStructure>(zero, zero, zero); }//w w w.ja va 2s .co m final DerivativeStructure absorptionCoeffDS; final DerivativeStructure specularReflectionCoeffDS; if (ABSORPTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, 0, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, specularReflectionCoeff); } else if (REFLECTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, 0, specularReflectionCoeff); } else { throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, paramName, ABSORPTION_COEFFICIENT + ", " + REFLECTION_COEFFICIENT); } final DerivativeStructure diffuseReflectionCoeffDS = absorptionCoeffDS.add(specularReflectionCoeffDS) .subtract(1).negate(); // radiation flux in spacecraft frame final Vector3D fluxSat = rotation.applyTo(flux); // solar array contribution Vector3D normal = getNormal(date, frame, position, rotation); double dot = Vector3D.dotProduct(normal, fluxSat); if (dot > 0) { // the solar array is illuminated backward, // fix signs to compute contribution correctly dot = -dot; normal = normal.negate(); } FieldVector3D<DerivativeStructure> force = facetRadiationAcceleration(normal, solarArrayArea, fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS); // body facets contribution for (final Facet bodyFacet : facets) { normal = bodyFacet.getNormal(); dot = Vector3D.dotProduct(normal, fluxSat); if (dot < 0) { // the facet intercepts the incoming flux force = force.add(facetRadiationAcceleration(normal, bodyFacet.getArea(), fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS)); } } // convert to inertial return FieldRotation.applyInverseTo(rotation, new FieldVector3D<DerivativeStructure>(1.0 / mass, force)); }
From source file:org.orekit.forces.drag.DragForce.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 { // retrieve derivation properties final int parameters = mass.getFreeParameters(); final int order = mass.getOrder(); // get atmosphere properties in atmosphere own frame final Frame atmFrame = atmosphere.getFrame(); final Transform toBody = frame.getTransformTo(atmFrame, date); final FieldVector3D<DerivativeStructure> posBodyDS = toBody.transformPosition(position); final Vector3D posBody = posBodyDS.toVector3D(); final double rho = atmosphere.getDensity(date, posBody, atmFrame); final Vector3D vAtmBody = atmosphere.getVelocity(date, posBody, atmFrame); // we consider that at first order the atmosphere velocity in atmosphere frame // does not depend on local position; however atmosphere velocity in inertial // frame DOES depend on position since the transform between the frames depends // on it, due to central body rotation rate and velocity composition. // So we use the transform to get the correct partial derivatives on vAtm final FieldVector3D<DerivativeStructure> vAtmBodyDS = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(parameters, order, vAtmBody.getX()), new DerivativeStructure(parameters, order, vAtmBody.getY()), new DerivativeStructure(parameters, order, vAtmBody.getZ())); final FieldPVCoordinates<DerivativeStructure> pvAtmBody = new FieldPVCoordinates<DerivativeStructure>( posBodyDS, vAtmBodyDS);//www.java 2 s . co m final FieldPVCoordinates<DerivativeStructure> pvAtm = toBody.getInverse().transformPVCoordinates(pvAtmBody); // now we can compute relative velocity, it takes into account partial derivatives with respect to position final FieldVector3D<DerivativeStructure> relativeVelocity = pvAtm.getVelocity().subtract(velocity); // compute acceleration with all its partial derivatives return spacecraft.dragAcceleration(date, frame, position, rotation, mass, rho, relativeVelocity); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName) throws OrekitException, IllegalArgumentException { complainIfNotSupported(paramName);/*from w ww . j a va 2 s. c o m*/ // get the position in body frame final AbsoluteDate date = s.getDate(); final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); // gradient of the non-central part of the gravity field final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position))); return new FieldVector3D<DerivativeStructure>( new DerivativeStructure(1, 1, gInertial.getX(), gInertial.getX() / mu), new DerivativeStructure(1, 1, gInertial.getY(), gInertial.getY() / mu), new DerivativeStructure(1, 1, gInertial.getZ(), gInertial.getZ() / mu)); }
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 *///from w w w.j a va 2s .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.numerical.Jacobianizer.java
/** Compute acceleration and derivatives with respect to parameter. * @param s current state//from w w w .j a v a 2 s . c om * @param paramName parameter with respect to which derivation is desired * @return acceleration with derivatives * @exception OrekitException if the underlying force models cannot compute the acceleration * or the parameter is not supported */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName) throws OrekitException { if (!hParam.containsKey(paramName)) { // build the list of supported parameters final StringBuilder builder = new StringBuilder(); for (final String available : hParam.keySet()) { if (builder.length() > 0) { builder.append(", "); } builder.append(available); } throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, paramName, builder.toString()); } final double hP = hParam.get(paramName); final AccelerationRetriever nominal = new AccelerationRetriever(); nominal.setOrbit(s.getOrbit()); forceModel.addContribution(s, nominal); final double nx = nominal.getAcceleration().getX(); final double ny = nominal.getAcceleration().getY(); final double nz = nominal.getAcceleration().getZ(); final double paramValue = forceModel.getParameter(paramName); forceModel.setParameter(paramName, paramValue + hP); final AccelerationRetriever shifted = new AccelerationRetriever(); shifted.setOrbit(s.getOrbit()); forceModel.addContribution(s, shifted); final double sx = shifted.getAcceleration().getX(); final double sy = shifted.getAcceleration().getY(); final double sz = shifted.getAcceleration().getZ(); forceModel.setParameter(paramName, paramValue); return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, nx, (sx - nx) / hP), new DerivativeStructure(1, 1, ny, (sy - ny) / hP), new DerivativeStructure(1, 1, nz, (sz - nz) / hP)); }
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) {//w ww.j ava 2 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; }