FieldEquinoctialParametersConverter.java
/* Copyright 2022-2026 Romain Serra
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.orbits;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.FieldSinCos;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitMessages;
import org.orekit.utils.FieldPVCoordinates;
/**
* Class for converting between equinoctial elements and Cartesian coordinates (Field version).
* @param <T> type of the field element
* @author Romain Serra
* @see FieldEquinoctialParameters
* @see EquinoctialParametersConverter
* @since 14.0
*/
public class FieldEquinoctialParametersConverter<T extends CalculusFieldElement<T>> {
/** Central body gravitational parameter. */
private final T mu;
/**
* Constructor.
* @param mu central body gravitational parameter
*/
public FieldEquinoctialParametersConverter(final T mu) {
this.mu = mu;
}
/**
* Convert Cartesian coordinates to equinoctial elements.
* @param cartesian position and velocity in inertial frame
* @param positionAngleType type of position angle to use
* @return equinoctial elements
*/
public FieldEquinoctialParameters<T> toParameters(final FieldPVCoordinates<T> cartesian,
final PositionAngleType positionAngleType) {
// compute semi-major axis
final FieldVector3D<T> pvP = cartesian.getPosition();
final T r = pvP.getNorm2();
final T v2 = cartesian.getVelocity().getNorm2Sq();
final T rV2OnMu = r.multiply(v2).divide(mu);
final T a = r.divide(rV2OnMu.negate().add(2));
if (a.getReal() < 0.) {
throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
getClass().getName());
}
// compute inclination vector
final FieldVector3D<T> w = cartesian.getMomentum().normalize();
final T one = a.getField().getOne();
final T d = one.divide(one.add(w.getZ()));
final T hx = d.negate().multiply(w.getY());
final T hy = d.multiply(w.getX());
// compute true longitude argument
final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
final T trueLongitude = sLv.atan2(cLv);
// compute eccentricity vector
final T eSE = FieldVector3D.dotProduct(pvP, cartesian.getVelocity()).divide(a.multiply(mu).sqrt());
final T eCE = rV2OnMu.subtract(1);
final T e2 = eCE.square().add(eSE.square());
final T f = eCE.subtract(e2);
final T g = e2.negate().add(1).sqrt().multiply(eSE);
final T ex = a.multiply(f.multiply(cLv).add( g.multiply(sLv))).divide(r);
final T ey = a.multiply(f.multiply(sLv).subtract(g.multiply(cLv))).divide(r);
final FieldEquinoctialParameters<T> equinoctialParameters = new FieldEquinoctialParameters<>(a, ex, ey, hx, hy,
trueLongitude, PositionAngleType.TRUE);
return positionAngleType == PositionAngleType.TRUE ? equinoctialParameters :
equinoctialParameters.withPositionAngleType(positionAngleType);
}
/**
* Convert equinoctial elements to Cartesian coordinates.
* @param elements equinoctial elements
* @return position and velocity in inertial frame
*/
public FieldPVCoordinates<T> toCartesian(final FieldEquinoctialParameters<T> elements) {
// get equinoctial parameters
final T a = elements.a();
final T ex = elements.ex();
final T ey = elements.ey();
final T hx = elements.hx();
final T hy = elements.hy();
final T lE = elements.positionAngleType() == PositionAngleType.ECCENTRIC ? elements.longitudeArgument() :
elements.withPositionAngleType(PositionAngleType.ECCENTRIC).longitudeArgument();
// inclination-related intermediate parameters
final T hx2 = hx.square();
final T hy2 = hy.square();
final T one = a.getField().getOne();
final T factH = one.divide(hx2.add(1.0).add(hy2));
// reference axes defining the orbital plane
final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
final T uy = hx.multiply(hy).multiply(factH).multiply(2);
final T uz = hy.multiply(-2).multiply(factH);
final T vx = uy;
final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
final T vz = hx.multiply(factH).multiply(2);
// eccentricity-related intermediate parameters
final T ex2 = ex.square();
final T exey = ex.multiply(ey);
final T ey2 = ey.square();
final T e2 = ex2.add(ey2);
final T eta = one.subtract(e2).sqrt().add(1);
final T beta = one.divide(eta);
// eccentric longitude argument
final FieldSinCos<T> scLe = FastMath.sinCos(lE);
final T cLe = scLe.cos();
final T sLe = scLe.sin();
final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));
// coordinates of position and velocity in the orbital plane
final T x = a.multiply(one.subtract(beta.multiply(ey2)).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(ex));
final T y = a.multiply(one.subtract(beta.multiply(ex2)).multiply(sLe).add(beta .multiply(exey).multiply(cLe)).subtract(ey));
final T factor = mu.divide(a).sqrt().divide(one.subtract(exCeyS));
final T xdot = factor.multiply(sLe.negate().add(beta.multiply(ey).multiply(exCeyS)));
final T ydot = factor.multiply(cLe.subtract(beta.multiply(ex).multiply(exCeyS)));
final FieldVector3D<T> position =
new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
x.multiply(uy).add(y.multiply(vy)),
x.multiply(uz).add(y.multiply(vz)));
final FieldVector3D<T> velocity =
new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));
return new FieldPVCoordinates<>(position, velocity);
}
}