List of usage examples for org.apache.commons.math3.util FastMath copySign
public static float copySign(float magnitude, float sign)
From source file:com.tussle.main.Utility.java
public static double addFrom(double value, double amount, double base) { if (amount < 0 && FastMath.min(value - base, base - value) > amount) return base; else//from www.j a va2 s .c o m return amount * FastMath.copySign(1, value - base) + value; }
From source file:org.orekit.bodies.Ellipse.java
/** Find the closest ellipse point. * @param p point in the ellipse plane to project on the ellipse itself * @return closest point belonging to 2D meridian ellipse *//*w ww . j a v a 2 s .c om*/ public Vector2D projectToEllipse(final Vector2D p) { final double x = FastMath.abs(p.getX()); final double y = p.getY(); if (x <= ANGULAR_THRESHOLD * FastMath.abs(y)) { // the point is almost on the minor axis, approximate the ellipse with // the osculating circle whose center is at evolute cusp along minor axis final double osculatingRadius = a2 / b; final double evoluteCuspZ = FastMath.copySign(a * e2 / g, -y); final double deltaZ = y - evoluteCuspZ; final double ratio = osculatingRadius / FastMath.hypot(deltaZ, x); return new Vector2D(FastMath.copySign(ratio * x, p.getX()), evoluteCuspZ + ratio * deltaZ); } if (FastMath.abs(y) <= ANGULAR_THRESHOLD * x) { // the point is almost on the major axis final double osculatingRadius = b2 / a; final double evoluteCuspR = a * e2; final double deltaR = x - evoluteCuspR; if (deltaR >= 0) { // the point is outside of the ellipse evolute, approximate the ellipse // with the osculating circle whose center is at evolute cusp along major axis final double ratio = osculatingRadius / FastMath.hypot(y, deltaR); return new Vector2D(FastMath.copySign(evoluteCuspR + ratio * deltaR, p.getX()), ratio * y); } // the point is on the part of the major axis within ellipse evolute // we can compute the closest ellipse point analytically final double rEllipse = x / e2; return new Vector2D(FastMath.copySign(rEllipse, p.getX()), FastMath.copySign(g * FastMath.sqrt(a2 - rEllipse * rEllipse), y)); } else { final double k = FastMath.hypot(x / a, y / b); double projectedX = x / k; double projectedY = y / k; double deltaX = Double.POSITIVE_INFINITY; double deltaY = Double.POSITIVE_INFINITY; int count = 0; final double threshold = ANGULAR_THRESHOLD * ANGULAR_THRESHOLD * a2; while ((deltaX * deltaX + deltaY * deltaY) > threshold && count++ < 100) { // this loop usually converges in 3 iterations final double omegaX = evoluteFactorX * projectedX * projectedX * projectedX; final double omegaY = evoluteFactorY * projectedY * projectedY * projectedY; final double dx = x - omegaX; final double dy = y - omegaY; final double alpha = b2 * dx * dx + a2 * dy * dy; final double beta = b2 * omegaX * dx + a2 * omegaY * dy; final double gamma = b2 * omegaX * omegaX + a2 * omegaY * omegaY - a2 * b2; final double deltaPrime = MathArrays.linearCombination(beta, beta, -alpha, gamma); final double ratio = (beta <= 0) ? (FastMath.sqrt(deltaPrime) - beta) / alpha : -gamma / (FastMath.sqrt(deltaPrime) + beta); final double previousX = projectedX; final double previousY = projectedY; projectedX = omegaX + ratio * dx; projectedY = omegaY + ratio * dy; deltaX = projectedX - previousX; deltaY = projectedY - previousY; } return new Vector2D(FastMath.copySign(projectedX, p.getX()), projectedY); } }
From source file:org.orekit.bodies.OneAxisEllipsoid.java
/** {@inheritDoc} */ public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date) throws OrekitException { // transform point to body frame final Vector3D pointInBodyFrame = frame.getTransformTo(bodyFrame, date).transformPosition(point); final double r2 = pointInBodyFrame.getX() * pointInBodyFrame.getX() + pointInBodyFrame.getY() * pointInBodyFrame.getY(); final double r = FastMath.sqrt(r2); final double z = pointInBodyFrame.getZ(); // set up the 2D meridian ellipse final Ellipse meridian = new Ellipse(Vector3D.ZERO, new Vector3D(pointInBodyFrame.getX() / r, pointInBodyFrame.getY() / r, 0), Vector3D.PLUS_K, getA(), getC(), bodyFrame);// w ww. ja v a2 s . c o m // project point on the 2D meridian ellipse final Vector2D ellipsePoint = meridian.projectToEllipse(new Vector2D(r, z)); // relative position of test point with respect to its ellipse sub-point final double dr = r - ellipsePoint.getX(); final double dz = z - ellipsePoint.getY(); final double insideIfNegative = g2 * (r2 - ae2) + z * z; return new GeodeticPoint(FastMath.atan2(ellipsePoint.getY(), g2 * ellipsePoint.getX()), FastMath.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX()), FastMath.copySign(FastMath.hypot(dr, dz), insideIfNegative)); }
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); }/*from ww w . j a v a 2 s. c o m*/ }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Computes the hyperbolic eccentric anomaly from the mean anomaly. * <p>//from w ww. ja v a 2s. c o m * The algorithm used here for solving hyperbolic Kepler equation is * Danby's iterative method (3rd order) with Vallado's initial guess. * </p> * @param M mean anomaly (rad) * @param ecc eccentricity * @return the hyperbolic eccentric anomaly */ private double meanToHyperbolicEccentric(final double M, final double ecc) { // Resolution of hyperbolic Kepler equation for keplerian parameters // Initial guess double H; if (ecc < 1.6) { if ((-FastMath.PI < M && M < 0.) || M > FastMath.PI) { H = M - ecc; } else { H = M + ecc; } } else { if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) { H = M - FastMath.copySign(ecc, M); } else { H = M / (ecc - 1.); } } // Iterative computation int iter = 0; do { final double f3 = ecc * FastMath.cosh(H); final double f2 = ecc * FastMath.sinh(H); final double f1 = f3 - 1.; final double f0 = f2 - H - M; final double f12 = 2. * f1; final double d = f0 / f12; final double fdf = f1 - d * f2; final double ds = f0 / fdf; final double shift = f0 / (fdf + ds * ds * f3 / 6.); H -= shift; if (FastMath.abs(shift) <= 1.0e-12) { return H; } } while (++iter < 50); throw new ConvergenceException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY, iter); }
From source file:org.orekit.propagation.analytical.AbstractAnalyticalPropagator.java
/** {@inheritDoc} */ public SpacecraftState propagate(final AbsoluteDate start, final AbsoluteDate target) throws PropagationException { try {/*from w w w .j a va2 s. c o m*/ lastPropagationStart = start; final double dt = target.durationFrom(start); final double epsilon = FastMath.ulp(dt); interpolator.storeDate(start); SpacecraftState state = interpolator.getInterpolatedState(); // evaluate step size final double stepSize; if (getMode() == MASTER_MODE) { if (Double.isNaN(getFixedStepSize())) { stepSize = FastMath.copySign(state.getKeplerianPeriod() / 100, dt); } else { stepSize = FastMath.copySign(getFixedStepSize(), dt); } } else { stepSize = dt; } // initialize event detectors for (final EventState<?> es : eventsStates) { es.init(state, target); } // initialize step handler if (getStepHandler() != null) { getStepHandler().init(state, target); } // iterate over the propagation range statesInitialized = false; isLastStep = false; do { // go ahead one step size interpolator.shift(); final AbsoluteDate t = interpolator.getCurrentDate().shiftedBy(stepSize); if ((dt == 0) || ((dt > 0) ^ (t.compareTo(target) <= 0))) { // current step exceeds target interpolator.storeDate(target); } else { // current step is within range interpolator.storeDate(t); } // accept the step, trigger events and step handlers state = acceptStep(target, epsilon); } while (!isLastStep); // return the last computed state lastPropagationEnd = state.getDate(); setStartDate(state.getDate()); return state; } catch (PropagationException pe) { throw pe; } catch (OrekitException oe) { throw PropagationException.unwrap(oe); } catch (TooManyEvaluationsException tmee) { throw PropagationException.unwrap(tmee); } catch (NoBracketingException nbe) { throw PropagationException.unwrap(nbe); } }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTSolarRadiationPressure.java
/** * Compute the real roots of a quartic equation. * * <pre>/*w w w . j a v a 2 s.c o m*/ * a[0] * x? + a[1] * x + a[2] * x + a[3] * x + a[4] = 0. * </pre> * * @param a the 5 coefficients * @param y the real roots * @return the number of real roots */ private int realQuarticRoots(final double[] a, final double[] y) { /* Treat the degenerate quartic as cubic */ if (Precision.equals(a[0], 0.)) { final double[] aa = new double[a.length - 1]; System.arraycopy(a, 1, aa, 0, aa.length); return realCubicRoots(aa, y); } // Transform coefficients final double b = a[1] / a[0]; final double c = a[2] / a[0]; final double d = a[3] / a[0]; final double e = a[4] / a[0]; final double bh = b * 0.5; // Solve resolvant cubic final double[] z3 = new double[3]; final int i3 = realCubicRoots(new double[] { 1.0, -c, b * d - 4. * e, e * (4. * c - b * b) - d * d }, z3); if (i3 == 0) { return 0; } // Largest real root of resolvant cubic final double z = z3[0]; // Compute auxiliary quantities final double zh = z * 0.5; final double p = FastMath.max(z + bh * bh - c, 0.); final double q = FastMath.max(zh * zh - e, 0.); final double r = bh * z - d; final double pp = FastMath.sqrt(p); final double qq = FastMath.copySign(FastMath.sqrt(q), r); // Solve quadratic factors of quartic equation final double[] y1 = new double[2]; final int n1 = realQuadraticRoots(new double[] { 1.0, bh - pp, zh - qq }, y1); final double[] y2 = new double[2]; final int n2 = realQuadraticRoots(new double[] { 1.0, bh + pp, zh + qq }, y2); if (n1 == 2) { if (n2 == 2) { y[0] = y1[0]; y[1] = y1[1]; y[2] = y2[0]; y[3] = y2[1]; return 4; } else { y[0] = y1[0]; y[1] = y1[1]; return 2; } } else { if (n2 == 2) { y[0] = y2[0]; y[1] = y2[1]; return 2; } else { return 0; } } }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTSolarRadiationPressure.java
/** * Compute the real roots of a cubic equation. * * <pre>//from w ww .j av a 2 s .c o m * a[0] * x + a[1] * x + a[2] * x + a[3] = 0. * </pre> * * @param a the 4 coefficients * @param y the real roots sorted in descending order * @return the number of real roots */ private int realCubicRoots(final double[] a, final double[] y) { if (Precision.equals(a[0], 0.)) { // Treat the degenerate cubic as quadratic final double[] aa = new double[a.length - 1]; System.arraycopy(a, 1, aa, 0, aa.length); return realQuadraticRoots(aa, y); } // Transform coefficients final double b = -a[1] / (3. * a[0]); final double c = a[2] / a[0]; final double d = a[3] / a[0]; final double b2 = b * b; final double p = b2 - c / 3.; final double q = b * (b2 - c * 0.5) - d * 0.5; // Compute discriminant final double disc = p * p * p - q * q; if (disc < 0.) { // One real root final double alpha = q + FastMath.copySign(FastMath.sqrt(-disc), q); final double cbrtAl = FastMath.cbrt(alpha); final double cbrtBe = p / cbrtAl; if (p < 0.) { y[0] = b + 2. * q / (cbrtAl * cbrtAl + cbrtBe * cbrtBe - p); } else if (p > 0.) { y[0] = b + cbrtAl + cbrtBe; } else { y[0] = b + cbrtAl; } return 1; } else if (disc > 0.) { // Three distinct real roots final double phi = FastMath.atan2(FastMath.sqrt(disc), q) / 3.; final double sqP = 2.0 * FastMath.sqrt(p); y[0] = b + sqP * FastMath.cos(phi); y[1] = b - sqP * FastMath.cos(FastMath.PI / 3. + phi); y[2] = b - sqP * FastMath.cos(FastMath.PI / 3. - phi); return 3; } else { // One distinct and two equals real roots final double cbrtQ = FastMath.cbrt(q); final double root1 = b + 2. * cbrtQ; final double root2 = b - cbrtQ; if (q < 0.) { y[0] = root2; y[1] = root2; y[2] = root1; } else { y[0] = root1; y[1] = root2; y[2] = root2; } return 3; } }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTSolarRadiationPressure.java
/** * Compute the real roots of a quadratic equation. * * <pre>/*from w w w . j av a 2s . c o m*/ * a[0] * x + a[1] * x + a[2] = 0. * </pre> * * @param a the 3 coefficients * @param y the real roots sorted in descending order * @return the number of real roots */ private int realQuadraticRoots(final double[] a, final double[] y) { if (Precision.equals(a[0], 0.)) { // Degenerate quadratic if (Precision.equals(a[1], 0.)) { // Degenerate linear equation: no real roots return 0; } // Linear equation: one real root y[0] = -a[2] / a[1]; return 1; } // Transform coefficients final double b = -0.5 * a[1] / a[0]; final double c = a[2] / a[0]; // Compute discriminant final double d = b * b - c; if (d < 0.) { // No real roots return 0; } else if (d > 0.) { // Two distinct real roots final double y0 = b + FastMath.copySign(FastMath.sqrt(d), b); final double y1 = c / y0; y[0] = FastMath.max(y0, y1); y[1] = FastMath.min(y0, y1); return 2; } else { // Discriminant is zero: two equal real roots y[0] = b; y[1] = b; return 2; } }
From source file:org.orekit.utils.TimeStampedAngularCoordinates.java
/** Interpolate angular coordinates. * <p>/*from w w w . j a v a 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); }