From 1f792509cc32cc084528510a6dfeab65dbec1274 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Tue, 25 Mar 2014 18:09:53 +0100
Subject: [PATCH] Added a los conversion between Cartesian and geodetic
coordinates.
The conversion is of course accurate only in the neighborhood of the
reference point, as a straight line in Cartesian is not a straight line
in geodetic coordinates. What is converted is the initial direction so
the two curves are tangent at the reference point.
Near Earth surface, the two curves remain within one millimeter of each
other after about 100 m travel.
---
.../orekit/rugged/core/ExtendedEllipsoid.java | 39 +++++++++++++++++++
.../rugged/core/ExtendedEllipsoidTest.java | 19 +++++++++
2 files changed, 58 insertions(+)
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 b8128c69..118446a4 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 ffbf3083..f1d632a0 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 {
--
GitLab