List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D dotProduct
public static double dotProduct(Vector3D v1, Vector3D v2)
From source file:org.orekit.frames.TransformTest.java
@Test public void testShiftDerivatives() { RandomGenerator random = new Well19937a(0x5acda4f605aadce7l); for (int i = 0; i < 10; ++i) { Transform t = randomTransform(random); for (double dt = -10.0; dt < 10.0; dt += 0.125) { Transform t0 = t.shiftedBy(dt); double v = t0.getVelocity().getNorm(); double a = t0.getAcceleration().getNorm(); double omega = t0.getRotationRate().getNorm(); double omegaDot = t0.getRotationAcceleration().getNorm(); // numerical derivatives double h = 0.01 / omega; Transform tm4h = t.shiftedBy(dt - 4 * h); Transform tm3h = t.shiftedBy(dt - 3 * h); Transform tm2h = t.shiftedBy(dt - 2 * h); Transform tm1h = t.shiftedBy(dt - 1 * h); Transform tp1h = t.shiftedBy(dt + 1 * h); Transform tp2h = t.shiftedBy(dt + 2 * h); Transform tp3h = t.shiftedBy(dt + 3 * h); Transform tp4h = t.shiftedBy(dt + 4 * h); double numXDot = derivative(h, tm4h.getTranslation().getX(), tm3h.getTranslation().getX(), tm2h.getTranslation().getX(), tm1h.getTranslation().getX(), tp1h.getTranslation().getX(), tp2h.getTranslation().getX(), tp3h.getTranslation().getX(), tp4h.getTranslation().getX()); double numYDot = derivative(h, tm4h.getTranslation().getY(), tm3h.getTranslation().getY(), tm2h.getTranslation().getY(), tm1h.getTranslation().getY(), tp1h.getTranslation().getY(), tp2h.getTranslation().getY(), tp3h.getTranslation().getY(), tp4h.getTranslation().getY()); double numZDot = derivative(h, tm4h.getTranslation().getZ(), tm3h.getTranslation().getZ(), tm2h.getTranslation().getZ(), tm1h.getTranslation().getZ(), tp1h.getTranslation().getZ(), tp2h.getTranslation().getZ(), tp3h.getTranslation().getZ(), tp4h.getTranslation().getZ()); double numXDot2 = derivative(h, tm4h.getVelocity().getX(), tm3h.getVelocity().getX(), tm2h.getVelocity().getX(), tm1h.getVelocity().getX(), tp1h.getVelocity().getX(), tp2h.getVelocity().getX(), tp3h.getVelocity().getX(), tp4h.getVelocity().getX()); double numYDot2 = derivative(h, tm4h.getVelocity().getY(), tm3h.getVelocity().getY(), tm2h.getVelocity().getY(), tm1h.getVelocity().getY(), tp1h.getVelocity().getY(), tp2h.getVelocity().getY(), tp3h.getVelocity().getY(), tp4h.getVelocity().getY()); double numZDot2 = derivative(h, tm4h.getVelocity().getZ(), tm3h.getVelocity().getZ(), tm2h.getVelocity().getZ(), tm1h.getVelocity().getZ(), tp1h.getVelocity().getZ(), tp2h.getVelocity().getZ(), tp3h.getVelocity().getZ(), tp4h.getVelocity().getZ()); double numQ0Dot = derivative(h, tm4h.getRotation().getQ0(), tm3h.getRotation().getQ0(), tm2h.getRotation().getQ0(), tm1h.getRotation().getQ0(), tp1h.getRotation().getQ0(), tp2h.getRotation().getQ0(), tp3h.getRotation().getQ0(), tp4h.getRotation().getQ0()); double numQ1Dot = derivative(h, tm4h.getRotation().getQ1(), tm3h.getRotation().getQ1(), tm2h.getRotation().getQ1(), tm1h.getRotation().getQ1(), tp1h.getRotation().getQ1(), tp2h.getRotation().getQ1(), tp3h.getRotation().getQ1(), tp4h.getRotation().getQ1()); double numQ2Dot = derivative(h, tm4h.getRotation().getQ2(), tm3h.getRotation().getQ2(), tm2h.getRotation().getQ2(), tm1h.getRotation().getQ2(), tp1h.getRotation().getQ2(), tp2h.getRotation().getQ2(), tp3h.getRotation().getQ2(), tp4h.getRotation().getQ2()); double numQ3Dot = derivative(h, tm4h.getRotation().getQ3(), tm3h.getRotation().getQ3(), tm2h.getRotation().getQ3(), tm1h.getRotation().getQ3(), tp1h.getRotation().getQ3(), tp2h.getRotation().getQ3(), tp3h.getRotation().getQ3(), tp4h.getRotation().getQ3()); double numOxDot = derivative(h, tm4h.getRotationRate().getX(), tm3h.getRotationRate().getX(), tm2h.getRotationRate().getX(), tm1h.getRotationRate().getX(), tp1h.getRotationRate().getX(), tp2h.getRotationRate().getX(), tp3h.getRotationRate().getX(), tp4h.getRotationRate().getX()); double numOyDot = derivative(h, tm4h.getRotationRate().getY(), tm3h.getRotationRate().getY(), tm2h.getRotationRate().getY(), tm1h.getRotationRate().getY(), tp1h.getRotationRate().getY(), tp2h.getRotationRate().getY(), tp3h.getRotationRate().getY(), tp4h.getRotationRate().getY()); double numOzDot = derivative(h, tm4h.getRotationRate().getZ(), tm3h.getRotationRate().getZ(), tm2h.getRotationRate().getZ(), tm1h.getRotationRate().getZ(), tp1h.getRotationRate().getZ(), tp2h.getRotationRate().getZ(), tp3h.getRotationRate().getZ(), tp4h.getRotationRate().getZ()); // theoretical derivatives double theXDot = t0.getVelocity().getX(); double theYDot = t0.getVelocity().getY(); double theZDot = t0.getVelocity().getZ(); double theXDot2 = t0.getAcceleration().getX(); double theYDot2 = t0.getAcceleration().getY(); double theZDot2 = t0.getAcceleration().getZ(); Rotation r0 = t0.getRotation(); Vector3D w = t0.getRotationRate(); Vector3D q = new Vector3D(r0.getQ1(), r0.getQ2(), r0.getQ3()); Vector3D qw = Vector3D.crossProduct(q, w); double theQ0Dot = -0.5 * Vector3D.dotProduct(q, w); double theQ1Dot = 0.5 * (r0.getQ0() * w.getX() + qw.getX()); double theQ2Dot = 0.5 * (r0.getQ0() * w.getY() + qw.getY()); double theQ3Dot = 0.5 * (r0.getQ0() * w.getZ() + qw.getZ()); double theOxDot2 = t0.getRotationAcceleration().getX(); double theOyDot2 = t0.getRotationAcceleration().getY(); double theOzDot2 = t0.getRotationAcceleration().getZ(); // check consistency Assert.assertEquals(theXDot, numXDot, 1.0e-13 * v); Assert.assertEquals(theYDot, numYDot, 1.0e-13 * v); Assert.assertEquals(theZDot, numZDot, 1.0e-13 * v); Assert.assertEquals(theXDot2, numXDot2, 1.0e-13 * a); Assert.assertEquals(theYDot2, numYDot2, 1.0e-13 * a); Assert.assertEquals(theZDot2, numZDot2, 1.0e-13 * a); Assert.assertEquals(theQ0Dot, numQ0Dot, 1.0e-13 * omega); Assert.assertEquals(theQ1Dot, numQ1Dot, 1.0e-13 * omega); Assert.assertEquals(theQ2Dot, numQ2Dot, 1.0e-13 * omega); Assert.assertEquals(theQ3Dot, numQ3Dot, 1.0e-13 * omega); Assert.assertEquals(theOxDot2, numOxDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOyDot2, numOyDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOzDot2, numOzDot, 1.0e-12 * omegaDot); }//from w ww . j ava2s. co m } }
From source file:org.orekit.models.earth.tessellation.AlongTrackAimingTest.java
@Test public void testAscending() throws OrekitException { final AlongTrackAiming tileAiming = new AlongTrackAiming(ellipsoid, orbit, true); for (double latitude = FastMath.toRadians(-50.21); latitude < FastMath .toRadians(50.21); latitude += 0.001) { final GeodeticPoint gp = new GeodeticPoint(latitude, 0.0, 0.0); final Vector3D aiming = tileAiming.alongTileDirection(ellipsoid.transform(gp), gp); Assert.assertEquals(1.0, aiming.getNorm(), 1.0e-12); final double elevation = 0.5 * FastMath.PI - Vector3D.angle(aiming, gp.getZenith()); final double azimuth = FastMath.atan2(Vector3D.dotProduct(aiming, gp.getEast()), Vector3D.dotProduct(aiming, gp.getNorth())); Assert.assertEquals(0.0, FastMath.toDegrees(elevation), 1.0e-6); if (FastMath.abs(FastMath.toDegrees(latitude)) > 49.6) { Assert.assertTrue(FastMath.toDegrees(azimuth) > 80.0); }/*from ww w . j av a 2 s.c o m*/ if (FastMath.abs(FastMath.toDegrees(latitude)) < 5.0) { Assert.assertTrue(FastMath.toDegrees(azimuth) < 37.0); } Assert.assertTrue(FastMath.toDegrees(azimuth) > 36.7); } }
From source file:org.orekit.models.earth.tessellation.AlongTrackAimingTest.java
@Test public void testDescending() throws OrekitException { final AlongTrackAiming tileAiming = new AlongTrackAiming(ellipsoid, orbit, false); for (double latitude = FastMath.toRadians(-50.21); latitude < FastMath .toRadians(50.21); latitude += 0.001) { final GeodeticPoint gp = new GeodeticPoint(latitude, 0.0, 0.0); final Vector3D aiming = tileAiming.alongTileDirection(ellipsoid.transform(gp), gp); Assert.assertEquals(1.0, aiming.getNorm(), 1.0e-12); final double elevation = 0.5 * FastMath.PI - Vector3D.angle(aiming, gp.getZenith()); final double azimuth = MathUtils.normalizeAngle(FastMath .atan2(Vector3D.dotProduct(aiming, gp.getEast()), Vector3D.dotProduct(aiming, gp.getNorth())), FastMath.PI);/* w w w . ja va2s . co m*/ Assert.assertEquals(0.0, FastMath.toDegrees(elevation), 1.0e-6); if (FastMath.abs(FastMath.toDegrees(latitude)) > 49.7) { Assert.assertTrue(FastMath.toDegrees(azimuth) < 99.0); } if (FastMath.abs(FastMath.toDegrees(latitude)) < 5.0) { Assert.assertTrue(FastMath.toDegrees(azimuth) > 143); } Assert.assertTrue(FastMath.toDegrees(azimuth) < 143.3); } }
From source file:org.orekit.models.earth.tessellation.EllipsoidTessellator.java
/** Estimate an approximate motion in the along direction. * @param start node at start of motion//from w w w .j a va 2s. com * @param end desired point at end of motion * @return approximate motion in the along direction */ private double estimateAlongMotion(final Mesh.Node start, final Vector3D end) { return Vector3D.dotProduct(start.getAlong(), end.subtract(start.getV())); }
From source file:org.orekit.models.earth.tessellation.EllipsoidTessellator.java
/** Estimate an approximate motion in the across direction. * @param start node at start of motion//from w ww . j a v a2s . c o m * @param end desired point at end of motion * @return approximate motion in the across direction */ private double estimateAcrossMotion(final Mesh.Node start, final Vector3D end) { return Vector3D.dotProduct(start.getAcross(), end.subtract(start.getV())); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** {@inheritDoc} */ public double getE() { final Vector3D pvP = getPVCoordinates().getPosition(); final Vector3D pvV = getPVCoordinates().getVelocity(); final double rV2OnMu = pvP.getNorm() * pvV.getNormSq() / getMu(); final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * getA()); final double eCE = rV2OnMu - 1; return FastMath.sqrt(eCE * eCE + eSE * eSE); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Compute shifted position and velocity in elliptic case. * @param dt time shift/* w w w.j av a 2s .c om*/ * @return shifted position and velocity */ private PVCoordinates shiftPVElliptic(final double dt) { // preliminary computation final Vector3D pvP = getPVCoordinates().getPosition(); final Vector3D pvV = getPVCoordinates().getVelocity(); final double r = pvP.getNorm(); final double rV2OnMu = r * pvV.getNormSq() / getMu(); final double a = getA(); final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; // we can use any arbitrary reference 2D frame in the orbital plane // in order to simplify some equations below, we use the current position as the u axis final Vector3D u = pvP.normalize(); final Vector3D v = Vector3D.crossProduct(getPVCoordinates().getMomentum(), u).normalize(); // the following equations rely on the specific choice of u explained above, // some coefficients that vanish to 0 in this case have already been removed here final double ex = (eCE - e2) * a / r; final double ey = -FastMath.sqrt(1 - e2) * eSE * a / r; final double beta = 1 / (1 + FastMath.sqrt(1 - e2)); final double thetaE0 = FastMath.atan2(ey + eSE * beta * ex, r / a + ex - eSE * beta * ey); final double thetaM0 = thetaE0 - ex * FastMath.sin(thetaE0) + ey * FastMath.cos(thetaE0); // compute in-plane shifted eccentric argument final double thetaM1 = thetaM0 + getKeplerianMeanMotion() * dt; final double thetaE1 = meanToEccentric(thetaM1, ex, ey); final double cTE = FastMath.cos(thetaE1); final double sTE = FastMath.sin(thetaE1); // compute shifted in-plane cartesian coordinates final double exey = ex * ey; final double exCeyS = ex * cTE + ey * sTE; final double x = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex); final double y = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey); final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS); final double xDot = factor * (-sTE + beta * ey * exCeyS); final double yDot = factor * (cTE - beta * ex * exCeyS); final Vector3D shiftedP = new Vector3D(x, u, y, v); final double r2 = x * x + y * y; final Vector3D shiftedV = new Vector3D(xDot, u, yDot, v); final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP); return new PVCoordinates(shiftedP, shiftedV, shiftedA); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Compute shifted position and velocity in hyperbolic case. * @param dt time shift//w w w . ja va 2 s . co m * @return shifted position and velocity */ private PVCoordinates shiftPVHyperbolic(final double dt) { final PVCoordinates pv = getPVCoordinates(); final Vector3D pvP = pv.getPosition(); final Vector3D pvV = pv.getVelocity(); final Vector3D pvM = pv.getMomentum(); final double r = pvP.getNorm(); final double rV2OnMu = r * pvV.getNormSq() / getMu(); final double a = getA(); final double muA = getMu() * a; final double e = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA); final double sqrt = FastMath.sqrt((e + 1) / (e - 1)); // compute mean anomaly final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA); final double eCH = rV2OnMu - 1; final double H0 = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2; final double M0 = e * FastMath.sinh(H0) - H0; // find canonical 2D frame with p pointing to perigee final double v0 = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2)); final Vector3D p = new Rotation(pvM, -v0).applyTo(pvP).normalize(); final Vector3D q = Vector3D.crossProduct(pvM, p).normalize(); // compute shifted eccentric anomaly final double M1 = M0 + getKeplerianMeanMotion() * dt; final double H1 = meanToHyperbolicEccentric(M1, e); // compute shifted in-plane cartesian coordinates final double cH = FastMath.cosh(H1); final double sH = FastMath.sinh(H1); final double sE2m1 = FastMath.sqrt((e - 1) * (e + 1)); // coordinates of position and velocity in the orbital plane final double x = a * (cH - e); final double y = -a * sE2m1 * sH; final double factor = FastMath.sqrt(getMu() / -a) / (e * cH - 1); final double xDot = -factor * sH; final double yDot = factor * sE2m1 * cH; final Vector3D shiftedP = new Vector3D(x, p, y, q); final double r2 = x * x + y * y; final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q); final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP); return new PVCoordinates(shiftedP, shiftedV, shiftedA); }
From source file:org.orekit.orbits.CartesianParametersTest.java
@Test public void testGeometry() { Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0); Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0); PVCoordinates pvCoordinates = new PVCoordinates(position, velocity); Vector3D momentum = pvCoordinates.getMomentum().normalize(); EquinoctialOrbit p = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu); double apogeeRadius = p.getA() * (1 + p.getE()); double perigeeRadius = p.getA() * (1 - p.getE()); for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) { p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(), p.getEquinoctialEy(), p.getHx(), p.getHy(), lv, PositionAngle.TRUE, p.getFrame(), date, mu); position = p.getPVCoordinates().getPosition(); // test if the norm of the position is in the range [perigee radius, apogee radius] // Warning: these tests are without absolute value by choice Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest)); Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest)); // Assert.assertTrue(position.getNorm() <= apogeeRadius); // Assert.assertTrue(position.getNorm() >= perigeeRadius); position = position.normalize(); velocity = p.getPVCoordinates().getVelocity().normalize(); // at this stage of computation, all the vectors (position, velocity and momemtum) are normalized here // test of orthogonality between position and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest); // test of orthogonality between velocity and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest); }/* w w w . j a va2s . c o m*/ }
From source file:org.orekit.orbits.CircularOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the {@link PVCoordinates} in inertial frame * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if frame is not a {@link * Frame#isPseudoInertial pseudo-inertial frame} *//*from w w w.j ava2s . c o m*/ public CircularOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute semi-major axis final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; if (rV2OnMu > 2) { throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName()); } a = r / (2 - rV2OnMu); // compute inclination final Vector3D momentum = pvCoordinates.getMomentum(); i = Vector3D.angle(momentum, Vector3D.PLUS_K); // compute right ascension of ascending node final Vector3D node = Vector3D.crossProduct(Vector3D.PLUS_K, momentum); raan = FastMath.atan2(node.getY(), node.getX()); // 2D-coordinates in the canonical frame final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double xP = pvP.getX(); final double yP = pvP.getY(); final double zP = pvP.getZ(); final double x2 = (xP * cosRaan + yP * sinRaan) / a; final double y2 = ((yP * cosRaan - xP * sinRaan) * cosI + zP * sinI) / a; // compute eccentricity vector final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; final double f = eCE - e2; final double g = FastMath.sqrt(1 - e2) * eSE; final double aOnR = a / r; final double a2OnR2 = aOnR * aOnR; ex = a2OnR2 * (f * x2 + g * y2); ey = a2OnR2 * (f * y2 - g * x2); // compute latitude argument final double beta = 1 / (1 + FastMath.sqrt(1 - ex * ex - ey * ey)); alphaV = eccentricToTrue(FastMath.atan2(y2 + ey + eSE * beta * ex, x2 + ex - eSE * beta * ey)); serializePV = true; }