List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation Rotation
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException
From source file:org.orekit.frames.TransformTest.java
private Transform evolvingTransform(final AbsoluteDate t0, final double dt) { // the following transform corresponds to a frame moving along the circle r = 1 // with its x axis always pointing to the reference frame center final double omega = 0.2; final AbsoluteDate date = t0.shiftedBy(dt); final double cos = FastMath.cos(omega * dt); final double sin = FastMath.sin(omega * dt); return new Transform(date, new Transform(date, new Vector3D(-cos, -sin, 0), new Vector3D(omega * sin, -omega * cos, 0), new Vector3D(omega * omega * cos, omega * omega * sin, 0)), new Transform(date, new Rotation(Vector3D.PLUS_K, FastMath.PI - omega * dt), new Vector3D(omega, Vector3D.PLUS_K))); }
From source file:org.orekit.frames.VEISProvider.java
/** Get the transform from GTOD at specified date. * @param date new value of the date// ww w. jav a 2 s . c om * @return transform at the specified date * @exception OrekitException if data embedded in the library cannot be read */ public Transform getTransform(final AbsoluteDate date) throws OrekitException { // offset from FIFTIES epoch (UT1 scale) final double dtai = date.durationFrom(VST_REFERENCE); final double dutc = TimeScalesFactory.getUTC().offsetFromTAI(date); final double dut1 = 0.0; // fixed at 0 since Veis parent is GTOD frame WITHOUT EOP corrections final double tut1 = dtai + dutc + dut1; final double ttd = tut1 / Constants.JULIAN_DAY; final double rdtt = ttd - (int) ttd; // compute Veis sidereal time, in radians final double vst = (VST0 + VST1 * ttd + MathUtils.TWO_PI * rdtt) % MathUtils.TWO_PI; // compute angular rotation of Earth, in rad/s final Vector3D rotationRate = new Vector3D(-VSTD, Vector3D.PLUS_K); // set up the transform from parent GTOD return new Transform(date, new Rotation(Vector3D.PLUS_K, vst), rotationRate); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Compute shifted position and velocity in hyperbolic case. * @param dt time shift// w ww . j av a 2 s. com * @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.propagation.analytical.EcksteinHechlerPropagatorTest.java
@Test public void testInitializationCorrectness() throws OrekitException, IOException { // Definition of initial conditions AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(154.); Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true); Frame eme2000 = FramesFactory.getEME2000(); Vector3D pole = itrf.getTransformTo(eme2000, date).transformVector(Vector3D.PLUS_K); Frame poleAligned = new Frame(FramesFactory.getEME2000(), new Transform(date, new Rotation(pole, Vector3D.PLUS_K)), "pole aligned", true); CircularOrbit initial = new CircularOrbit(7208669.8179538045, 1.3740461966386876E-4, -3.2364250248363356E-5, FastMath.toRadians(97.40236024565775), FastMath.toRadians(166.15873160992115), FastMath.toRadians(90.1282370098961), PositionAngle.MEAN, poleAligned, date, provider.getMu()); // find the default Eckstein-Hechler propagator initialized from the initial orbit EcksteinHechlerPropagator defaultEH = new EcksteinHechlerPropagator(initial, provider); // the osculating parameters recomputed by the default Eckstein-Hechler propagator are quite different // from initial orbit CircularOrbit defaultOrbit = (CircularOrbit) OrbitType.CIRCULAR .convertType(defaultEH.propagateOrbit(initial.getDate())); Assert.assertEquals(267.4, defaultOrbit.getA() - initial.getA(), 0.1); // the position on the other hand match perfectly Assert.assertEquals(0.0, Vector3D.distance(defaultOrbit.getPVCoordinates().getPosition(), initial.getPVCoordinates().getPosition()), 1.0e-8); // set up a reference numerical propagator starting for the specified start orbit // using the same force models (i.e. the first few zonal terms) double[][] tol = NumericalPropagator.tolerances(0.1, initial, OrbitType.CIRCULAR); AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]); integrator.setInitialStepSize(60);/*from ww w .ja v a2s .c o m*/ NumericalPropagator num = new NumericalPropagator(integrator); num.addForceModel( new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(provider))); num.setInitialState(new SpacecraftState(initial)); num.setOrbitType(OrbitType.CIRCULAR); // find the best Eckstein-Hechler propagator that match the orbit evolution PropagatorConverter converter = new FiniteDifferencePropagatorConverter( new EcksteinHechlerPropagatorBuilder(poleAligned, provider, OrbitType.CIRCULAR, PositionAngle.TRUE), 1.0e-6, 100); EcksteinHechlerPropagator fittedEH = (EcksteinHechlerPropagator) converter.convert(num, 3 * initial.getKeplerianPeriod(), 300); // the default Eckstein-Hechler propagator did however quite a good job, as it found // an orbit close to the best fitting CircularOrbit fittedOrbit = (CircularOrbit) OrbitType.CIRCULAR .convertType(fittedEH.propagateOrbit(initial.getDate())); Assert.assertEquals(0.623, defaultOrbit.getA() - fittedOrbit.getA(), 0.1); // the position on the other hand are slightly different // because the fitted orbit minimizes the residuals over a complete time span, // not on a single point Assert.assertEquals(58.0, Vector3D.distance(defaultOrbit.getPVCoordinates().getPosition(), fittedOrbit.getPVCoordinates().getPosition()), 0.1); }
From source file:org.orekit.propagation.numerical.PartialDerivativesTest.java
@Test public void testJacobianIssue18() throws OrekitException { // Body mu// ww w . j a va 2 s. c o m final double mu = 3.9860047e14; final double isp = 318; final double mass = 2500; final double a = 24396159; final double e = 0.72831215; final double i = FastMath.toRadians(7); final double omega = FastMath.toRadians(180); final double OMEGA = FastMath.toRadians(261); final double lv = 0; final double duration = 3653.99; final double f = 420; final double delta = FastMath.toRadians(-7.4978); final double alpha = FastMath.toRadians(351); final AttitudeProvider law = new InertialProvider( new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I)); final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01), new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC()); final Orbit orbit = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, FramesFactory.getEME2000(), initDate, mu); final SpacecraftState initialState = new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass); final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02), new TimeComponents(04, 15, 34.080), TimeScalesFactory.getUTC()); final ConstantThrustManeuver maneuver = new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I); double[] absTolerance = { 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001 }; double[] relTolerance = { 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7 }; AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance); integrator.setInitialStepSize(60); final NumericalPropagator propagator = new NumericalPropagator(integrator); propagator.setAttitudeProvider(law); propagator.addForceModel(maneuver); propagator.setOrbitType(OrbitType.CARTESIAN); PartialDerivativesEquations PDE = new PartialDerivativesEquations("derivatives", propagator); PDE.selectParamAndStep("thrust", Double.NaN); Assert.assertEquals(3, PDE.getAvailableParameters().size()); Assert.assertEquals("central attraction coefficient", PDE.getAvailableParameters().get(0)); Assert.assertEquals("thrust", PDE.getAvailableParameters().get(1)); Assert.assertEquals("flow rate", PDE.getAvailableParameters().get(2)); propagator.setInitialState(PDE.setInitialJacobians(initialState, 7, 1)); final AbsoluteDate finalDate = fireDate.shiftedBy(3800); final SpacecraftState finalorb = propagator.propagate(finalDate); Assert.assertEquals(0, finalDate.durationFrom(finalorb.getDate()), 1.0e-11); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Get a time-shifted state. * <p>/* w w w . j a va 2 s. co m*/ * The state can be slightly shifted to close dates. This shift is based on * an approximate solution of the fixed acceleration motion. It is <em>not</em> * intended as a replacement for proper attitude propagation but should be * sufficient for either small time shifts or coarse accuracy. * </p> * @param dt time shift in seconds * @return a new state, shifted with respect to the instance (which is immutable) */ public AngularCoordinates shiftedBy(final double dt) { // the shiftedBy method is based on a local approximation. // It considers separately the contribution of the constant // rotation, the linear contribution or the rate and the // quadratic contribution of the acceleration. The rate // and acceleration contributions are small rotations as long // as the time shift is small, which is the crux of the algorithm. // Small rotations are almost commutative, so we append these small // contributions one after the other, as if they really occurred // successively, despite this is not what really happens. // compute the linear contribution first, ignoring acceleration // BEWARE: there is really a minus sign here, because if // the target frame rotates in one direction, the vectors in the origin // frame seem to rotate in the opposite direction final double rate = rotationRate.getNorm(); final Rotation rateContribution = (rate == 0.0) ? Rotation.IDENTITY : new Rotation(rotationRate, -rate * dt); // append rotation and rate contribution final AngularCoordinates linearPart = new AngularCoordinates(rateContribution.applyTo(rotation), rotationRate); final double acc = rotationAcceleration.getNorm(); if (acc == 0.0) { // no acceleration, the linear part is sufficient return linearPart; } // compute the quadratic contribution, ignoring initial rotation and rotation rate // BEWARE: there is really a minus sign here, because if // the target frame rotates in one direction, the vectors in the origin // frame seem to rotate in the opposite direction final AngularCoordinates quadraticContribution = new AngularCoordinates( new Rotation(rotationAcceleration, -0.5 * acc * dt * dt), new Vector3D(dt, rotationAcceleration), rotationAcceleration); // the quadratic contribution is a small rotation: // its initial angle and angular rate are both zero. // small rotations are almost commutative, so we append the small // quadratic part after the linear part as a simple offset return quadraticContribution.addOffset(linearPart); }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testRodriguesSpecialCases() { // identity/*from ww w. java2s . c o m*/ double[][] identity = new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO) .getModifiedRodrigues(1.0); for (double[] row : identity) { for (double element : row) { Assert.assertEquals(0.0, element, Precision.SAFE_MIN); } } AngularCoordinates acId = AngularCoordinates.createFromModifiedRodrigues(identity); Assert.assertEquals(0.0, acId.getRotation().getAngle(), Precision.SAFE_MIN); Assert.assertEquals(0.0, acId.getRotationRate().getNorm(), Precision.SAFE_MIN); // PI angle rotation (which is singular for non-modified Rodrigues vector) RandomGenerator random = new Well1024a(0x2158523e6accb859l); for (int i = 0; i < 100; ++i) { Vector3D axis = randomVector(random, 1.0); AngularCoordinates original = new AngularCoordinates(new Rotation(axis, FastMath.PI), Vector3D.ZERO, Vector3D.ZERO); AngularCoordinates rebuilt = AngularCoordinates .createFromModifiedRodrigues(original.getModifiedRodrigues(1.0)); Assert.assertEquals(FastMath.PI, rebuilt.getRotation().getAngle(), 1.0e-15); Assert.assertEquals(0.0, FastMath.sin(Vector3D.angle(axis, rebuilt.getRotation().getAxis())), 1.0e-15); Assert.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-16); } }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test @Deprecated // to be removed when AngularCoordinates.interpolate is removed public void testInterpolationSimple() throws OrekitException { AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH; double alpha0 = 0.5 * FastMath.PI; double omega = 0.5 * FastMath.PI; AngularCoordinates reference = new AngularCoordinates(new Rotation(Vector3D.PLUS_K, alpha0), new Vector3D(omega, Vector3D.MINUS_K), Vector3D.ZERO); List<Pair<AbsoluteDate, AngularCoordinates>> sample = new ArrayList<Pair<AbsoluteDate, AngularCoordinates>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { sample.add(new Pair<AbsoluteDate, AngularCoordinates>(date.shiftedBy(dt), reference.shiftedBy(dt))); }/*from w w w .ja va2 s .co m*/ for (double dt = 0; dt < 1.0; dt += 0.001) { AngularCoordinates interpolated = AngularCoordinates.interpolate(date.shiftedBy(dt), true, sample); Rotation r = interpolated.getRotation(); Vector3D rate = interpolated.getRotationRate(); Vector3D acceleration = interpolated.getRotationAcceleration(); Assert.assertEquals(0.0, Rotation.distance(new Rotation(Vector3D.PLUS_K, alpha0 + omega * dt), r), 1.1e-15); Assert.assertEquals(0.0, Vector3D.distance(new Vector3D(omega, Vector3D.MINUS_K), rate), 4.0e-15); Assert.assertEquals(0.0, Vector3D.distance(Vector3D.ZERO, acceleration), 3.0e-14); } }
From source file:org.orekit.utils.TimeStampedAngularCoordinates.java
/** Interpolate angular coordinates. * <p>/*from w ww . ja va 2 s .c o m*/ * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation. * </p> * <p> * This method is based on Sergei Tanygin's paper <a * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues * vector as described in Malcolm D. Shuster's paper <a * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A * Survey of Attitude Representations</a>. This change avoids the singularity at . * There is still a singularity at 2, which is handled by slightly offsetting all rotations * when this singularity is detected. * </p> * <p> * Note that even if first and second time derivatives (rotation rates and acceleration) * from sample can be ignored, the interpolated instance always includes * interpolated derivatives. This feature can be used explicitly to * compute these derivatives when it would be too complex to compute them * from an analytical formula: just compute a few sample points from the * explicit formula and set the derivatives to zero in these sample points, * then use interpolation to add derivatives consistent with the rotations. * </p> * @param date interpolation date * @param filter filter for derivatives from the sample to use in interpolation * @param sample sample points on which interpolation should be done * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedAngularCoordinates> sample) throws OrekitException { // set up safety elements for 2 singularity avoidance final double epsilon = 2 * FastMath.PI / sample.size(); final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4)); // set up a linear model canceling mean rotation rate final Vector3D meanRate; if (filter != AngularDerivativesFilter.USE_R) { Vector3D sum = Vector3D.ZERO; for (final TimeStampedAngularCoordinates datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new Vector3D(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } Vector3D sum = Vector3D.ZERO; TimeStampedAngularCoordinates previous = null; for (final TimeStampedAngularCoordinates datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.date))); } previous = datedAC; } meanRate = new Vector3D(1.0 / (sample.size() - 1), sum); } TimeStampedAngularCoordinates offset = new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO); boolean restart = true; for (int i = 0; restart && i < sample.size() + 2; ++i) { // offset adaptation parameters restart = false; // set up an interpolator taking derivatives into account final HermiteInterpolator interpolator = new HermiteInterpolator(); // add sample points double sign = +1.0; Rotation previous = Rotation.IDENTITY; for (final TimeStampedAngularCoordinates ac : sample) { // remove linear offset from the current coordinates final double dt = ac.date.durationFrom(date); final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt)); // make sure all interpolated points will be on the same branch final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), fixed.getRotation().getQ1(), previous.getQ1(), fixed.getRotation().getQ2(), previous.getQ2(), fixed.getRotation().getQ3(), previous.getQ3()); sign = FastMath.copySign(1.0, dot * sign); previous = fixed.getRotation(); // check modified Rodrigues vector singularity if (fixed.getRotation().getQ0() * sign < threshold) { // the sample point is close to a modified Rodrigues vector singularity // we need to change the linear offset model to avoid this restart = true; break; } final double[][] rodrigues = fixed.getModifiedRodrigues(sign); switch (filter) { case USE_RRA: // populate sample with rotation, rotation rate and acceleration data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]); break; case USE_RR: // populate sample with rotation and rotation rate data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]); break; case USE_R: // populate sample with rotation data only interpolator.addSamplePoint(dt, rodrigues[0]); break; default: // this should never happen throw new OrekitInternalError(null); } } if (restart) { // interpolation failed, some intermediate rotation was too close to 2 // we need to offset all rotations to avoid the singularity offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon), Vector3D.ZERO, Vector3D.ZERO)); } else { // interpolation succeeded with the current offset final DerivativeStructure zero = new DerivativeStructure(1, 2, 0, 0.0); final DerivativeStructure[] p = interpolator.value(zero); final AngularCoordinates ac = createFromModifiedRodrigues( new double[][] { { p[0].getValue(), p[1].getValue(), p[2].getValue() }, { p[0].getPartialDerivative(1), p[1].getPartialDerivative(1), p[2].getPartialDerivative(1) }, { p[0].getPartialDerivative(2), p[1].getPartialDerivative(2), p[2].getPartialDerivative(2) } }); return new TimeStampedAngularCoordinates(offset.getDate(), ac.getRotation(), ac.getRotationRate(), ac.getRotationAcceleration()).addOffset(offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testInterpolationAroundPI() throws OrekitException { List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>(); // add angular coordinates at t0: 179.999 degrees rotation along X axis AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates ac0 = new TimeStampedAngularCoordinates(t0, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(179.999)), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO); sample.add(ac0);// w ww .j a v a2 s. co m // add angular coordinates at t1: -179.999 degrees rotation (= 180.001 degrees) along X axis AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(t1, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(-179.999)), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO); sample.add(ac1); // get interpolated angular coordinates at mid time between t0 and t1 AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates interpolated = TimeStampedAngularCoordinates.interpolate(t, AngularDerivativesFilter.USE_R, sample); Assert.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle(), 1.0e-12); }