List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D Vector3D
public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2)
From source file:org.orekit.models.earth.tessellation.InsideFinder.java
/** {@inheritDoc} */ @Override// w ww .j a va2s . c o m public void visitLeafNode(final BSPTree<Sphere2D> node) { // we have already found a good point if (insidePointFirstChoice != null) { return; } if ((Boolean) node.getAttribute()) { // transform this inside leaf cell into a simple convex polygon final SphericalPolygonsSet convex = new SphericalPolygonsSet( node.pruneAroundConvexCell(Boolean.TRUE, Boolean.FALSE, null), zone.getTolerance()); // extract the start of the single loop boundary of the convex cell final List<Vertex> boundary = convex.getBoundaryLoops(); final Vertex start = boundary.get(0); int n = 0; Vector3D sumB = Vector3D.ZERO; for (Edge e = start.getOutgoing(); n == 0 || e.getStart() != start; e = e.getEnd().getOutgoing()) { sumB = new Vector3D(1, sumB, e.getLength(), e.getCircle().getPole()); n++; } final S2Point candidate = new S2Point(sumB); // check the candidate point is really considered inside // it may appear outside if the current leaf cell is very thin // and checkPoint selects another (very close) tree leaf node if (zone.checkPoint(candidate) == Location.INSIDE) { insidePointFirstChoice = candidate; } else { insidePointSecondChoice = candidate; } } }
From source file:org.orekit.models.earth.tessellation.Tile.java
/** Interpolate a vector along a unit sphere. * @param p0 first base unit vector//w w w . jav a2 s . co m * @param p1 second base unit vector * @param r interpolation parameter (0 for p0, 1 for p1) * @return interpolated unit vector */ private Vector3D interpolate(final Vector3D p0, final Vector3D p1, final double r) { // find all interpolation angles final double theta = Vector3D.angle(p0, p1); final double alpha = r * theta; final double thetaMAlpha = (1 - r) * theta; final double sinTheta = FastMath.sin(theta); final double sinAlpha = FastMath.sin(alpha); final double sinThetaMAlpha = FastMath.sin(thetaMAlpha); // interpolate return new Vector3D(sinThetaMAlpha / sinTheta, p0, sinAlpha / sinTheta, p1); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Compute shifted position and velocity in elliptic case. * @param dt time shift/*from w w w . j av a2 s .c om*/ * @return shifted position and velocity */ private PVCoordinates shiftPVElliptic(final double dt) { // preliminary computation final Vector3D pvP = getPVCoordinates().getPosition(); final Vector3D pvV = getPVCoordinates().getVelocity(); final double r = pvP.getNorm(); final double rV2OnMu = r * pvV.getNormSq() / getMu(); final double a = getA(); final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; // we can use any arbitrary reference 2D frame in the orbital plane // in order to simplify some equations below, we use the current position as the u axis final Vector3D u = pvP.normalize(); final Vector3D v = Vector3D.crossProduct(getPVCoordinates().getMomentum(), u).normalize(); // the following equations rely on the specific choice of u explained above, // some coefficients that vanish to 0 in this case have already been removed here final double ex = (eCE - e2) * a / r; final double ey = -FastMath.sqrt(1 - e2) * eSE * a / r; final double beta = 1 / (1 + FastMath.sqrt(1 - e2)); final double thetaE0 = FastMath.atan2(ey + eSE * beta * ex, r / a + ex - eSE * beta * ey); final double thetaM0 = thetaE0 - ex * FastMath.sin(thetaE0) + ey * FastMath.cos(thetaE0); // compute in-plane shifted eccentric argument final double thetaM1 = thetaM0 + getKeplerianMeanMotion() * dt; final double thetaE1 = meanToEccentric(thetaM1, ex, ey); final double cTE = FastMath.cos(thetaE1); final double sTE = FastMath.sin(thetaE1); // compute shifted in-plane cartesian coordinates final double exey = ex * ey; final double exCeyS = ex * cTE + ey * sTE; final double x = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex); final double y = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey); final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS); final double xDot = factor * (-sTE + beta * ey * exCeyS); final double yDot = factor * (cTE - beta * ex * exCeyS); final Vector3D shiftedP = new Vector3D(x, u, y, v); final double r2 = x * x + y * y; final Vector3D shiftedV = new Vector3D(xDot, u, yDot, v); final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP); return new PVCoordinates(shiftedP, shiftedV, shiftedA); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** Compute shifted position and velocity in hyperbolic case. * @param dt time shift//from ww w . j av a 2 s . c o m * @return shifted position and velocity */ private PVCoordinates shiftPVHyperbolic(final double dt) { final PVCoordinates pv = getPVCoordinates(); final Vector3D pvP = pv.getPosition(); final Vector3D pvV = pv.getVelocity(); final Vector3D pvM = pv.getMomentum(); final double r = pvP.getNorm(); final double rV2OnMu = r * pvV.getNormSq() / getMu(); final double a = getA(); final double muA = getMu() * a; final double e = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA); final double sqrt = FastMath.sqrt((e + 1) / (e - 1)); // compute mean anomaly final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA); final double eCH = rV2OnMu - 1; final double H0 = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2; final double M0 = e * FastMath.sinh(H0) - H0; // find canonical 2D frame with p pointing to perigee final double v0 = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2)); final Vector3D p = new Rotation(pvM, -v0).applyTo(pvP).normalize(); final Vector3D q = Vector3D.crossProduct(pvM, p).normalize(); // compute shifted eccentric anomaly final double M1 = M0 + getKeplerianMeanMotion() * dt; final double H1 = meanToHyperbolicEccentric(M1, e); // compute shifted in-plane cartesian coordinates final double cH = FastMath.cosh(H1); final double sH = FastMath.sinh(H1); final double sE2m1 = FastMath.sqrt((e - 1) * (e + 1)); // coordinates of position and velocity in the orbital plane final double x = a * (cH - e); final double y = -a * sE2m1 * sH; final double factor = FastMath.sqrt(getMu() / -a) / (e * cH - 1); final double xDot = -factor * sH; final double yDot = factor * sE2m1 * cH; final Vector3D shiftedP = new Vector3D(x, p, y, q); final double r2 = x * x + y * y; final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q); final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP); return new PVCoordinates(shiftedP, shiftedV, shiftedA); }
From source file:org.orekit.orbits.CircularOrbit.java
/** {@inheritDoc} */ protected double[][] computeJacobianMeanWrtCartesian() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double pv = Vector3D.dotProduct(position, velocity); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double v2 = velocity.getNormSq(); final double mu = getMu(); final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a); final double rOa = r / a; final double aOr = a / r; final double aOr2 = a / r2; final double a2 = a * a; final double ex2 = ex * ex; final double ey2 = ey * ey; final double e2 = ex2 + ey2; final double epsilon = FastMath.sqrt(1 - e2); final double beta = 1 / (1 + epsilon); final double eCosE = 1 - rOa; final double eSinE = pv * oOsqrtMuA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); // da/*from w ww. ja va 2 s . c o m*/ fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0); fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3); // differentials of the normalized momentum final Vector3D danP = new Vector3D(v2, position, -pv, velocity); final Vector3D danV = new Vector3D(r2, velocity, -pv, position); final double recip = 1 / pvc.getMomentum().getNorm(); final double recip2 = recip * recip; final Vector3D dwXP = new Vector3D(recip, new Vector3D(0, vz, -vy), -recip2 * sinRaan * sinI, danP); final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP); final Vector3D dwZP = new Vector3D(recip, new Vector3D(vy, -vx, 0), -recip2 * cosI, danP); final Vector3D dwXV = new Vector3D(recip, new Vector3D(0, -z, y), -recip2 * sinRaan * sinI, danV); final Vector3D dwYV = new Vector3D(recip, new Vector3D(z, 0, -x), recip2 * cosRaan * sinI, danV); final Vector3D dwZV = new Vector3D(recip, new Vector3D(-y, x, 0), -recip2 * cosI, danV); // di fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0); fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3); // dRaan fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0); fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3); // orbital frame: (p, q, w) p along ascending node, w along momentum // the coordinates of the spacecraft in this frame are: (u, v, 0) final double u = x * cosRaan + y * sinRaan; final double cv = -x * sinRaan + y * cosRaan; final double v = cv * cosI + z * sinI; // du final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP, cv * sinRaan / sinI, dwYP, 1, new Vector3D(cosRaan, sinRaan, 0)); final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV, cv * sinRaan / sinI, dwYV); // dv final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP, -u * sinRaan * cosI / sinI - cosRaan * z, dwYP, cv, dwZP, 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI)); final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV, -u * sinRaan * cosI / sinI - cosRaan * z, dwYV, cv, dwZV); final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position, -2 * aOr2 * eSinE * oOsqrtMuA, velocity); final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position, 2 / mu, velocity); final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position, aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity); final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position, eSinE / (mu * epsilon), velocity); final double cof1 = aOr2 * (eCosE - e2); final double cof2 = aOr2 * epsilon * eSinE; final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP); final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV); final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP); final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV); fillHalfRow(1, dexP, jacobian[1], 0); fillHalfRow(1, dexV, jacobian[1], 3); fillHalfRow(1, deyP, jacobian[2], 0); fillHalfRow(1, deyV, jacobian[2], 3); final double cle = u / a + ex - eSinE * beta * ey; final double sle = v / a + ey + eSinE * beta * ex; final double m1 = beta * eCosE; final double m2 = 1 - m1 * eCosE; final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey); final double m4 = -sle + cle * eSinE * beta; final double m5 = cle + sle * eSinE * beta; fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position, (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity, m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP, jacobian[5], 0); fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position, (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity, m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Initialize the position/velocity coordinates, elliptic case. * @param p unit vector in the orbital plane pointing towards perigee * @param q unit vector in the orbital plane in quadrature with p * @return computed position/velocity coordinates *///from ww w .j a va 2 s. co m private TimeStampedPVCoordinates initPVCoordinatesElliptical(final Vector3D p, final Vector3D q) { // elliptic eccentric anomaly final double uME2 = (1 - e) * (1 + e); final double s1Me2 = FastMath.sqrt(uME2); final double E = getEccentricAnomaly(); final double cosE = FastMath.cos(E); final double sinE = FastMath.sin(E); // coordinates of position and velocity in the orbital plane final double x = a * (cosE - e); final double y = a * sinE * s1Me2; final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cosE); final double xDot = -sinE * factor; final double yDot = cosE * s1Me2 * factor; final Vector3D position = new Vector3D(x, p, y, q); final double r2 = x * x + y * y; final Vector3D velocity = new Vector3D(xDot, p, yDot, q); final Vector3D acceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), position); return new TimeStampedPVCoordinates(getDate(), position, velocity, acceleration); }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Initialize the position/velocity coordinates, hyperbolic case. * @param p unit vector in the orbital plane pointing towards perigee * @param q unit vector in the orbital plane in quadrature with p * @return computed position/velocity coordinates */// www .ja va2 s . c om private TimeStampedPVCoordinates initPVCoordinatesHyperbolic(final Vector3D p, final Vector3D q) { // compute position and velocity factors final double sinV = FastMath.sin(v); final double cosV = FastMath.cos(v); final double f = a * (1 - e * e); final double posFactor = f / (1 + e * cosV); final double velFactor = FastMath.sqrt(getMu() / f); final Vector3D position = new Vector3D(posFactor * cosV, p, posFactor * sinV, q); final Vector3D velocity = new Vector3D(-velFactor * sinV, p, velFactor * (e + cosV), q); final Vector3D acceleration = new Vector3D(-getMu() / (posFactor * posFactor * posFactor), position); return new TimeStampedPVCoordinates(getDate(), position, velocity, acceleration); }