List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Line toSpace
public Vector3D toSpace(final Vector<Euclidean1D> point)
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public GeodeticPoint getIntersectionPoint(final Line line, final Vector3D close, final Frame frame, final AbsoluteDate date) throws OrekitException { // transform line and close to body frame final Transform frameToBodyFrame = frame.getTransformTo(bodyFrame, date); final Line lineInBodyFrame = frameToBodyFrame.transformLine(line); final Vector3D closeInBodyFrame = frameToBodyFrame.transformPosition(close); final double closeAbscissa = lineInBodyFrame.toSubSpace(closeInBodyFrame).getX(); // compute some miscellaneous variables outside of the loop final Vector3D point = lineInBodyFrame.getOrigin(); final double x = point.getX(); final double y = point.getY(); final double z = point.getZ(); final double z2 = z * z; final double r2 = x * x + y * y; final Vector3D direction = lineInBodyFrame.getDirection(); final double dx = direction.getX(); final double dy = direction.getY(); final double dz = direction.getZ(); final double cz2 = dx * dx + dy * dy; // abscissa of the intersection as a root of a 2nd degree polynomial : // a k^2 - 2 b k + c = 0 final double a = 1.0 - e2 * cz2; final double b = -(g2 * (x * dx + y * dy) + z * dz); final double c = g2 * (r2 - ae2) + z2; final double b2 = b * b; final double ac = a * c; if (b2 < ac) { return null; }//www. java2s. com final double s = FastMath.sqrt(b2 - ac); final double k1 = (b < 0) ? (b - s) / a : c / (b + s); final double k2 = c / (a * k1); // select the right point final double k = (FastMath.abs(k1 - closeAbscissa) < FastMath.abs(k2 - closeAbscissa)) ? k1 : k2; final Vector3D intersection = lineInBodyFrame.toSpace(new Vector1D(k)); final double ix = intersection.getX(); final double iy = intersection.getY(); final double iz = intersection.getZ(); final double lambda = FastMath.atan2(iy, ix); final double phi = FastMath.atan2(iz, g2 * FastMath.sqrt(ix * ix + iy * iy)); return new GeodeticPoint(phi, lambda, 0.0); }
From source file:org.orekit.bodies.OneAxisEllipsoidTest.java
@Test public void testIntersectionFromPoints() throws OrekitException { AbsoluteDate date = new AbsoluteDate(new DateComponents(2008, 03, 21), TimeComponents.H12, TimeScalesFactory.getUTC()); Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); OneAxisEllipsoid earth = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, frame); // Satellite on polar position // *************************** final double mu = 3.9860047e14; CircularOrbit circ = new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(90.), FastMath.toRadians(60.), FastMath.toRadians(90.), PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu);//from w w w.j a v a 2 s . c o m // Transform satellite position to position/velocity parameters in EME2000 and ITRF200B PVCoordinates pvSatEME2000 = circ.getPVCoordinates(); PVCoordinates pvSatItrf = frame.getTransformTo(FramesFactory.getEME2000(), date) .transformPVCoordinates(pvSatEME2000); Vector3D pSatItrf = pvSatItrf.getPosition(); // Test first visible surface points GeodeticPoint geoPoint = new GeodeticPoint(FastMath.toRadians(70.), FastMath.toRadians(60.), 0.); Vector3D pointItrf = earth.transform(geoPoint); Line line = new Line(pSatItrf, pointItrf, 1.0e-10); GeodeticPoint geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // Test second visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(65.), FastMath.toRadians(-120.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // Test non visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(30.), FastMath.toRadians(60.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); // For polar satellite position, intersection point is at the same longitude but different latitude Assert.assertEquals(1.04437199, geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(1.36198012, geoInter.getLatitude(), Utils.epsilonAngle); // Satellite on equatorial position // ******************************** circ = new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(1.e-4), FastMath.toRadians(0.), FastMath.toRadians(0.), PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); // Transform satellite position to position/velocity parameters in EME2000 and ITRF200B pvSatEME2000 = circ.getPVCoordinates(); pvSatItrf = frame.getTransformTo(FramesFactory.getEME2000(), date).transformPVCoordinates(pvSatEME2000); pSatItrf = pvSatItrf.getPosition(); // Test first visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(5.), FastMath.toRadians(0.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); Assert.assertTrue(line.toSubSpace(pSatItrf).getX() < 0); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // With the point opposite to satellite point along the line GeodeticPoint geoInter2 = earth.getIntersectionPoint(line, line.toSpace(new Vector1D(-line.toSubSpace(pSatItrf).getX())), frame, date); Assert.assertTrue( FastMath.abs(geoInter.getLongitude() - geoInter2.getLongitude()) > FastMath.toRadians(0.1)); Assert.assertTrue(FastMath.abs(geoInter.getLatitude() - geoInter2.getLatitude()) > FastMath.toRadians(0.1)); // Test second visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(-5.), FastMath.toRadians(0.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // Test non visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(0.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(-0.00768481, geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(0.32180410, geoInter.getLatitude(), Utils.epsilonAngle); // Satellite on any position // ************************* circ = new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(50.), FastMath.toRadians(0.), FastMath.toRadians(90.), PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); // Transform satellite position to position/velocity parameters in EME2000 and ITRF200B pvSatEME2000 = circ.getPVCoordinates(); pvSatItrf = frame.getTransformTo(FramesFactory.getEME2000(), date).transformPVCoordinates(pvSatEME2000); pSatItrf = pvSatItrf.getPosition(); // Test first visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(90.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // Test second visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(60.), FastMath.toRadians(90.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle); // Test non visible surface points geoPoint = new GeodeticPoint(FastMath.toRadians(0.), FastMath.toRadians(90.), 0.); pointItrf = earth.transform(geoPoint); line = new Line(pSatItrf, pointItrf, 1.0e-10); geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date); Assert.assertEquals(FastMath.toRadians(89.5364061088196), geoInter.getLongitude(), Utils.epsilonAngle); Assert.assertEquals(FastMath.toRadians(35.555543683351125), geoInter.getLatitude(), Utils.epsilonAngle); }