Commit 8ec4df0d authored by Bryan Cazabonne's avatar Bryan Cazabonne

Added field methods for Topocentric frame.

parent b359b932
......@@ -16,19 +16,25 @@
*/
package org.orekit.frames;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.analysis.UnivariateFunction;
import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
import org.hipparchus.analysis.solvers.UnivariateSolver;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
import org.orekit.bodies.BodyShape;
import org.orekit.bodies.FieldGeodeticPoint;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.errors.OrekitException;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.Constants;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedPVCoordinates;
......@@ -95,6 +101,19 @@ public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
return point;
}
/** Get the surface point defining the origin of the frame.
* @param <T> tyoe of the elements
* @param field of the elements
* @return surface point defining the origin of the frame
* @since 9.3
*/
public <T extends RealFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) {
final T zero = field.getZero();
return new FieldGeodeticPoint<>(zero.add(point.getLatitude()),
zero.add(point.getLongitude()),
zero.add(point.getAltitude()));
}
/** Get the zenith direction of topocentric frame, 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
......@@ -170,6 +189,27 @@ public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
return extPointTopo.getDelta();
}
/** Get the elevation of a point with regards to the local point.
* <p>The elevation is the angle between the local horizontal and
* the direction from local point to given point.</p>
* @param <T> type of the elements
* @param extPoint point for which elevation shall be computed
* @param frame frame in which the point is defined
* @param date computation date
* @return elevation of the point
* @since 9.3
*/
public <T extends RealFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame,
final FieldAbsoluteDate<T> date) {
// Transform given point from given frame to topocentric frame
final FieldTransform<T> t = frame.getTransformTo(this, date);
final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
// Elevation angle is PI/2 - angle between zenith and given point direction
return extPointTopo.getDelta();
}
/** Get the azimuth of a point with regards to the topocentric frame center point.
* <p>The azimuth is the angle between the North direction at local point and
* the projection in local horizontal plane of the direction from local point
......@@ -195,6 +235,33 @@ public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
}
/** Get the azimuth of a point with regards to the topocentric frame center point.
* <p>The azimuth is the angle between the North direction at local point and
* the projection in local horizontal plane of the direction from local point
* to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
* @param <T> type of the elements
* @param extPoint point for which elevation shall be computed
* @param frame frame in which the point is defined
* @param date computation date
* @return azimuth of the point
* @since 9.3
*/
public <T extends RealFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame,
final FieldAbsoluteDate<T> date) {
// Transform given point from given frame to topocentric frame
final FieldTransform<T> t = getTransformTo(frame, date).getInverse();
final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
// Compute azimuth
T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
if (azimuth.getReal() < 0.) {
azimuth = azimuth.add(MathUtils.TWO_PI);
}
return azimuth;
}
/** Get the range of a point with regards to the topocentric frame center point.
* @param extPoint point for which range shall be computed
* @param frame frame in which the point is defined
......@@ -213,6 +280,26 @@ public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
}
/** Get the range of a point with regards to the topocentric frame center point.
* @param <T> type of the elements
* @param extPoint point for which range shall be computed
* @param frame frame in which the point is defined
* @param date computation date
* @return range (distance) of the point
* @since 9.3
*/
public <T extends RealFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame,
final FieldAbsoluteDate<T> date) {
// Transform given point from given frame to topocentric frame
final FieldTransform<T> t = frame.getTransformTo(this, date);
final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
// Compute range
return extPointTopo.getNorm();
}
/** Get the range rate of a point with regards to the topocentric frame center point.
* @param extPV point/velocity for which range rate shall be computed
* @param frame frame in which the point is defined
......@@ -232,6 +319,27 @@ public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
}
/** Get the range rate of a point with regards to the topocentric frame center point.
* @param <T> type of the elements
* @param extPV point/velocity for which range rate shall be computed
* @param frame frame in which the point is defined
* @param date computation date
* @return range rate of the point (positive if point departs from frame)
* @since 9.3
*/
public <T extends RealFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame,
final FieldAbsoluteDate<T> date) {
// Transform given point from given frame to topocentric frame
final FieldTransform<T> t = frame.getTransformTo(this, date);
final FieldPVCoordinates<T> extPVTopo = t.transformPVCoordinates(extPV);
// Compute range rate (doppler) : relative rate along the line of sight
return FieldVector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()).divide(
extPVTopo.getPosition().getNorm());
}
/**
* Compute the limit visibility point for a satellite in a given direction.
* <p>
......
......@@ -34,7 +34,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -141,9 +140,7 @@ public class EstimatedViennaOneModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Set drivers reference date
for (final ParameterDriver driver : model.getParametersDrivers()) {
......@@ -278,9 +275,7 @@ public class EstimatedViennaOneModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Set drivers reference date
for (final ParameterDriver driver : model.getParametersDrivers()) {
......
......@@ -34,7 +34,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -147,9 +146,7 @@ public class EstimatedViennaThreeTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Add parameter as a variable
final List<ParameterDriver> drivers = model.getParametersDrivers();
......@@ -279,9 +276,7 @@ public class EstimatedViennaThreeTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Set up parameter drivers
for (final ParameterDriver driver : model.getParametersDrivers()) {
......
......@@ -36,7 +36,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -170,9 +169,7 @@ public class FieldGlobalMappingFunctionModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Compute mapping factors state derivatives
final DerivativeStructure[] factors = model.mappingFactors(dsElevation, zero, model.getParameters(field), dsDate);
......
......@@ -35,7 +35,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -236,9 +235,7 @@ public class FieldMendesPavlisModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Compute Delay with state derivatives
final DerivativeStructure delay = model.pathDelay(dsElevation, zero, model.getParameters(field), dsDate);
......
......@@ -36,7 +36,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -170,9 +169,7 @@ public class FieldNiellMappingFunctionModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Compute mapping factors with state derivatives
final DerivativeStructure[] factors = model.mappingFactors(dsElevation, zero, model.getParameters(field), dsDate);
......
......@@ -35,7 +35,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -202,9 +201,7 @@ public class FieldViennaOneModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Compute delay state derivatives
final DerivativeStructure delay = model.pathDelay(dsElevation, zero, model.getParameters(field), dsDate);
......
......@@ -35,7 +35,6 @@ import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
......@@ -307,9 +306,7 @@ public class FieldViennaThreeModelTest {
// Initial satellite elevation
final FieldVector3D<DerivativeStructure> position = dsState.getPVCoordinates().getPosition();
final FieldTransform<DerivativeStructure> t = dsState.getFrame().getTransformTo(baseFrame, dsDate);
final FieldVector3D<DerivativeStructure> extPointTopo = t.transformPosition(position);
final DerivativeStructure dsElevation = extPointTopo.getDelta();
final DerivativeStructure dsElevation = baseFrame.getElevation(position, frame, dsDate);
// Compute delay state derivatives
final DerivativeStructure delay = model.pathDelay(dsElevation, zero, model.getParameters(field), dsDate);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment