List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getZ
public double getZ()
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** Compute both the gradient and the hessian of the non-central part of the gravity field. * @param date current date//from w w w .ja v a 2 s . c o m * @param position position at which gravity field is desired in body frame * @return gradient and hessian of the non-central part of the gravity field * @exception OrekitException if position cannot be converted to central body frame */ public GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position) throws OrekitException { final int degree = provider.getMaxDegree(); final int order = provider.getMaxOrder(); final NormalizedSphericalHarmonics harmonics = provider.onDate(date); // allocate the columns for recursion double[] pnm0Plus2 = new double[degree + 1]; double[] pnm0Plus1 = new double[degree + 1]; double[] pnm0 = new double[degree + 1]; double[] pnm1Plus1 = new double[degree + 1]; double[] pnm1 = new double[degree + 1]; final double[] pnm2 = new double[degree + 1]; // compute polar coordinates final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double x2 = x * x; final double y2 = y * y; final double z2 = z * z; final double r2 = x2 + y2 + z2; final double r = FastMath.sqrt(r2); final double rho2 = x2 + y2; final double rho = FastMath.sqrt(rho2); final double t = z / r; // cos(theta), where theta is the polar angle final double u = rho / r; // sin(theta), where theta is the polar angle final double tOu = z / rho; // compute distance powers final double[] aOrN = createDistancePowersArray(provider.getAe() / r); // compute longitude cosines/sines final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho); // outer summation over order int index = 0; double value = 0; final double[] gradient = new double[3]; final double[][] hessian = new double[3][3]; for (int m = degree; m >= 0; --m) { // compute tesseral terms index = computeTesseral(m, degree, index, t, u, tOu, pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2); if (m <= order) { // compute contribution of current order to field (equation 5 of the paper) // inner summation over degree, for fixed order double sumDegreeS = 0; double sumDegreeC = 0; double dSumDegreeSdR = 0; double dSumDegreeCdR = 0; double dSumDegreeSdTheta = 0; double dSumDegreeCdTheta = 0; double d2SumDegreeSdRdR = 0; double d2SumDegreeSdRdTheta = 0; double d2SumDegreeSdThetadTheta = 0; double d2SumDegreeCdRdR = 0; double d2SumDegreeCdRdTheta = 0; double d2SumDegreeCdThetadTheta = 0; for (int n = FastMath.max(2, m); n <= degree; ++n) { final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m); final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m); final double nOr = n / r; final double nnP1Or2 = nOr * (n + 1) / r; final double s0 = pnm0[n] * qSnm; final double c0 = pnm0[n] * qCnm; final double s1 = pnm1[n] * qSnm; final double c1 = pnm1[n] * qCnm; final double s2 = pnm2[n] * qSnm; final double c2 = pnm2[n] * qCnm; sumDegreeS += s0; sumDegreeC += c0; dSumDegreeSdR -= nOr * s0; dSumDegreeCdR -= nOr * c0; dSumDegreeSdTheta += s1; dSumDegreeCdTheta += c1; d2SumDegreeSdRdR += nnP1Or2 * s0; d2SumDegreeSdRdTheta -= nOr * s1; d2SumDegreeSdThetadTheta += s2; d2SumDegreeCdRdR += nnP1Or2 * c0; d2SumDegreeCdRdTheta -= nOr * c1; d2SumDegreeCdThetadTheta += c2; } // contribution to outer summation over order final double sML = cosSinLambda[1][m]; final double cML = cosSinLambda[0][m]; value = value * u + sML * sumDegreeS + cML * sumDegreeC; gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR; gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC); gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta; hessian[0][0] = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR; hessian[1][0] = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR); hessian[2][0] = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta; hessian[1][1] = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC); hessian[2][1] = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta); hessian[2][2] = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta; } // rotate the recursion arrays final double[] tmp0 = pnm0Plus2; pnm0Plus2 = pnm0Plus1; pnm0Plus1 = pnm0; pnm0 = tmp0; final double[] tmp1 = pnm1Plus1; pnm1Plus1 = pnm1; pnm1 = tmp1; } // scale back value = FastMath.scalb(value, SCALING); for (int i = 0; i < 3; ++i) { gradient[i] = FastMath.scalb(gradient[i], SCALING); for (int j = 0; j <= i; ++j) { hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING); } } // apply the global mu/r factor final double muOr = mu / r; value *= muOr; gradient[0] = muOr * gradient[0] - value / r; gradient[1] *= muOr; gradient[2] *= muOr; hessian[0][0] = muOr * hessian[0][0] - 2 * gradient[0] / r; hessian[1][0] = muOr * hessian[1][0] - gradient[1] / r; hessian[2][0] = muOr * hessian[2][0] - gradient[2] / r; hessian[1][1] *= muOr; hessian[2][1] *= muOr; hessian[2][2] *= muOr; // convert gradient and Hessian from spherical to Cartesian final SphericalCoordinates sc = new SphericalCoordinates(position); return new GradientHessian(sc.toCartesianGradient(gradient), sc.toCartesianHessian(hessian, gradient)); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** {@inheritDoc} */ public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { // get the position in body frame final AbsoluteDate date = s.getDate(); final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); // gradient of the non-central part of the gravity field final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position))); adder.addXYZAcceleration(gInertial.getX(), gInertial.getY(), gInertial.getZ()); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName) throws OrekitException, IllegalArgumentException { complainIfNotSupported(paramName);/*from www. ja va 2 s . c o m*/ // get the position in body frame final AbsoluteDate date = s.getDate(); final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); // gradient of the non-central part of the gravity field final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position))); return new FieldVector3D<DerivativeStructure>( new DerivativeStructure(1, 1, gInertial.getX(), gInertial.getX() / mu), new DerivativeStructure(1, 1, gInertial.getY(), gInertial.getY() / mu), new DerivativeStructure(1, 1, gInertial.getZ(), gInertial.getZ() / mu)); }
From source file:org.orekit.forces.gravity.ReferenceFieldModel.java
public Dfp nonCentralPart(final AbsoluteDate date, final Vector3D position) throws OrekitException { int degree = provider.getMaxDegree(); int order = provider.getMaxOrder(); //use coefficients without caring if they are the correct type final RawSphericalHarmonics harmonics = raw(provider).onDate(date); Dfp x = dfpField.newDfp(position.getX()); Dfp y = dfpField.newDfp(position.getY()); Dfp z = dfpField.newDfp(position.getZ()); Dfp rho2 = x.multiply(x).add(y.multiply(y)); Dfp rho = rho2.sqrt();//from w w w. java 2 s . co m Dfp r2 = rho2.add(z.multiply(z)); Dfp r = r2.sqrt(); Dfp aOr = dfpField.newDfp(provider.getAe()).divide(r); Dfp lambda = position.getX() > 0 ? dfpField.getTwo().multiply(DfpMath.atan(y.divide(rho.add(x)))) : dfpField.getPi().subtract(dfpField.getTwo().multiply(DfpMath.atan(y.divide(rho.subtract(x))))); Dfp cosTheta = z.divide(r); Dfp value = dfpField.getZero(); Dfp aOrN = aOr; for (int n = 2; n <= degree; ++n) { Dfp sum = dfpField.getZero(); for (int m = 0; m <= FastMath.min(n, order); ++m) { double cnm = harmonics.getRawCnm(n, m); double snm = harmonics.getRawSnm(n, m); Dfp mLambda = lambda.multiply(m); Dfp c = DfpMath.cos(mLambda).multiply(dfpField.newDfp(cnm)); Dfp s = DfpMath.sin(mLambda).multiply(dfpField.newDfp(snm)); Dfp pnm = alf[n][m].value(cosTheta); sum = sum.add(pnm.multiply(c.add(s))); } aOrN = aOrN.multiply(aOr); value = value.add(aOrN.multiply(sum)); } return value.multiply(dfpField.newDfp(provider.getMu())).divide(r); }
From source file:org.orekit.forces.gravity.SolidTidesField.java
/** {@inheritDoc} */ @Override//from w w w . j a va 2 s. c om public NormalizedSphericalHarmonics onDate(final AbsoluteDate date) throws OrekitException { // computed Cnm and Snm coefficients final double[][] cnm = buildTriangularArray(5, true); final double[][] snm = buildTriangularArray(5, true); // work array to hold Legendre coefficients final double[][] pnm = buildTriangularArray(5, true); // step 1: frequency independent part // equations 6.6 (for degrees 2 and 3) and 6.7 (for degree 4) in IERS conventions 2010 for (final CelestialBody body : bodies) { // compute tide generating body state final Vector3D position = body.getPVCoordinates(date, centralBodyFrame).getPosition(); // compute polar coordinates final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double x2 = x * x; final double y2 = y * y; final double z2 = z * z; final double r2 = x2 + y2 + z2; final double r = FastMath.sqrt(r2); final double rho2 = x2 + y2; final double rho = FastMath.sqrt(rho2); // evaluate Pnm evaluateLegendre(z / r, rho / r, pnm); // update spherical harmonic coefficients frequencyIndependentPart(r, body.getGM(), x / rho, y / rho, pnm, cnm, snm); } // step 2: frequency dependent corrections frequencyDependentPart(date, cnm, snm); if (centralTideSystem == TideSystem.ZERO_TIDE) { // step 3: remove permanent tide which is already considered // in the central body gravity field removePermanentTide(cnm); } if (poleTideFunction != null) { // add pole tide poleTide(date, cnm, snm); } return new TideHarmonics(date, cnm, snm); }
From source file:org.orekit.forces.gravity.ThirdBodyAttraction.java
/** {@inheritDoc} */ public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { // compute bodies separation vectors and squared norm final Vector3D centralToBody = body.getPVCoordinates(s.getDate(), s.getFrame()).getPosition(); final double r2Central = centralToBody.getNormSq(); final Vector3D satToBody = centralToBody.subtract(s.getPVCoordinates().getPosition()); final double r2Sat = satToBody.getNormSq(); // compute relative acceleration final Vector3D gamma = new Vector3D(gm / (r2Sat * FastMath.sqrt(r2Sat)), satToBody, -gm / (r2Central * FastMath.sqrt(r2Central)), centralToBody); // add contribution to the ODE second member adder.addXYZAcceleration(gamma.getX(), gamma.getY(), gamma.getZ()); }
From source file:org.orekit.forces.maneuvers.ImpulseManeuverTest.java
@Test public void testInertialManeuver() throws OrekitException { final double mu = CelestialBodyFactory.getEarth().getGM(); final double initialX = 7100e3; final double initialY = 0.0; final double initialZ = 1300e3; final double initialVx = 0; final double initialVy = 8000; final double initialVz = 1000; final Vector3D position = new Vector3D(initialX, initialY, initialZ); final Vector3D velocity = new Vector3D(initialVx, initialVy, initialVz); final AbsoluteDate epoch = new AbsoluteDate(2010, 1, 1, 0, 0, 0, TimeScalesFactory.getUTC()); final TimeStampedPVCoordinates state = new TimeStampedPVCoordinates(epoch, position, velocity, Vector3D.ZERO);/*w ww . jav a2 s .c om*/ final Orbit initialOrbit = new CartesianOrbit(state, FramesFactory.getEME2000(), mu); final double totalPropagationTime = 0.00001; final double driftTimeInSec = totalPropagationTime / 2.0; final double deltaX = 0.01; final double deltaY = 0.02; final double deltaZ = 0.03; final double isp = 300; final Vector3D deltaV = new Vector3D(deltaX, deltaY, deltaZ); KeplerianPropagator propagator = new KeplerianPropagator(initialOrbit, new LofOffset(initialOrbit.getFrame(), LOFType.VNC)); DateDetector dateDetector = new DateDetector(epoch.shiftedBy(driftTimeInSec)); InertialProvider attitudeOverride = new InertialProvider(new Rotation(RotationOrder.XYX, 0, 0, 0)); ImpulseManeuver<DateDetector> burnAtEpoch = new ImpulseManeuver<DateDetector>(dateDetector, attitudeOverride, deltaV, isp).withThreshold(driftTimeInSec / 4); propagator.addEventDetector(burnAtEpoch); SpacecraftState finalState = propagator.propagate(epoch.shiftedBy(totalPropagationTime)); final double finalVxExpected = initialVx + deltaX; final double finalVyExpected = initialVy + deltaY; final double finalVzExpected = initialVz + deltaZ; final double maneuverTolerance = 1e-4; final Vector3D finalVelocity = finalState.getPVCoordinates().getVelocity(); Assert.assertEquals(finalVxExpected, finalVelocity.getX(), maneuverTolerance); Assert.assertEquals(finalVyExpected, finalVelocity.getY(), maneuverTolerance); Assert.assertEquals(finalVzExpected, finalVelocity.getZ(), maneuverTolerance); }
From source file:org.orekit.frames.HelmertTransformationTest.java
private Vector3D computeOffsetLinearly(final double t1, final double t2, final double t3, final double r1, final double r2, final double r3, final double t1Dot, final double t2Dot, final double t3Dot, final double r1Dot, final double r2Dot, final double r3Dot, final Vector3D p, final double dt) { double t1U = (t1 + dt * t1Dot) * 1.0e-3; double t2U = (t2 + dt * t2Dot) * 1.0e-3; double t3U = (t3 + dt * t3Dot) * 1.0e-3; double r1U = FastMath.toRadians((r1 + dt * r1Dot) / 3.6e6); double r2U = FastMath.toRadians((r2 + dt * r2Dot) / 3.6e6); double r3U = FastMath.toRadians((r3 + dt * r3Dot) / 3.6e6); return new Vector3D(t1U - r3U * p.getY() + r2U * p.getZ(), t2U + r3U * p.getX() - r1U * p.getZ(), t3U - r2U * p.getX() + r1U * p.getY()); }
From source file:org.orekit.frames.ITRFEquinoxProviderTest.java
private void checkFrames(Frame frame1, Frame frame2, double toleranceMicroAS) throws OrekitException { AbsoluteDate t0 = new AbsoluteDate(2005, 5, 30, TimeScalesFactory.getUTC()); for (double dt = 0; dt < Constants.JULIAN_YEAR; dt += Constants.JULIAN_DAY / 4) { AbsoluteDate date = t0.shiftedBy(dt); Transform t = FramesFactory.getNonInterpolatingTransform(frame1, frame2, date); Vector3D a = t.getRotation().getAxis(); double delta = FastMath.copySign(radToMicroAS(t.getRotation().getAngle()), a.getZ()); Assert.assertEquals(0.0, delta, toleranceMicroAS); }/*w w w .j av a 2 s . c om*/ }
From source file:org.orekit.frames.ITRFProviderWithoutTidalEffectsTest.java
@Test public void testMSLIBTransformJ2000_TerVrai() throws OrekitException { AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 10, 14), new TimeComponents(02, 00, 00), TimeScalesFactory.getUTC()); Transform trans = FramesFactory.getEME2000() .getTransformTo(FramesFactory.getTIRF(IERSConventions.IERS_2010), date); // Positions/* w ww . j av a 2 s.c om*/ Vector3D posTIRF = trans.transformPosition(new Vector3D(6500000.0, -1234567.0, 4000000.0)); Assert.assertEquals(0, 3011109.361 - posTIRF.getX(), 0.38); Assert.assertEquals(0, -5889822.669 - posTIRF.getY(), 0.38); Assert.assertEquals(0, 4002170.039 - posTIRF.getZ(), 0.27); }