List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getNormSq
public double getNormSq()
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>//from ww w. ja va2s .co m * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianElliptical() { 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 Vector3D momentum = pvc.getMomentum(); final double v2 = velocity.getNormSq(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double px = position.getX(); final double py = position.getY(); final double pz = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double sqrtMuA = FastMath.sqrt(a * mu); final double sqrtAoMu = FastMath.sqrt(a / mu); final double a2 = a * a; final double twoA = 2 * a; final double rOnA = r / a; final double oMe2 = 1 - e * e; final double epsilon = FastMath.sqrt(oMe2); final double sqrtRec = 1 / epsilon; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosPA = FastMath.cos(pa); final double sinPA = FastMath.sin(pa); final double pv = Vector3D.dotProduct(position, velocity); final double cosE = (a - r) / (a * e); final double sinE = pv / (e * sqrtMuA); // da final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu); fillHalfRow(1, vectorAR, jacobian[0], 0); fillHalfRow(1, vectorARDot, jacobian[0], 3); // de final double factorER3 = pv / twoA; final Vector3D vectorER = new Vector3D(cosE * v2 / (r * mu), position, sinE / sqrtMuA, velocity, -factorER3 * sinE / sqrtMuA, vectorAR); final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position, cosE * 2 * r / mu, velocity, -factorER3 * sinE / sqrtMuA, vectorARDot); fillHalfRow(1, vectorER, jacobian[1], 0); fillHalfRow(1, vectorERDot, jacobian[1], 3); // dE / dr (Eccentric anomaly) final double coefE = cosE / (e * sqrtMuA); final Vector3D vectorEAnR = new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity, -factorER3 * coefE, vectorAR); // dE / drDot final Vector3D vectorEAnRDot = new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position, -factorER3 * coefE, vectorARDot); // precomputing some more factors final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu; final double s2 = -cosE * pz / r3; final double s3 = -sinE * vz / (2 * sqrtMuA); final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu); final double t2 = sqrtRec * (-sinE * pz / r3); final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA); final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu); final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K, s1, vectorEAnR, s2, position, s3, vectorAR); final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K, s1, vectorEAnRDot, s3, vectorARDot); final Vector3D t = new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K) .add(new Vector3D(t1, vectorEAnR, t2, position, t3, vectorAR, t4, vectorER)); final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K, t1, vectorEAnRDot, t3, vectorARDot, t4, vectorERDot); // di final double factorI1 = -sinI * sqrtRec / sqrtMuA; final double i1 = factorI1; final double i2 = -factorI1 * mz / twoA; final double i3 = factorI1 * mz * e / oMe2; final double i4 = cosI * sinPA; final double i5 = cosI * cosPA; fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t, jacobian[2], 0); fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot, jacobian[2], 3); // dpa fillHalfRow(cosPA / sinI, s, -sinPA / sinI, t, jacobian[3], 0); fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3); // dRaan final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI); fillHalfRow(-factorRaanR * my, new Vector3D(0, vz, -vy), factorRaanR * mx, new Vector3D(-vz, 0, vx), jacobian[4], 0); fillHalfRow(-factorRaanR * my, new Vector3D(0, -pz, py), factorRaanR * mx, new Vector3D(pz, 0, -px), jacobian[4], 3); // dM fillHalfRow(rOnA, vectorEAnR, -sinE, vectorER, jacobian[5], 0); fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>//from w ww . ja v a 2s. c om * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianHyperbolic() { 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 Vector3D momentum = pvc.getMomentum(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; 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 mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double absA = -a; final double sqrtMuA = FastMath.sqrt(absA * mu); final double a2 = a * a; final double rOa = r / absA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double pv = Vector3D.dotProduct(position, velocity); // da final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu); fillHalfRow(-1, vectorAR, jacobian[0], 0); fillHalfRow(-1, vectorARDot, jacobian[0], 3); // differentials of the momentum final double m = momentum.getNorm(); final double oOm = 1 / m; final Vector3D dcXP = new Vector3D(0, vz, -vy); final Vector3D dcYP = new Vector3D(-vz, 0, vx); final Vector3D dcZP = new Vector3D(vy, -vx, 0); final Vector3D dcXV = new Vector3D(0, -z, y); final Vector3D dcYV = new Vector3D(z, 0, -x); final Vector3D dcZV = new Vector3D(-y, x, 0); final Vector3D dCP = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP); final Vector3D dCV = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV); // dp final double mOMu = m / mu; final Vector3D dpP = new Vector3D(2 * mOMu, dCP); final Vector3D dpV = new Vector3D(2 * mOMu, dCV); // de final double p = m * mOMu; final double moO2ae = 1 / (2 * absA * e); final double m2OaMu = -p / absA; fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR, jacobian[1], 0); fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3); // di final double cI1 = 1 / (m * sinI); final double cI2 = cosI * cI1; fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0); fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3); // dPA final double cP1 = y * oOm; final double cP2 = -x * oOm; final double cP3 = -(mx * cP1 + my * cP2); final double cP4 = cP3 * oOm; final double cP5 = -1 / (r2 * sinI * sinI); final double cP6 = z * cP5; final double cP7 = cP3 * cP5; final Vector3D dacP = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0)); final Vector3D dacV = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV); final Vector3D dpoP = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K); final Vector3D dpoV = new Vector3D(cP6, dacV); final double re2 = r2 * e * e; final double recOre2 = (p - r) / re2; final double resOre2 = (pv * mOMu) / re2; final Vector3D dreP = new Vector3D(mOMu, velocity, pv / mu, dCP); final Vector3D dreV = new Vector3D(mOMu, position, pv / mu, dCV); final Vector3D davP = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position); final Vector3D davV = new Vector3D(-resOre2, dpV, recOre2, dreV); fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0); fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3); // dRAAN final double cO0 = cI1 * cI1; final double cO1 = mx * cO0; final double cO2 = -my * cO0; fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0); fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3); // dM final double s2a = pv / (2 * absA); final double oObux = 1 / FastMath.sqrt(m * m + mu * absA); final double scasbu = pv * oObux; final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR); final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot); final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR, m * oObux, dCP); final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV); final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP); final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV); fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0); fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianParametersTest.java
private void fillColumn(PositionAngle type, int i, KeplerianOrbit orbit, double hP, double[][] jacobian) { // at constant energy (i.e. constant semi major axis), we have dV = -mu dP / (V * r^2) // we use this to compute a velocity step size from the position step size Vector3D p = orbit.getPVCoordinates().getPosition(); Vector3D v = orbit.getPVCoordinates().getVelocity(); double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq()); double h;//from w w w . j a v a 2 s. c om Vector3D dP = Vector3D.ZERO; Vector3D dV = Vector3D.ZERO; switch (i) { case 0: h = hP; dP = new Vector3D(hP, 0, 0); break; case 1: h = hP; dP = new Vector3D(0, hP, 0); break; case 2: h = hP; dP = new Vector3D(0, 0, hP); break; case 3: h = hV; dV = new Vector3D(hV, 0, 0); break; case 4: h = hV; dV = new Vector3D(0, hV, 0); break; default: h = hV; dV = new Vector3D(0, 0, hV); break; } KeplerianOrbit oM4h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oM3h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oM2h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oM1h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oP1h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oP2h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oP3h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); KeplerianOrbit oP4h = new KeplerianOrbit( new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) + 32 * (oP3h.getA() - oM3h.getA()) - 168 * (oP2h.getA() - oM2h.getA()) + 672 * (oP1h.getA() - oM1h.getA())) / (840 * h); jacobian[1][i] = (-3 * (oP4h.getE() - oM4h.getE()) + 32 * (oP3h.getE() - oM3h.getE()) - 168 * (oP2h.getE() - oM2h.getE()) + 672 * (oP1h.getE() - oM1h.getE())) / (840 * h); jacobian[2][i] = (-3 * (oP4h.getI() - oM4h.getI()) + 32 * (oP3h.getI() - oM3h.getI()) - 168 * (oP2h.getI() - oM2h.getI()) + 672 * (oP1h.getI() - oM1h.getI())) / (840 * h); jacobian[3][i] = (-3 * (oP4h.getPerigeeArgument() - oM4h.getPerigeeArgument()) + 32 * (oP3h.getPerigeeArgument() - oM3h.getPerigeeArgument()) - 168 * (oP2h.getPerigeeArgument() - oM2h.getPerigeeArgument()) + 672 * (oP1h.getPerigeeArgument() - oM1h.getPerigeeArgument())) / (840 * h); jacobian[4][i] = (-3 * (oP4h.getRightAscensionOfAscendingNode() - oM4h.getRightAscensionOfAscendingNode()) + 32 * (oP3h.getRightAscensionOfAscendingNode() - oM3h.getRightAscensionOfAscendingNode()) - 168 * (oP2h.getRightAscensionOfAscendingNode() - oM2h.getRightAscensionOfAscendingNode()) + 672 * (oP1h.getRightAscensionOfAscendingNode() - oM1h.getRightAscensionOfAscendingNode())) / (840 * h); jacobian[5][i] = (-3 * (oP4h.getAnomaly(type) - oM4h.getAnomaly(type)) + 32 * (oP3h.getAnomaly(type) - oM3h.getAnomaly(type)) - 168 * (oP2h.getAnomaly(type) - oM2h.getAnomaly(type)) + 672 * (oP1h.getAnomaly(type) - oM1h.getAnomaly(type))) / (840 * h); }
From source file:org.orekit.orbits.KeplerianParametersTest.java
@Test public void testKeplerianDerivatives() { final KeplerianOrbit o = new KeplerianOrbit(new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.), new Vector3D(-2079., 5291., -7842.)), FramesFactory.getEME2000(), date, 3.9860047e14); final Vector3D p = o.getPVCoordinates().getPosition(); final Vector3D v = o.getPVCoordinates().getVelocity(); final Vector3D a = o.getPVCoordinates().getAcceleration(); // check that despite we did not provide acceleration, it got recomputed Assert.assertEquals(7.605422, a.getNorm(), 1.0e-6); FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1); // check velocity is the derivative of position double vx = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getPosition().getX(); }/* ww w.jav a2 s . c o m*/ }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getVelocity().getX(), vx, 3.0e-12 * v.getNorm()); double vy = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getPosition().getY(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getVelocity().getY(), vy, 3.0e-12 * v.getNorm()); double vz = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getPosition().getZ(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getVelocity().getZ(), vz, 3.0e-12 * v.getNorm()); // check acceleration is the derivative of velocity double ax = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getVelocity().getX(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getAcceleration().getX(), ax, 3.0e-12 * a.getNorm()); double ay = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getVelocity().getY(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getAcceleration().getY(), ay, 3.0e-12 * a.getNorm()); double az = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getVelocity().getZ(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(o.getPVCoordinates().getAcceleration().getZ(), az, 3.0e-12 * a.getNorm()); // check jerk is the derivative of acceleration final double r2 = p.getNormSq(); final double r = FastMath.sqrt(r2); Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v); double jx = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getAcceleration().getX(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(keplerianJerk.getX(), jx, 3.0e-12 * keplerianJerk.getNorm()); double jy = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getAcceleration().getY(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(keplerianJerk.getY(), jy, 3.0e-12 * keplerianJerk.getNorm()); double jz = differentiator.differentiate(new UnivariateFunction() { public double value(double dt) { return o.shiftedBy(dt).getPVCoordinates().getAcceleration().getZ(); } }).value(new DerivativeStructure(1, 1, 0, 0.0)).getPartialDerivative(1); Assert.assertEquals(keplerianJerk.getZ(), jz, 3.0e-12 * keplerianJerk.getNorm()); }
From source file:org.orekit.propagation.analytical.TabulatedEphemerisTest.java
@Test public void testInterpolationKeplerianAcceleration() throws OrekitException { // with Keplerian-only acceleration, interpolation is quite wrong checkInterpolation(new StateFilter() { public SpacecraftState filter(SpacecraftState state) { CartesianOrbit c = (CartesianOrbit) state.getOrbit(); Vector3D p = c.getPVCoordinates().getPosition(); Vector3D v = c.getPVCoordinates().getVelocity(); double r2 = p.getNormSq(); double r = FastMath.sqrt(r2); Vector3D kepA = new Vector3D(-c.getMu() / (r * r2), c.getPVCoordinates().getPosition()); return new SpacecraftState(new CartesianOrbit(new TimeStampedPVCoordinates(c.getDate(), p, v, kepA), c.getFrame(), c.getMu()), state.getAttitude(), state.getMass(), state.getAdditionalStates()); }/*from www. ja v a 2 s. c om*/ }, 8.5, 0.22); }
From source file:org.orekit.propagation.numerical.Jacobianizer.java
/** Compute acceleration and derivatives with respect to state. * @param date current date//from w ww . j a va 2s . c om * @param frame inertial reference frame for state (both orbit and attitude) * @param position position of spacecraft in reference frame * @param velocity velocity of spacecraft in reference frame * @param rotation orientation (attitude) of the spacecraft with respect to reference frame * @param mass spacecraft mass * @return acceleration with derivatives * @exception OrekitException if the underlying force models cannot compute the acceleration */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass) throws OrekitException { final int parameters = mass.getFreeParameters(); final int order = mass.getOrder(); // estimate the scalar velocity step, assuming energy conservation // and hence differentiating equation V = sqrt(mu (2/r - 1/a)) final Vector3D p0 = position.toVector3D(); final Vector3D v0 = velocity.toVector3D(); final double r2 = p0.getNormSq(); final double hVel = mu * hPos / (v0.getNorm() * r2); // estimate mass step, applying the same relative value as position final double hMass = mass.getValue() * hPos / FastMath.sqrt(r2); // compute nominal acceleration final AccelerationRetriever nominal = new AccelerationRetriever(); computeShiftedAcceleration(nominal, date, frame, p0, v0, rotation.toRotation(), mass.getValue()); final double[] a0 = nominal.getAcceleration().toArray(); // compute accelerations with shifted states final AccelerationRetriever shifted = new AccelerationRetriever(); // shift position by hPos alon x, y and z computeShiftedAcceleration(shifted, date, frame, shift(position, 0, hPos), v0, shift(rotation, 0, hPos), shift(mass, 0, hPos)); final double[] derPx = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, shift(position, 1, hPos), v0, shift(rotation, 1, hPos), shift(mass, 1, hPos)); final double[] derPy = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, shift(position, 2, hPos), v0, shift(rotation, 2, hPos), shift(mass, 2, hPos)); final double[] derPz = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray(); // shift velocity by hVel alon x, y and z computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 3, hVel), shift(rotation, 3, hVel), shift(mass, 3, hVel)); final double[] derVx = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 4, hVel), shift(rotation, 4, hVel), shift(mass, 4, hVel)); final double[] derVy = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 5, hVel), shift(rotation, 5, hVel), shift(mass, 5, hVel)); final double[] derVz = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray(); final double[] derM; if (parameters < 7) { derM = null; } else { // shift mass by hMass computeShiftedAcceleration(shifted, date, frame, p0, v0, shift(rotation, 6, hMass), shift(mass, 6, hMass)); derM = new Vector3D(1 / hMass, shifted.getAcceleration(), -1 / hMass, nominal.getAcceleration()) .toArray(); } final double[] derivatives = new double[1 + parameters]; final DerivativeStructure[] accDer = new DerivativeStructure[3]; for (int i = 0; i < 3; ++i) { // first element is value of acceleration derivatives[0] = a0[i]; // next three elements are derivatives with respect to position derivatives[1] = derPx[i]; derivatives[2] = derPy[i]; derivatives[3] = derPz[i]; // next three elements are derivatives with respect to velocity derivatives[4] = derVx[i]; derivatives[5] = derVy[i]; derivatives[6] = derVz[i]; if (derM != null) { derivatives[7] = derM[i]; } accDer[i] = new DerivativeStructure(parameters, order, derivatives); } return new FieldVector3D<DerivativeStructure>(accDer); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Find a vector from two known cross products. * <p>/* w ww .j ava2 s . c om*/ * We want to find such that: v? = c? and v = c * </p> * <p> * The first equation ( v? = c?) will always be fulfilled exactly, * and the second one will be fulfilled if possible. * </p> * @param v1 vector forming the first known cross product * @param c1 know vector for cross product v? * @param v2 vector forming the second known cross product * @param c2 know vector for cross product v * @param tolerance relative tolerance factor used to check singularities * @return vector such that: v? = c? and v = c * @exception MathIllegalArgumentException if vectors are inconsistent and * no solution can be found */ private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1, final Vector3D v2, final Vector3D c2, final double tolerance) throws MathIllegalArgumentException { final double v12 = v1.getNormSq(); final double v1n = FastMath.sqrt(v12); final double v22 = v2.getNormSq(); final double v2n = FastMath.sqrt(v22); final double threshold = tolerance * FastMath.max(v1n, v2n); Vector3D omega; try { // create the over-determined linear system representing the two cross products final RealMatrix m = MatrixUtils.createRealMatrix(6, 3); m.setEntry(0, 1, v1.getZ()); m.setEntry(0, 2, -v1.getY()); m.setEntry(1, 0, -v1.getZ()); m.setEntry(1, 2, v1.getX()); m.setEntry(2, 0, v1.getY()); m.setEntry(2, 1, -v1.getX()); m.setEntry(3, 1, v2.getZ()); m.setEntry(3, 2, -v2.getY()); m.setEntry(4, 0, -v2.getZ()); m.setEntry(4, 2, v2.getX()); m.setEntry(5, 0, v2.getY()); m.setEntry(5, 1, -v2.getX()); final RealVector rhs = MatrixUtils.createRealVector( new double[] { c1.getX(), c1.getY(), c1.getZ(), c2.getX(), c2.getY(), c2.getZ() }); // find the best solution we can final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver(); final RealVector v = solver.solve(rhs); omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2)); } catch (SingularMatrixException sme) { // handle some special cases for which we can compute a solution final double c12 = c1.getNormSq(); final double c1n = FastMath.sqrt(c12); final double c22 = c2.getNormSq(); final double c2n = FastMath.sqrt(c22); if (c1n <= threshold && c2n <= threshold) { // simple special case, velocities are cancelled return Vector3D.ZERO; } else if (v1n <= threshold && c1n >= threshold) { // this is inconsistent, if v? is zero, c? must be 0 too throw new NumberIsTooLargeException(c1n, 0, true); } else if (v2n <= threshold && c2n >= threshold) { // this is inconsistent, if v is zero, c must be 0 too throw new NumberIsTooLargeException(c2n, 0, true); } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) { // simple special case, v is redundant with v?, we just ignore it // use the simplest : orthogonal to both v? and c? omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1)); } else { throw sme; } } // check results final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1); if (d1 > threshold) { throw new NumberIsTooLargeException(d1, 0, true); } final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2); if (d2 > threshold) { throw new NumberIsTooLargeException(d2, 0, true); } return omega; }
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test public void testGetAngularVelocity() { //setup// ww w . j a v a 2s . com Vector3D p = new Vector3D(1, -2, 3); Vector3D v = new Vector3D(-9, 8, -7); //action + verify Assert.assertEquals(new PVCoordinates(p, v).getAngularVelocity(), p.crossProduct(v).scalarMultiply(1.0 / p.getNormSq())); //check extra simple cases Assert.assertEquals(new PVCoordinates(Vector3D.PLUS_I, Vector3D.MINUS_I).getAngularVelocity(), Vector3D.ZERO); Assert.assertEquals(new PVCoordinates(new Vector3D(2, 0, 0), Vector3D.PLUS_J).getAngularVelocity(), Vector3D.PLUS_K.scalarMultiply(0.5)); }
From source file:org.rhwlab.dispim.nucleus.NamedNucleusFile.java
License:asdf
static public RealMatrix rotationMatrix(Vector3D A, Vector3D B) { Vector3D a = A.normalize();/* w ww. j ava 2s . c o m*/ Vector3D b = B.normalize(); Vector3D v = a.crossProduct(b); double s = v.getNormSq(); double c = a.dotProduct(b); RealMatrix vx = MatrixUtils.createRealMatrix(3, 3); vx.setEntry(1, 0, v.getZ()); vx.setEntry(0, 1, -v.getZ()); vx.setEntry(2, 0, -v.getY()); vx.setEntry(0, 2, v.getY()); vx.setEntry(2, 1, v.getX()); vx.setEntry(1, 2, -v.getX()); RealMatrix vx2 = vx.multiply(vx); RealMatrix scaled = vx2.scalarMultiply((1.0 - c) / s); RealMatrix ident = MatrixUtils.createRealIdentityMatrix(3); RealMatrix sum = vx.add(scaled); RealMatrix ret = ident.add(sum); return ret; }