diff --git a/rugged-core/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java b/rugged-core/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java index b8128c6924de0bc36da82b5dd73d7a887ebe1d52..118446a406147f53cfb33187c403d1a81e27818a 100644 --- a/rugged-core/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java +++ b/rugged-core/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java @@ -36,6 +36,12 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid { /** Convergence threshold for {@link #pointAtAltitude(Vector3D, Vector3D, double)}. */ private static final double ALTITUDE_CONVERGENCE = 1.0e-3; + /** Equatorial radius power 2. */ + private final double a2; + + /** Polar radius power 2. */ + private final double b2; + /** Simple constructor. * @param ae equatorial radius * @param f the flattening (f = (a-b)/a) @@ -44,6 +50,9 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid { */ public ExtendedEllipsoid(final double ae, final double f, final Frame bodyFrame) { super(ae, f, bodyFrame); + a2 = ae * ae; + final double b = ae * (1.0 - f); + b2 = b * b; } /** Get point at some latitude along a pixel line of sight. @@ -190,4 +199,34 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid { } } + /** Convert a line-of-sight from Cartesian to topocentric. + * @param point geodetic point on the line-of-sight + * @param los line-of-sight, not necessarily normalized (in body frame and Cartesian coordinates) + * @return line-of-sight in topocentric frame (East, North, Zenith) of the point, + * scaled to match radians in the horizontal plane and meters along the vertical axis + */ + public Vector3D convertLos(final GeodeticPoint point, final Vector3D los) + throws OrekitException { + + // Cartesian coordinates of the topocentric frame origin + final Vector3D p3D = transform(point); + + // local radius of curvature in the East-West direction (parallel) + final double r = FastMath.hypot(p3D.getX(), p3D.getY()); + + // local radius of curvature in the North-South direction (meridian) + final double b2r = b2 * r; + final double b4r2 = b2r * b2r; + final double a2z = a2 * p3D.getZ(); + final double a4z2 = a2z * a2z; + final double q = a4z2 + b4r2; + final double rho = q * FastMath.sqrt(q) / (b2 * a4z2 + a2 * b4r2); + + final double norm = los.getNorm(); + return new Vector3D(Vector3D.dotProduct(los, point.getEast()) / (norm * r), + Vector3D.dotProduct(los, point.getNorth()) / (norm * rho), + Vector3D.dotProduct(los, point.getZenith()) / norm); + + } + } diff --git a/rugged-core/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java b/rugged-core/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java index ffbf3083ab3c122b3961199884af49a5319edce8..f1d632a0f21b06db83fb876c0ffaf0dc6a6c618c 100644 --- a/rugged-core/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java +++ b/rugged-core/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java @@ -19,6 +19,7 @@ package org.orekit.rugged.core; import java.io.File; import java.net.URISyntaxException; +import org.apache.commons.math3.geometry.euclidean.threed.Line; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.util.FastMath; import org.junit.After; @@ -178,6 +179,24 @@ public class ExtendedEllipsoidTest { } + @Test + public void testConvertLOS() throws RuggedException, OrekitException { + + GeodeticPoint gp = new GeodeticPoint(-0.2, 1.8, 2400.0); + Vector3D p = ellipsoid.transform(gp); + Vector3D los = new Vector3D(-1, -2, -3); + Vector3D converted = ellipsoid.convertLos(gp, los); + Line line = new Line(p, new Vector3D(1.0, p, 1000, los), 1.0e-10); + + for (double delta = 0; delta < 100.0; delta += 0.1) { + GeodeticPoint shifted = new GeodeticPoint(gp.getLatitude() + delta * converted.getY(), + gp.getLongitude() + delta * converted.getX(), + gp.getAltitude() + delta * converted.getZ()); + Assert.assertEquals(0.0, line.distance(ellipsoid.transform(shifted)), 1.0e-3); + } + + } + @Before public void setUp() { try {