/* (non-Javadoc)
* @see net.finmath.stochastic.RandomVariableInterface#cos()
*/
public RandomVariableInterface cos() {
if (isDeterministic()) {
double newValueIfNonStochastic = FastMath.cos(valueIfNonStochastic);
return new RandomVariable(time, newValueIfNonStochastic);
} else {
double[] newRealizations = new double[realizations.length];
for (int i = 0; i < newRealizations.length; i++)
newRealizations[i] = FastMath.cos(realizations[i]);
return new RandomVariable(time, newRealizations);
}
}
@Test
public void testNoFlattening() throws OrekitException {
final double r = 7000000.0;
final double lambda = 2.345;
final double phi = -1.23;
final double cL = FastMath.cos(lambda);
final double sL = FastMath.sin(lambda);
final double cH = FastMath.cos(phi);
final double sH = FastMath.sin(phi);
checkCartesianToEllipsoidic(
6378137.0, 0, r * cL * cH, r * sL * cH, r * sH, lambda, phi, r - 6378137.0);
}
/**
* ell2xyz Converts wgs84 ellipsoid cn to geocentric cartesian coord. input: - phi,lam,hei
* (geodetic co-latitude, longitude, [rad] h [m] output: - cn XYZ
*/
public static Point ell2xyz(final double phi, final double lambda, final double height)
throws IllegalArgumentException {
if (phi > Math.PI || phi < -Math.PI || lambda > Math.PI || lambda < -Math.PI) {
throw new IllegalArgumentException(
"Ellipsoid.ell2xyz : input values for phi/lambda have to be in radians!");
}
final double N = computeEllipsoidNormal(phi);
final double Nph = N + height;
final double A = Nph * FastMath.cos(phi);
return new Point(
A * FastMath.cos(lambda), A * FastMath.sin(lambda), (Nph - e2 * N) * FastMath.sin(phi));
}
/**
* Convert xyz cartesian coordinates to Geodetic ellipsoid coordinates latlonh xyz2ell
*
* <p>Converts geocentric cartesian coordinates in the XXXX reference frame to geodetic
* coordinates. method of bowring see globale en locale geodetische systemen input: - ellipsinfo,
* xyz, (phi,lam,hei) output: - void (returned double[] lam<-pi,pi>, phi<-pi,pi>, hei)
*/
public static double[] xyz2ell(final Point xyz) {
final double r = Math.sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
final double nu = Math.atan2((xyz.z * a), (r * b));
final double sinNu = FastMath.sin(nu);
final double cosNu = FastMath.cos(nu);
final double sin3 = sinNu * sinNu * sinNu;
final double cos3 = cosNu * cosNu * cosNu;
final double phi = Math.atan2((xyz.z + e2b * b * sin3), (r - e2 * a * cos3));
final double lambda = Math.atan2(xyz.y, xyz.x);
final double N = computeEllipsoidNormal(phi);
final double height = (r / FastMath.cos(phi)) - N;
return new double[] {phi, lambda, height};
}
@Override
public IComplexNumber exp() {
IComplexNumber result = dup();
double realExp = FastMath.exp(realComponent());
return result.set(
realExp * FastMath.cos(imaginaryComponent()), realExp * FastMath.sin(imaginaryComponent()));
}
/**
* Computes the n-th roots of this complex number. The nth roots are defined by the formula:
*
* <pre>
* <code>
* z<sub>k</sub> = abs<sup>1/n</sup> (cos(phi + 2πk/n) + i (sin(phi + 2πk/n))
* </code>
* </pre>
*
* for <i>{@code k=0, 1, ..., n-1}</i>, where {@code abs} and {@code phi} are respectively the
* {@link #abs() modulus} and {@link #getArgument() argument} of this complex number. <br>
* If one or both parts of this complex number is NaN, a list with just one element, {@link #NaN}
* is returned. if neither part is NaN, but at least one part is infinite, the result is a
* one-element list containing {@link #INF}.
*
* @param n Degree of root.
* @return a List<Complex> of all {@code n}-th roots of {@code this}.
* @throws NotPositiveException if {@code n <= 0}.
* @since 2.0
*/
public List<Complex> nthRoot(int n) throws NotPositiveException {
if (n <= 0) {
throw new NotPositiveException(LocalizedFormats.CANNOT_COMPUTE_NTH_ROOT_FOR_NEGATIVE_N, n);
}
final List<Complex> result = new ArrayList<Complex>();
if (isNaN) {
result.add(NaN);
return result;
}
if (isInfinite()) {
result.add(INF);
return result;
}
// nth root of abs -- faster / more accurate to use a solver here?
final double nthRootOfAbs = FastMath.pow(abs(), 1.0 / n);
// Compute nth roots of complex number with k = 0, 1, ... n-1
final double nthPhi = getArgument() / n;
final double slice = 2 * FastMath.PI / n;
double innerPart = nthPhi;
for (int k = 0; k < n; k++) {
// inner part
final double realPart = nthRootOfAbs * FastMath.cos(innerPart);
final double imaginaryPart = nthRootOfAbs * FastMath.sin(innerPart);
result.add(createComplex(realPart, imaginaryPart));
innerPart += slice;
}
return result;
}
@Test
public void testEventsScheduling() {
FirstOrderDifferentialEquations sincos =
new FirstOrderDifferentialEquations() {
public int getDimension() {
return 2;
}
public void computeDerivatives(double t, double[] y, double[] yDot) {
yDot[0] = y[1];
yDot[1] = -y[0];
}
};
SchedulingChecker sinChecker = new SchedulingChecker(0); // events at 0, PI, 2PI ...
SchedulingChecker cosChecker = new SchedulingChecker(1); // events at PI/2, 3PI/2, 5PI/2 ...
FirstOrderIntegrator integ = new DormandPrince853Integrator(0.001, 1.0, 1.0e-12, 0.0);
integ.addEventHandler(sinChecker, 0.01, 1.0e-7, 100);
integ.addStepHandler(sinChecker);
integ.addEventHandler(cosChecker, 0.01, 1.0e-7, 100);
integ.addStepHandler(cosChecker);
double t0 = 0.5;
double[] y0 = new double[] {FastMath.sin(t0), FastMath.cos(t0)};
double t = 10.0;
double[] y = new double[2];
integ.integrate(sincos, t0, y0, t, y);
}
/**
* Compute the <a href="http://mathworld.wolfram.com/ExponentialFunction.html" TARGET="_top">
* exponential function</a> of this complex number. Implements the formula:
*
* <pre>
* <code>
* exp(a + bi) = exp(a)cos(b) + exp(a)sin(b)i
* </code>
* </pre>
*
* where the (real) functions on the right-hand side are {@link java.lang.Math#exp}, {@link
* java.lang.Math#cos}, and {@link java.lang.Math#sin}. <br>
* Returns {@link Complex#NaN} if either real or imaginary part of the input argument is {@code
* NaN}. <br>
* Infinite values in real or imaginary parts of the input may result in infinite or NaN values
* returned in parts of the result.
*
* <pre>
* Examples:
* <code>
* exp(1 ± INFINITY i) = NaN + NaN i
* exp(INFINITY + i) = INFINITY + INFINITY i
* exp(-INFINITY + i) = 0 + 0i
* exp(±INFINITY ± INFINITY i) = NaN + NaN i
* </code>
* </pre>
*
* @return <code><i>e</i><sup>this</sup></code>.
* @since 1.2
*/
public Complex exp() {
if (isNaN) {
return NaN;
}
double expReal = FastMath.exp(real);
return createComplex(expReal * FastMath.cos(imaginary), expReal * FastMath.sin(imaginary));
}
public static Point ell2xyz(final double[] phiLambdaHeight) throws IllegalArgumentException {
final double phi = phiLambdaHeight[0];
final double lambda = phiLambdaHeight[1];
final double height = phiLambdaHeight[2];
if (phi > Math.PI || phi < -Math.PI || lambda > Math.PI || lambda < -Math.PI) {
throw new IllegalArgumentException(
"Ellipsoid.ell2xyz(): phi/lambda values has to be in radians!");
}
final double N = computeEllipsoidNormal(phi);
final double Nph = N + height;
final double A = Nph * FastMath.cos(phi);
return new Point(
A * FastMath.cos(lambda), A * FastMath.sin(lambda), (Nph - e2 * N) * FastMath.sin(phi));
}
private List<Vector2D> makeCircles(double noise, double factor) {
Preconditions.checkArgument(factor >= 0 && factor <= 1);
NormalDistribution dist = new NormalDistribution(random, 0.0, noise, 1e-9);
List<Vector2D> points = new ArrayList<>();
double step = 2.0 * PI / (samples / 2.0 + 1);
for (double angle = 0; angle < 2.0 * FastMath.PI; angle += step) {
points.add(
new Vector2D(FastMath.cos(angle), FastMath.sin(angle)).add(generateNoiseVector(dist)));
points.add(
new Vector2D(FastMath.cos(angle), FastMath.sin(angle))
.scalarMultiply(factor)
.add(generateNoiseVector(dist)));
}
return points;
}
/**
* Compute the <a href="http://mathworld.wolfram.com/HyperbolicSine.html" TARGET="_top">
* hyperbolic sine</a> of this complex number. Implements the formula:
*
* <pre>
* <code>
* sinh(a + bi) = sinh(a)cos(b)) + cosh(a)sin(b)i
* </code>
* </pre>
*
* where the (real) functions on the right-hand side are {@link java.lang.Math#sin}, {@link
* java.lang.Math#cos}, {@link FastMath#cosh} and {@link FastMath#sinh}. <br>
* Returns {@link Complex#NaN} if either real or imaginary part of the input argument is {@code
* NaN}. <br>
* Infinite values in real or imaginary parts of the input may result in infinite or NaN values
* returned in parts of the result.
*
* <pre>
* Examples:
* <code>
* sinh(1 ± INFINITY i) = NaN + NaN i
* sinh(±INFINITY + i) = ± INFINITY + INFINITY i
* sinh(±INFINITY ± INFINITY i) = NaN + NaN i
* </code>
* </pre>
*
* @return the hyperbolic sine of {@code this}.
* @since 1.2
*/
public Complex sinh() {
if (isNaN) {
return NaN;
}
return createComplex(
FastMath.sinh(real) * FastMath.cos(imaginary),
FastMath.cosh(real) * FastMath.sin(imaginary));
}
private double sincHanning(final double x) {
return x >= -halfKernelSize && x <= halfKernelSize
? x == 0
? 1.0
: FastMath.sin(x * Math.PI)
/ (x * Math.PI)
* (0.5 * (1.0 + FastMath.cos(DoublePI * x / kernelSize1)))
: 0.0;
}
public double execute(double in) throws DMLRuntimeException {
switch (bFunc) {
case SIN:
return FASTMATH ? FastMath.sin(in) : Math.sin(in);
case COS:
return FASTMATH ? FastMath.cos(in) : Math.cos(in);
case TAN:
return FASTMATH ? FastMath.tan(in) : Math.tan(in);
case ASIN:
return FASTMATH ? FastMath.asin(in) : Math.asin(in);
case ACOS:
return FASTMATH ? FastMath.acos(in) : Math.acos(in);
case ATAN:
return Math.atan(in); // faster in Math
case CEIL:
return FASTMATH ? FastMath.ceil(in) : Math.ceil(in);
case FLOOR:
return FASTMATH ? FastMath.floor(in) : Math.floor(in);
case LOG:
return FASTMATH ? FastMath.log(in) : Math.log(in);
case LOG_NZ:
return (in == 0) ? 0 : FASTMATH ? FastMath.log(in) : Math.log(in);
case ABS:
return Math.abs(in); // no need for FastMath
case SIGN:
return FASTMATH ? FastMath.signum(in) : Math.signum(in);
case SQRT:
return Math.sqrt(in); // faster in Math
case EXP:
return FASTMATH ? FastMath.exp(in) : Math.exp(in);
case ROUND:
return Math.round(in); // no need for FastMath
case PLOGP:
if (in == 0.0) return 0.0;
else if (in < 0) return Double.NaN;
else return (in * (FASTMATH ? FastMath.log(in) : Math.log(in)));
case SPROP:
// sample proportion: P*(1-P)
return in * (1 - in);
case SIGMOID:
// sigmoid: 1/(1+exp(-x))
return FASTMATH ? 1 / (1 + FastMath.exp(-in)) : 1 / (1 + Math.exp(-in));
case SELP:
// select positive: x*(x>0)
return (in > 0) ? in : 0;
default:
throw new DMLRuntimeException("Builtin.execute(): Unknown operation: " + bFunc);
}
}
private List<Vector2D> makeMoons(double noise) {
NormalDistribution dist = new NormalDistribution(random, 0.0, noise, 1e-9);
int nSamplesOut = samples / 2;
int nSamplesIn = samples - nSamplesOut;
List<Vector2D> points = new ArrayList<>();
double step = PI / (nSamplesOut / 2.0);
for (double angle = 0; angle < PI; angle += step) {
points.add(
new Vector2D(FastMath.cos(angle), FastMath.sin(angle)).add(generateNoiseVector(dist)));
}
step = PI / (nSamplesIn / 2.0);
for (double angle = 0; angle < PI; angle += step) {
points.add(
new Vector2D(1 - FastMath.cos(angle), 1 - FastMath.sin(angle) - 0.5)
.add(generateNoiseVector(dist)));
}
return points;
}
public double value(double[] x) {
double f = 0;
double fac;
for (int i = 0; i < x.length; ++i) {
fac = FastMath.pow(axisratio, (i - 1.) / (x.length - 1.));
if (i == 0 && x[i] < 0) fac *= 1.;
f +=
fac * fac * x[i] * x[i]
+ amplitude * (1. - FastMath.cos(2. * FastMath.PI * fac * x[i]));
}
return f;
}
/**
* Set this ray to a random diffuse reflection of the input ray.
*
* @param ray
* @param random
*/
public final void diffuseReflection(Ray ray, Random random) {
set(ray);
// get random point on unit disk
double x1 = random.nextDouble();
double x2 = random.nextDouble();
double r = FastMath.sqrt(x1);
double theta = 2 * Math.PI * x2;
// project to point on hemisphere in tangent space
double tx = r * FastMath.cos(theta);
double ty = r * FastMath.sin(theta);
double tz = FastMath.sqrt(1 - x1);
// transform from tangent space to world space
double xx, xy, xz;
double ux, uy, uz;
double vx, vy, vz;
if (QuickMath.abs(n.x) > .1) {
xx = 0;
xy = 1;
xz = 0;
} else {
xx = 1;
xy = 0;
xz = 0;
}
ux = xy * n.z - xz * n.y;
uy = xz * n.x - xx * n.z;
uz = xx * n.y - xy * n.x;
r = 1 / FastMath.sqrt(ux * ux + uy * uy + uz * uz);
ux *= r;
uy *= r;
uz *= r;
vx = uy * n.z - uz * n.y;
vy = uz * n.x - ux * n.z;
vz = ux * n.y - uy * n.x;
d.x = ux * tx + vx * ty + n.x * tz;
d.y = uy * tx + vy * ty + n.y * tz;
d.z = uz * tx + vz * ty + n.z * tz;
x.scaleAdd(Ray.OFFSET, d, x);
currentMaterial = prevMaterial;
specular = false;
}
/**
* Compute the differential effect of J2 on an orbit.
*
* @param orbit1 original orbit at t₁, without differential J2
* @return orbit at t₁, always taking the effect into account
*/
private Orbit updateOrbit(final Orbit orbit1) {
// convert current orbital state to equinoctial elements
final EquinoctialOrbit original = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit1);
// compute differential effect
final AbsoluteDate date = original.getDate();
final double dt = date.durationFrom(referenceDate);
final double dPaRaan = (dPaDot + dRaanDot) * dt;
final double cPaRaan = FastMath.cos(dPaRaan);
final double sPaRaan = FastMath.sin(dPaRaan);
final double dRaan = dRaanDot * dt;
final double cRaan = FastMath.cos(dRaan);
final double sRaan = FastMath.sin(dRaan);
final double ex = original.getEquinoctialEx() * cPaRaan - original.getEquinoctialEy() * sPaRaan;
final double ey = original.getEquinoctialEx() * sPaRaan + original.getEquinoctialEy() * cPaRaan;
final double hx = original.getHx() * cRaan - original.getHy() * sRaan;
final double hy = original.getHx() * sRaan + original.getHy() * cRaan;
final double lambda = original.getLv() + dPaRaan;
// build updated orbit
final EquinoctialOrbit updated =
new EquinoctialOrbit(
original.getA(),
ex,
ey,
hx,
hy,
lambda,
PositionAngle.TRUE,
original.getFrame(),
date,
original.getMu());
// convert to required type
return orbit1.getType().convertType(updated);
}
public double value(double[] x) {
double f = 0;
double res2 = 0;
double fac = 0;
for (int i = 0; i < x.length; ++i) {
fac = FastMath.pow(axisratio, (i - 1.) / (x.length - 1.));
f += fac * fac * x[i] * x[i];
res2 += FastMath.cos(2. * FastMath.PI * fac * x[i]);
}
f =
(20.
- 20. * FastMath.exp(-0.2 * FastMath.sqrt(f / x.length))
+ FastMath.exp(1.)
- FastMath.exp(res2 / x.length));
return f;
}
/**
* Compute the <a href="http://mathworld.wolfram.com/HyperbolicTangent.html" TARGET="_top">
* hyperbolic tangent</a> of this complex number. Implements the formula:
*
* <pre>
* <code>
* tan(a + bi) = sinh(2a)/(cosh(2a)+cos(2b)) + [sin(2b)/(cosh(2a)+cos(2b))]i
* </code>
* </pre>
*
* where the (real) functions on the right-hand side are {@link FastMath#sin}, {@link
* FastMath#cos}, {@link FastMath#cosh} and {@link FastMath#sinh}. <br>
* Returns {@link Complex#NaN} if either real or imaginary part of the input argument is {@code
* NaN}. <br>
* Infinite values in real or imaginary parts of the input may result in infinite or NaN values
* returned in parts of the result.
*
* <pre>
* Examples:
* <code>
* tanh(a ± INFINITY i) = NaN + NaN i
* tanh(±INFINITY + bi) = ±1 + 0 i
* tanh(±INFINITY ± INFINITY i) = NaN + NaN i
* tanh(0 + (π/2)i) = NaN + INFINITY i
* </code>
* </pre>
*
* @return the hyperbolic tangent of {@code this}.
* @since 1.2
*/
public Complex tanh() {
if (isNaN || Double.isInfinite(imaginary)) {
return NaN;
}
if (real > 20.0) {
return createComplex(1.0, 0.0);
}
if (real < -20.0) {
return createComplex(-1.0, 0.0);
}
double real2 = 2.0 * real;
double imaginary2 = 2.0 * imaginary;
double d = FastMath.cosh(real2) + FastMath.cos(imaginary2);
return createComplex(FastMath.sinh(real2) / d, FastMath.sin(imaginary2) / d);
}
@Test
public void testModelsMerging() throws MaxCountExceededException, MathIllegalArgumentException {
// theoretical solution: y[0] = cos(t), y[1] = sin(t)
FirstOrderDifferentialEquations problem =
new FirstOrderDifferentialEquations() {
public void computeDerivatives(double t, double[] y, double[] dot) {
dot[0] = -y[1];
dot[1] = y[0];
}
public int getDimension() {
return 2;
}
};
// integrate backward from π to 0;
ContinuousOutputModel cm1 = new ContinuousOutputModel();
FirstOrderIntegrator integ1 = new DormandPrince853Integrator(0, 1.0, 1.0e-8, 1.0e-8);
integ1.addStepHandler(cm1);
integ1.integrate(problem, FastMath.PI, new double[] {-1.0, 0.0}, 0, new double[2]);
// integrate backward from 2π to π
ContinuousOutputModel cm2 = new ContinuousOutputModel();
FirstOrderIntegrator integ2 = new DormandPrince853Integrator(0, 0.1, 1.0e-12, 1.0e-12);
integ2.addStepHandler(cm2);
integ2.integrate(
problem, 2.0 * FastMath.PI, new double[] {1.0, 0.0}, FastMath.PI, new double[2]);
// merge the two half circles
ContinuousOutputModel cm = new ContinuousOutputModel();
cm.append(cm2);
cm.append(new ContinuousOutputModel());
cm.append(cm1);
// check circle
Assert.assertEquals(2.0 * FastMath.PI, cm.getInitialTime(), 1.0e-12);
Assert.assertEquals(0, cm.getFinalTime(), 1.0e-12);
Assert.assertEquals(cm.getFinalTime(), cm.getInterpolatedTime(), 1.0e-12);
for (double t = 0; t < 2.0 * FastMath.PI; t += 0.1) {
cm.setInterpolatedTime(t);
double[] y = cm.getInterpolatedState();
Assert.assertEquals(FastMath.cos(t), y[0], 1.0e-7);
Assert.assertEquals(FastMath.sin(t), y[1], 1.0e-7);
}
}
/** {@inheritDoc} */
public double[] gradient(final double x, final double... parameters) {
final double[] gradient = new double[secularDegree + 1 + 2 * pulsations.length];
// secular part
double xN = 1.0;
for (int i = 0; i <= secularDegree; ++i) {
gradient[i] = xN;
xN *= x;
}
// harmonic part
for (int i = 0; i < pulsations.length; ++i) {
gradient[secularDegree + 2 * i + 1] = FastMath.cos(pulsations[i] * x);
gradient[secularDegree + 2 * i + 2] = FastMath.sin(pulsations[i] * x);
}
return gradient;
}
/**
* Simple constructor.
*
* <p>The {@code applyBefore} parameter is mainly used when the differential effect is associated
* with a maneuver. In this case, the parameter must be set to {@code false}.
*
* @param orbit0 original orbit at reference date
* @param orbit1 shifted orbit at reference date
* @param applyBefore if true, effect is applied both before and after reference date, if false it
* is only applied after reference date
* @param referenceRadius reference radius of the Earth for the potential model (m)
* @param mu central attraction coefficient (m³/s²)
* @param j2 un-normalized zonal coefficient (about +1.08e-3 for Earth)
*/
public J2DifferentialEffect(
final Orbit orbit0,
final Orbit orbit1,
final boolean applyBefore,
final double referenceRadius,
final double mu,
final double j2) {
this.referenceDate = orbit0.getDate();
this.applyBefore = applyBefore;
// extract useful parameters
final double a0 = orbit0.getA();
final double e0 = orbit0.getE();
final double i0 = orbit0.getI();
final double a1 = orbit1.getA();
final double e1 = orbit1.getE();
final double i1 = orbit1.getI();
// compute reference drifts
final double oMe2 = 1 - e0 * e0;
final double ratio = referenceRadius / (a0 * oMe2);
final double cosI = FastMath.cos(i0);
final double sinI = FastMath.sin(i0);
final double n = FastMath.sqrt(mu / a0) / a0;
final double c = ratio * ratio * n * j2;
final double refPaDot = 0.75 * c * (4 - 5 * sinI * sinI);
final double refRaanDot = -1.5 * c * cosI;
// differential model on perigee argument drift
final double dPaDotDa = -3.5 * refPaDot / a0;
final double dPaDotDe = 4 * refPaDot * e0 / oMe2;
final double dPaDotDi = -7.5 * c * sinI * cosI;
dPaDot = dPaDotDa * (a1 - a0) + dPaDotDe * (e1 - e0) + dPaDotDi * (i1 - i0);
// differential model on ascending node drift
final double dRaanDotDa = -3.5 * refRaanDot / a0;
final double dRaanDotDe = 4 * refRaanDot * e0 / oMe2;
final double dRaanDotDi = -refRaanDot * FastMath.tan(i0);
dRaanDot = dRaanDotDa * (a1 - a0) + dRaanDotDe * (e1 - e0) + dRaanDotDi * (i1 - i0);
}
/**
* Get value truncated to first components.
*
* @param degree degree of polynomial secular part
* @param harmonics number of harmonics terms to consider
* @param time time parameter
* @param parameters models parameters (must include all parameters, including the ones ignored
* due to model truncation)
* @return truncated value
*/
private double truncatedValue(
final int degree, final int harmonics, final double time, final double... parameters) {
double value = 0;
// secular part
double tN = 1.0;
for (int i = 0; i <= degree; ++i) {
value += parameters[i] * tN;
tN *= time;
}
// harmonic part
for (int i = 0; i < harmonics; ++i) {
value +=
parameters[secularDegree + 2 * i + 1] * FastMath.cos(pulsations[i] * time)
+ parameters[secularDegree + 2 * i + 2] * FastMath.sin(pulsations[i] * time);
}
return value;
}
/**
* Compute DEM traversal step sizes (in degree) in latitude and longitude.
*
* @throws Exception The exceptions.
*/
private void computeDEMTraversalSampleInterval() throws Exception {
double[] latLonMinMax = new double[4];
computeImageGeoBoundary(0, sourceImageWidth - 1, 0, sourceImageHeight - 1, latLonMinMax);
final double groundRangeSpacing = SARGeocoding.getRangePixelSpacing(sourceProduct);
final double azimuthPixelSpacing = SARGeocoding.getAzimuthPixelSpacing(sourceProduct);
final double spacing = Math.min(groundRangeSpacing, azimuthPixelSpacing);
// final double spacing = (groundRangeSpacing + azimuthPixelSpacing)/2.0;
final double latMin = latLonMinMax[0];
final double latMax = latLonMinMax[1];
double minAbsLat;
if (latMin * latMax > 0) {
minAbsLat = Math.min(Math.abs(latMin), Math.abs(latMax)) * Constants.DTOR;
} else {
minAbsLat = 0.0;
}
delLat = spacing / Constants.MeanEarthRadius * Constants.RTOD;
delLon = spacing / (Constants.MeanEarthRadius * FastMath.cos(minAbsLat)) * Constants.RTOD;
delLat = Math.min(delLat, delLon); // (delLat + delLon)/2.0;
delLon = delLat;
}
/**
* Get second derivative truncated to first components.
*
* @param degree degree of polynomial secular part
* @param harmonics number of harmonics terms to consider
* @param time time parameter
* @param parameters models parameters (must include all parameters, including the ones ignored
* due to model truncation)
* @return truncated second derivative
*/
private double truncatedSecondDerivative(
final int degree, final int harmonics, final double time, final double... parameters) {
double d2 = 0;
// secular part
double tN = 1.0;
for (int i = 2; i <= degree; ++i) {
d2 += (i - 1) * i * parameters[i] * tN;
tN *= time;
}
// harmonic part
for (int i = 0; i < harmonics; ++i) {
d2 +=
-pulsations[i]
* pulsations[i]
* (parameters[secularDegree + 2 * i + 1] * FastMath.cos(pulsations[i] * time)
+ parameters[secularDegree + 2 * i + 2] * FastMath.sin(pulsations[i] * time));
}
return d2;
}
/**
* Find a location on an existing street near the given point, without actually creating any
* vertices or edges.
*
* @return a new Split object, or null if no edge was found in range.
*/
public static Split find(
double lat,
double lon,
double searchRadiusMeters,
StreetLayer streetLayer,
StreetMode streetMode) {
// After this conversion, the entire geometric calculation is happening in fixed precision int
// degrees.
int fixedLat = VertexStore.floatingDegreesToFixed(lat);
int fixedLon = VertexStore.floatingDegreesToFixed(lon);
// We won't worry about the perpendicular walks yet.
// Just insert or find a vertex on the nearest road and return that vertex.
final double metersPerDegreeLat = 111111.111;
double cosLat =
FastMath.cos(FastMath.toRadians(lat)); // The projection factor, Earth is a "sphere"
// Use longs for radii and their square because squaring the fixed-point radius _will_ overflow
// a signed int32.
long radiusFixedLat =
VertexStore.floatingDegreesToFixed(searchRadiusMeters / metersPerDegreeLat);
long radiusFixedLon =
(int) (radiusFixedLat / cosLat); // Expand the X search space, don't shrink it.
Envelope envelope = new Envelope(fixedLon, fixedLon, fixedLat, fixedLat);
envelope.expandBy(radiusFixedLon, radiusFixedLat);
long squaredRadiusFixedLat = radiusFixedLat * radiusFixedLat;
EdgeStore.Edge edge = streetLayer.edgeStore.getCursor();
// Iterate over the set of forward (even) edges that may be near the given coordinate.
TIntCollection candidateEdges = streetLayer.findEdgesInEnvelope(envelope);
// The split location currently being examined and the best one seen so far.
Split curr = new Split();
Split best = new Split();
candidateEdges.forEach(
e -> {
curr.edge = e;
edge.seek(e);
// Skip Link edges those are links between transit stops/P+R/Bike share vertices and graph
// Without this origin or destination point can link to those edges because they have ALL
// permissions
// and route is never found since point is inaccessible because edges leading to it don't
// have required permission
if (edge.getFlag(EdgeStore.EdgeFlag.LINK)) return true;
// If an edge does not allow traversal with the specified mode, skip over it.
if (streetMode == StreetMode.WALK && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_PEDESTRIAN))
return true;
if (streetMode == StreetMode.BICYCLE && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_BIKE))
return true;
if (streetMode == StreetMode.CAR && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_CAR))
return true;
// The distance to this edge is the distance to the closest segment of its geometry.
edge.forEachSegment(
(seg, fixedLat0, fixedLon0, fixedLat1, fixedLon1) -> {
// Find the fraction along the current segment
curr.seg = seg;
curr.frac =
GeometryUtils.segmentFraction(
fixedLon0, fixedLat0, fixedLon1, fixedLat1, fixedLon, fixedLat, cosLat);
// Project to get the closest point on the segment.
// Note: the fraction is scaleless, xScale is accounted for in the segmentFraction
// function.
curr.fixedLon = (int) (fixedLon0 + curr.frac * (fixedLon1 - fixedLon0));
curr.fixedLat = (int) (fixedLat0 + curr.frac * (fixedLat1 - fixedLat0));
// Find squared distance to edge (avoid taking root)
long dx = (long) ((curr.fixedLon - fixedLon) * cosLat);
long dy = (long) (curr.fixedLat - fixedLat);
curr.distSquared = dx * dx + dy * dy;
// Ignore segments that are too far away (filter false positives).
if (curr.distSquared < squaredRadiusFixedLat) {
if (curr.distSquared < best.distSquared) {
// Update the best segment if we've found something closer.
best.setFrom(curr);
} else if (curr.distSquared == best.distSquared && curr.edge < best.edge) {
// Break distance ties by favoring lower edge IDs. This makes destination
// linking
// deterministic where centroids are equidistant to edges (see issue #159).
best.setFrom(curr);
}
}
});
// The loop over the edges should continue.
return true;
});
if (best.edge < 0) {
// No edge found nearby.
return null;
}
// We found an edge. Iterate over its segments again, accumulating distances along its geometry.
// The distance calculations involve square roots so are deferred to happen here, only on the
// selected edge.
// TODO accumulate before/after geoms. Split point can be passed over since it's not an
// intermediate.
// The length is are stored in one-element array to dodge Java's "effectively final" BS.
edge.seek(best.edge);
best.vertex0 = edge.getFromVertex();
best.vertex1 = edge.getToVertex();
double[] lengthBefore_fixedDeg = new double[1];
edge.forEachSegment(
(seg, fLat0, fLon0, fLat1, fLon1) -> {
// Sum lengths only up to the split point.
// lengthAfter should be total length minus lengthBefore, which ensures splits do not
// change total lengths.
if (seg <= best.seg) {
double dx = (fLon1 - fLon0) * cosLat;
double dy = (fLat1 - fLat0);
double length = FastMath.sqrt(dx * dx + dy * dy);
if (seg == best.seg) {
length *= best.frac;
}
lengthBefore_fixedDeg[0] += length;
}
});
// Convert the fixed-precision degree measurements into (milli)meters
double lengthBefore_floatDeg =
VertexStore.fixedDegreesToFloating((int) lengthBefore_fixedDeg[0]);
best.distance0_mm = (int) (lengthBefore_floatDeg * metersPerDegreeLat * 1000);
// FIXME perhaps we should be using the sphericalDistanceLibrary here, or the other way around.
// The initial edge lengths are set using that library on OSM node coordinates, and they are
// slightly different.
// We are using a single cosLat value at the linking point, instead of a different value at each
// segment.
if (best.distance0_mm < 0) {
best.distance0_mm = 0;
LOG.error("Length of first street segment was not positive.");
}
if (best.distance0_mm > edge.getLengthMm()) {
// This mistake happens because the linear distance calculation we're using comes out longer
// than the
// spherical distance. The graph remains coherent because we force the two split edge lengths
// to add up
// to the original edge length.
LOG.debug(
"Length of first street segment was greater than the whole edge ({} > {}).",
best.distance0_mm,
edge.getLengthMm());
best.distance0_mm = edge.getLengthMm();
}
best.distance1_mm = edge.getLengthMm() - best.distance0_mm;
return best;
}
/**
* Find a split on a particular edge. FIXME this appears to be copy-pasted from another method and
* only used for park and rides. Can we reuse some code here?
*/
public static Split findOnEdge(double lat, double lon, EdgeStore.Edge edge) {
// After this conversion, the entire geometric calculation is happening in fixed precision int
// degrees.
int fixedLat = VertexStore.floatingDegreesToFixed(lat);
int fixedLon = VertexStore.floatingDegreesToFixed(lon);
// We won't worry about the perpendicular walks yet.
// Just insert or find a vertex on the nearest road and return that vertex.
final double metersPerDegreeLat = 111111.111;
double cosLat =
FastMath.cos(FastMath.toRadians(lat)); // The projection factor, Earth is a "sphere"
// TODO copy paste code
// The split location currently being examined and the best one seen so far.
Split curr = new Split();
Split best = new Split();
curr.edge = edge.edgeIndex;
best.vertex0 = edge.getFromVertex();
best.vertex1 = edge.getToVertex();
double[] lengthBefore_fixedDeg = new double[1];
edge.forEachSegment(
(seg, fixedLat0, fixedLon0, fixedLat1, fixedLon1) -> {
// Find the fraction along the current segment
curr.seg = seg;
curr.frac =
GeometryUtils.segmentFraction(
fixedLon0, fixedLat0, fixedLon1, fixedLat1, fixedLon, fixedLat, cosLat);
// Project to get the closest point on the segment.
// Note: the fraction is scaleless, xScale is accounted for in the segmentFraction
// function.
curr.fixedLon = (int) (fixedLon0 + curr.frac * (fixedLon1 - fixedLon0));
curr.fixedLat = (int) (fixedLat0 + curr.frac * (fixedLat1 - fixedLat0));
double dx = (fixedLon1 - fixedLon0) * cosLat;
double dy = (fixedLat1 - fixedLat0);
double length = FastMath.sqrt(dx * dx + dy * dy);
curr.distance0_mm =
(int) ((lengthBefore_fixedDeg[0] + length * curr.frac) * metersPerDegreeLat * 1000);
lengthBefore_fixedDeg[0] += length;
curr.distSquared = (long) (dx * dx + dy * dy);
// Replace the best segment if we've found something closer.
if (curr.distSquared < best.distSquared) {
best.setFrom(curr);
}
}); // end loop over segments
int edgeLengthMm = edge.getLengthMm();
if (best.distance0_mm > edgeLengthMm) {
// rounding errors
best.distance0_mm = edgeLengthMm;
best.distance1_mm = 0;
} else {
best.distance1_mm = edgeLengthMm - best.distance0_mm;
}
return best;
}
/**
* Computes the potential U derivatives.
*
* <p>The following elements are computed from expression 3.3 - (4).
*
* <pre>
* dU / da
* dU / dh
* dU / dk
* dU / dλ
* dU / dα
* dU / dβ
* dU / dγ
* </pre>
*
* @param date current date
* @return potential derivatives
* @throws OrekitException if an error occurs
*/
private double[] computeUDerivatives(final AbsoluteDate date) throws OrekitException {
// Potential derivatives
double dUda = 0.;
double dUdh = 0.;
double dUdk = 0.;
double dUdl = 0.;
double dUdAl = 0.;
double dUdBe = 0.;
double dUdGa = 0.;
// Compute only if there is at least one resonant tesseral
if (!resOrders.isEmpty()) {
// Gmsj and Hmsj polynomials
final GHmsjPolynomials ghMSJ = new GHmsjPolynomials(k, h, alpha, beta, I);
// GAMMAmns function
final GammaMnsFunction gammaMNS = new GammaMnsFunction(fact, gamma, I);
// R / a up to power degree
final double[] roaPow = new double[maxDegree + 1];
roaPow[0] = 1.;
for (int i = 1; i <= maxDegree; i++) {
roaPow[i] = roa * roaPow[i - 1];
}
// SUM over resonant terms {j,m}
for (int m : resOrders) {
// Resonant index for the current resonant order
final int j = FastMath.max(1, (int) FastMath.round(ratio * m));
// Phase angle
final double jlMmt = j * lm - m * theta;
final double sinPhi = FastMath.sin(jlMmt);
final double cosPhi = FastMath.cos(jlMmt);
// Potential derivatives components for a given resonant pair {j,m}
double dUdaCos = 0.;
double dUdaSin = 0.;
double dUdhCos = 0.;
double dUdhSin = 0.;
double dUdkCos = 0.;
double dUdkSin = 0.;
double dUdlCos = 0.;
double dUdlSin = 0.;
double dUdAlCos = 0.;
double dUdAlSin = 0.;
double dUdBeCos = 0.;
double dUdBeSin = 0.;
double dUdGaCos = 0.;
double dUdGaSin = 0.;
// s-SUM from -sMin to sMax
final int sMin = FastMath.min(maxEccPow - j, maxDegree);
final int sMax = FastMath.min(maxEccPow + j, maxDegree);
for (int s = 0; s <= sMax; s++) {
// Compute the initial values for Hansen coefficients using newComb operators
this.hansenObjects[s + maxDegree][j].computeInitValues(e2, chi, chi2);
// n-SUM for s positive
final double[][] nSumSpos =
computeNSum(date, j, m, s, maxDegree, roaPow, ghMSJ, gammaMNS);
dUdaCos += nSumSpos[0][0];
dUdaSin += nSumSpos[0][1];
dUdhCos += nSumSpos[1][0];
dUdhSin += nSumSpos[1][1];
dUdkCos += nSumSpos[2][0];
dUdkSin += nSumSpos[2][1];
dUdlCos += nSumSpos[3][0];
dUdlSin += nSumSpos[3][1];
dUdAlCos += nSumSpos[4][0];
dUdAlSin += nSumSpos[4][1];
dUdBeCos += nSumSpos[5][0];
dUdBeSin += nSumSpos[5][1];
dUdGaCos += nSumSpos[6][0];
dUdGaSin += nSumSpos[6][1];
// n-SUM for s negative
if (s > 0 && s <= sMin) {
// Compute the initial values for Hansen coefficients using newComb operators
this.hansenObjects[maxDegree - s][j].computeInitValues(e2, chi, chi2);
final double[][] nSumSneg =
computeNSum(date, j, m, -s, maxDegree, roaPow, ghMSJ, gammaMNS);
dUdaCos += nSumSneg[0][0];
dUdaSin += nSumSneg[0][1];
dUdhCos += nSumSneg[1][0];
dUdhSin += nSumSneg[1][1];
dUdkCos += nSumSneg[2][0];
dUdkSin += nSumSneg[2][1];
dUdlCos += nSumSneg[3][0];
dUdlSin += nSumSneg[3][1];
dUdAlCos += nSumSneg[4][0];
dUdAlSin += nSumSneg[4][1];
dUdBeCos += nSumSneg[5][0];
dUdBeSin += nSumSneg[5][1];
dUdGaCos += nSumSneg[6][0];
dUdGaSin += nSumSneg[6][1];
}
}
// Assembly of potential derivatives componants
dUda += cosPhi * dUdaCos + sinPhi * dUdaSin;
dUdh += cosPhi * dUdhCos + sinPhi * dUdhSin;
dUdk += cosPhi * dUdkCos + sinPhi * dUdkSin;
dUdl += cosPhi * dUdlCos + sinPhi * dUdlSin;
dUdAl += cosPhi * dUdAlCos + sinPhi * dUdAlSin;
dUdBe += cosPhi * dUdBeCos + sinPhi * dUdBeSin;
dUdGa += cosPhi * dUdGaCos + sinPhi * dUdGaSin;
}
dUda *= -moa / a;
dUdh *= moa;
dUdk *= moa;
dUdl *= moa;
dUdAl *= moa;
dUdBe *= moa;
dUdGa *= moa;
}
return new double[] {dUda, dUdh, dUdk, dUdl, dUdAl, dUdBe, dUdGa};
}
/** {@inheritDoc} */
@Override
public double[] getShortPeriodicVariations(final AbsoluteDate date, final double[] meanElements)
throws OrekitException {
// Initialise the short periodic variations
final double[] shortPeriodicVariation = new double[] {0., 0., 0., 0., 0., 0.};
// Compute only if there is at least one non-resonant tesseral or
// only the m-daily tesseral should be taken into account
if (!nonResOrders.isEmpty() || mDailiesOnly) {
// Build an Orbit object from the mean elements
final Orbit meanOrbit =
OrbitType.EQUINOCTIAL.mapArrayToOrbit(
meanElements, PositionAngle.MEAN, date, provider.getMu(), this.frame);
// Build an auxiliary object
final AuxiliaryElements aux = new AuxiliaryElements(meanOrbit, I);
// Central body rotation angle from equation 2.7.1-(3)(4).
final Transform t = bodyFrame.getTransformTo(aux.getFrame(), aux.getDate());
final Vector3D xB = t.transformVector(Vector3D.PLUS_I);
final Vector3D yB = t.transformVector(Vector3D.PLUS_J);
final double currentTheta =
FastMath.atan2(
-f.dotProduct(yB) + I * g.dotProduct(xB), f.dotProduct(xB) + I * g.dotProduct(yB));
// Add the m-daily contribution
for (int m = 1; m <= maxOrderMdailyTesseralSP; m++) {
// Phase angle
final double jlMmt = -m * currentTheta;
final double sinPhi = FastMath.sin(jlMmt);
final double cosPhi = FastMath.cos(jlMmt);
// compute contribution for each element
for (int i = 0; i < 6; i++) {
shortPeriodicVariation[i] +=
tesseralSPCoefs.getCijm(i, 0, m, date) * cosPhi
+ tesseralSPCoefs.getSijm(i, 0, m, date) * sinPhi;
}
}
// loop through all non-resonant (j,m) pairs
for (final Map.Entry<Integer, List<Integer>> entry : nonResOrders.entrySet()) {
final int m = entry.getKey();
final List<Integer> listJ = entry.getValue();
for (int j : listJ) {
// Phase angle
final double jlMmt = j * meanElements[5] - m * currentTheta;
final double sinPhi = FastMath.sin(jlMmt);
final double cosPhi = FastMath.cos(jlMmt);
// compute contribution for each element
for (int i = 0; i < 6; i++) {
shortPeriodicVariation[i] +=
tesseralSPCoefs.getCijm(i, j, m, date) * cosPhi
+ tesseralSPCoefs.getSijm(i, j, m, date) * sinPhi;
}
}
}
}
return shortPeriodicVariation;
}
/**
* @param xTimesOmegaPlusPhase {@code omega * x + phase}.
* @param amplitude Amplitude.
* @return the value of the harmonic oscillator function at {@code x}.
*/
private static double value(double xTimesOmegaPlusPhase, double amplitude) {
return amplitude * FastMath.cos(xTimesOmegaPlusPhase);
}
