List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D negate
public Vector3D negate()
From source file:lambertmrev.Lambert.java
/** Constructs and solves a Lambert problem. * * \param[in] R1 first cartesian position * \param[in] R2 second cartesian position * \param[in] tof time of flight//from ww w . j av a 2s .co m * \param[in] mu gravity parameter * \param[in] cw when 1 a retrograde orbit is assumed * \param[in] multi_revs maximum number of multirevolutions to compute */ public void lambert_problem(Vector3D r1, Vector3D r2, double tof, double mu, Boolean cw, int multi_revs) { // sanity checks if (tof <= 0) { System.out.println("ToF is negative! \n"); } if (mu <= 0) { System.out.println("mu is below zero"); } // 1 - getting lambda and T double m_c = FastMath.sqrt((r2.getX() - r1.getX()) * (r2.getX() - r1.getX()) + (r2.getY() - r1.getY()) * (r2.getY() - r1.getY()) + (r2.getZ() - r1.getZ()) * (r2.getZ() - r1.getZ())); double R1 = r1.getNorm(); double R2 = r2.getNorm(); double m_s = (m_c + R1 + R2) / 2.0; Vector3D ir1 = r1.normalize(); Vector3D ir2 = r2.normalize(); Vector3D ih = Vector3D.crossProduct(ir1, ir2); ih = ih.normalize(); if (ih.getZ() == 0) { System.out.println("angular momentum vector has no z component \n"); } double lambda2 = 1.0 - m_c / m_s; double m_lambda = FastMath.sqrt(lambda2); Vector3D it1 = new Vector3D(0.0, 0.0, 0.0); Vector3D it2 = new Vector3D(0.0, 0.0, 0.0); if (ih.getZ() < 0.0) { // Transfer angle is larger than 180 degrees as seen from abive the z axis m_lambda = -m_lambda; it1 = Vector3D.crossProduct(ir1, ih); it2 = Vector3D.crossProduct(ir2, ih); } else { it1 = Vector3D.crossProduct(ih, ir1); it2 = Vector3D.crossProduct(ih, ir2); } it1.normalize(); it2.normalize(); if (cw) { // Retrograde motion m_lambda = -m_lambda; it1.negate(); it2.negate(); } double lambda3 = m_lambda * lambda2; double T = FastMath.sqrt(2.0 * mu / m_s / m_s / m_s) * tof; // 2 - We now hava lambda, T and we will find all x // 2.1 - let us first detect the maximum number of revolutions for which there exists a solution int m_Nmax = FastMath.toIntExact(FastMath.round(T / FastMath.PI)); double T00 = FastMath.acos(m_lambda) + m_lambda * FastMath.sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * FastMath.PI); double T1 = 2.0 / 3.0 * (1.0 - lambda3); double DT = 0.0; double DDT = 0.0; double DDDT = 0.0; if (m_Nmax > 0) { if (T < T0) { // We use Halley iterations to find xM and TM int it = 0; double err = 1.0; double T_min = T0; double x_old = 0.0, x_new = 0.0; while (true) { ArrayRealVector deriv = dTdx(x_old, T_min, m_lambda); DT = deriv.getEntry(0); DDT = deriv.getEntry(1); DDDT = deriv.getEntry(2); if (DT != 0.0) { x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); } err = FastMath.abs(x_old - x_new); if ((err < 1e-13) || (it > 12)) { break; } tof = x2tof(x_new, m_Nmax, m_lambda); x_old = x_new; it++; } if (T_min > T) { m_Nmax -= 1; } } } // We exit this if clause with Mmax being the maximum number of revolutions // for which there exists a solution. We crop it to multi_revs m_Nmax = FastMath.min(multi_revs, m_Nmax); // 2.2 We now allocate the memory for the output variables m_v1 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_v2 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_iters = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); //RealMatrix m_x = MatrixUtils.createRealMatrix(m_Nmax*2+1, 3); ArrayRealVector m_x = new ArrayRealVector(m_Nmax * 2 + 1); // 3 - We may now find all solution in x,y // 3.1 0 rev solution // 3.1.1 initial guess if (T >= T00) { m_x.setEntry(0, -(T - T00) / (T - T00 + 4)); } else if (T <= T1) { m_x.setEntry(0, T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1); } else { m_x.setEntry(0, FastMath.pow((T / T00), 0.69314718055994529 / FastMath.log(T1 / T00)) - 1.0); } // 3.1.2 Householder iterations //m_iters.setEntry(0, 0, housOutTmp.getEntry(0)); m_x.setEntry(0, householder(T, m_x.getEntry(0), 0, 1e-5, 15, m_lambda)); // 3.2 multi rev solutions double tmp; double x0; for (int i = 1; i < m_Nmax + 1; i++) { // 3.2.1 left householder iterations tmp = FastMath.pow((i * FastMath.PI + FastMath.PI) / (8.0 * T), 2.0 / 3.0); m_x.setEntry(2 * i - 1, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i - 1), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i - 1, x0); //m_iters.setEntry(2*i-1, 0, housOutTmp.getEntry(0)); //3.2.1 right Householder iterations tmp = FastMath.pow((8.0 * T) / (i * FastMath.PI), 2.0 / 3.0); m_x.setEntry(2 * i, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i, x0); //m_iters.setEntry(2*i, 0, housOutTmp.getEntry(0)); } // 4 - For each found x value we recontruct the terminal velocities double gamma = FastMath.sqrt(mu * m_s / 2.0); double rho = (R1 - R2) / m_c; double sigma = FastMath.sqrt(1 - rho * rho); double vr1, vt1, vr2, vt2, y; ArrayRealVector ir1_vec = new ArrayRealVector(3); ArrayRealVector ir2_vec = new ArrayRealVector(3); ArrayRealVector it1_vec = new ArrayRealVector(3); ArrayRealVector it2_vec = new ArrayRealVector(3); // set Vector3D values to a mutable type ir1_vec.setEntry(0, ir1.getX()); ir1_vec.setEntry(1, ir1.getY()); ir1_vec.setEntry(2, ir1.getZ()); ir2_vec.setEntry(0, ir2.getX()); ir2_vec.setEntry(1, ir2.getY()); ir2_vec.setEntry(2, ir2.getZ()); it1_vec.setEntry(0, it1.getX()); it1_vec.setEntry(1, it1.getY()); it1_vec.setEntry(2, it1.getZ()); it2_vec.setEntry(0, it2.getX()); it2_vec.setEntry(1, it2.getY()); it2_vec.setEntry(2, it2.getZ()); for (int i = 0; i < m_x.getDimension(); i++) { y = FastMath.sqrt(1.0 - lambda2 + lambda2 * m_x.getEntry(i) * m_x.getEntry(i)); vr1 = gamma * ((m_lambda * y - m_x.getEntry(i)) - rho * (m_lambda * y + m_x.getEntry(i))) / R1; vr2 = -gamma * ((m_lambda * y - m_x.getEntry(i)) + rho * (m_lambda * y + m_x.getEntry(i))) / R2; double vt = gamma * sigma * (y + m_lambda * m_x.getEntry(i)); vt1 = vt / R1; vt2 = vt / R2; for (int j = 0; j < 3; ++j) m_v1.setEntry(i, j, vr1 * ir1_vec.getEntry(j) + vt1 * it1_vec.getEntry(j)); for (int j = 0; j < 3; ++j) m_v2.setEntry(i, j, vr2 * ir2_vec.getEntry(j) + vt2 * it2_vec.getEntry(j)); } }
From source file:nova.core.wrapper.mc.forge.v17.wrapper.entity.forward.BWRigidBody.java
void updateRotation(double deltaTime) { //Integrate angular velocity to angular displacement Rotation angularVel = angularVelocity(); Rotation deltaRotation = RotationUtil.slerp(Rotation.IDENTITY, angularVel, deltaTime); entity().transform().setRotation(entity().rotation().applyTo(deltaRotation)); //Integrate torque to angular velocity Vector3D torque = netTorque.scalarMultiply(deltaTime); if (!Vector3D.ZERO.equals(torque)) { setAngularVelocity(angularVelocity().applyTo(new Rotation(Vector3DUtil.FORWARD, torque))); }/* w w w.ja v a 2 s . c om*/ //Clear net torque netTorque = Vector3D.ZERO; //Apply drag Vector3D eulerAngularVel = angularVelocity().applyInverseTo(Vector3DUtil.FORWARD); addTorque(eulerAngularVel.negate().scalarMultiply(angularDrag())); }
From source file:org.jtrfp.trcl.obj.BillboardSprite.java
@Override protected void recalculateTransRotMBuffer() { final Camera camera = getTr().mainRenderer.get().getCamera(); final Vector3D cLookAt = camera.getLookAtVector(); final Rotation rot = new Rotation(cLookAt, rotation); this.setHeading(rot.applyTo(cLookAt.negate())); this.setTop(rot.applyTo(camera.getUpVector())); super.recalculateTransRotMBuffer(); }
From source file:org.jtrfp.trcl.obj.TunnelExitObject.java
public TunnelExitObject(TR tr, Tunnel tun) { super(tr);//from w ww. j av a 2 s . c o m addBehavior(new TunnelExitBehavior()); final DirectionVector v = tun.getSourceTunnel().getExit(); final double EXIT_Y_NUDGE = 0; final InterpolatingAltitudeMap map = tr.getGame().getCurrentMission().getOverworldSystem().getAltitudeMap(); final double exitY = map.heightAt(TR.legacy2Modern(v.getZ()), TR.legacy2Modern(v.getX())) + EXIT_Y_NUDGE; this.exitLocation = new Vector3D(TR.legacy2Modern(v.getZ()), exitY, TR.legacy2Modern(v.getX())); this.tun = tun; exitHeading = map.normalAt(exitLocation.getZ() / TR.mapSquareSize, exitLocation.getX() / TR.mapSquareSize); Vector3D horiz = exitHeading.crossProduct(Vector3D.MINUS_J); if (horiz.getNorm() == 0) { horiz = Vector3D.PLUS_I; } else horiz = horiz.normalize(); exitTop = exitHeading.crossProduct(horiz.negate()).normalize().negate(); exitLocation = exitLocation.add(exitHeading.scalarMultiply(10000)); this.tr = tr; setVisible(false); try { Model m = tr.getResourceManager().getBINModel("SHIP.BIN", tr.getGlobalPaletteVL(), null, tr.gpu.get().getGl()); setModel(m); } catch (Exception e) { e.printStackTrace(); } }
From source file:org.orekit.bodies.EllipsoidTest.java
private void checkPrincipalAxes(Ellipse ps, Vector3D expectedU, Vector3D expectedV) { if (Vector3D.dotProduct(expectedU, ps.getU()) >= 0) { Assert.assertEquals(0, Vector3D.distance(expectedU, ps.getU()), 1.0e-15); Assert.assertEquals(0, Vector3D.distance(expectedV, ps.getV()), 1.0e-15); } else {/*from w w w .ja v a 2 s .c o m*/ Assert.assertEquals(0, Vector3D.distance(expectedU.negate(), ps.getU()), 1.0e-15); Assert.assertEquals(0, Vector3D.distance(expectedV.negate(), ps.getV()), 1.0e-15); } }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java
/** {@inheritDoc} */ public Vector3D radiationPressureAcceleration(final AbsoluteDate date, final Frame frame, final Vector3D position, final Rotation rotation, final double mass, final Vector3D flux) throws OrekitException { if (flux.getNormSq() < Precision.SAFE_MIN) { // null illumination (we are probably in umbra) return Vector3D.ZERO; }/*from ww w. j a v a 2 s.co m*/ // radiation flux in spacecraft frame final Vector3D fluxSat = rotation.applyTo(flux); // solar array contribution Vector3D normal = getNormal(date, frame, position, rotation); double dot = Vector3D.dotProduct(normal, fluxSat); if (dot > 0) { // the solar array is illuminated backward, // fix signs to compute contribution correctly dot = -dot; normal = normal.negate(); } Vector3D force = facetRadiationAcceleration(normal, solarArrayArea, fluxSat, dot); // body facets contribution for (final Facet bodyFacet : facets) { normal = bodyFacet.getNormal(); dot = Vector3D.dotProduct(normal, fluxSat); if (dot < 0) { // the facet intercepts the incoming flux force = force.add(facetRadiationAcceleration(normal, bodyFacet.getArea(), fluxSat, dot)); } } // convert to inertial frame return rotation.applyInverseTo(new Vector3D(1.0 / mass, force)); }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> radiationPressureAcceleration(final AbsoluteDate date, final Frame frame, final Vector3D position, final Rotation rotation, final double mass, final Vector3D flux, final String paramName) throws OrekitException { if (flux.getNormSq() < Precision.SAFE_MIN) { // null illumination (we are probably in umbra) final DerivativeStructure zero = new DerivativeStructure(1, 1, 0.0); return new FieldVector3D<DerivativeStructure>(zero, zero, zero); }/*from w w w . ja v a 2s .c o m*/ final DerivativeStructure absorptionCoeffDS; final DerivativeStructure specularReflectionCoeffDS; if (ABSORPTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, 0, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, specularReflectionCoeff); } else if (REFLECTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, 0, specularReflectionCoeff); } else { throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, paramName, ABSORPTION_COEFFICIENT + ", " + REFLECTION_COEFFICIENT); } final DerivativeStructure diffuseReflectionCoeffDS = absorptionCoeffDS.add(specularReflectionCoeffDS) .subtract(1).negate(); // radiation flux in spacecraft frame final Vector3D fluxSat = rotation.applyTo(flux); // solar array contribution Vector3D normal = getNormal(date, frame, position, rotation); double dot = Vector3D.dotProduct(normal, fluxSat); if (dot > 0) { // the solar array is illuminated backward, // fix signs to compute contribution correctly dot = -dot; normal = normal.negate(); } FieldVector3D<DerivativeStructure> force = facetRadiationAcceleration(normal, solarArrayArea, fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS); // body facets contribution for (final Facet bodyFacet : facets) { normal = bodyFacet.getNormal(); dot = Vector3D.dotProduct(normal, fluxSat); if (dot < 0) { // the facet intercepts the incoming flux force = force.add(facetRadiationAcceleration(normal, bodyFacet.getArea(), fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS)); } } // convert to inertial return FieldRotation.applyInverseTo(rotation, new FieldVector3D<DerivativeStructure>(1.0 / mass, force)); }
From source file:org.orekit.forces.radiation.SolarRadiationPressure.java
/** Get the useful angles for eclipse computation. * @param position the satellite's position in the selected frame. * @param frame in which is defined the position * @param date the date//from w ww .jav a2 s. com * @return the 3 angles {(satCentral, satSun), Central body apparent radius, Sun apparent radius} * @exception OrekitException if the trajectory is inside the Earth */ private double[] getEclipseAngles(final Vector3D position, final Frame frame, final AbsoluteDate date) throws OrekitException { final double[] angle = new double[3]; final Vector3D satSunVector = sun.getPVCoordinates(date, frame).getPosition().subtract(position); // Sat-Sun / Sat-CentralBody angle angle[0] = Vector3D.angle(satSunVector, position.negate()); // Central body apparent radius final double r = position.getNorm(); if (r <= equatorialRadius) { throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE, r); } angle[1] = FastMath.asin(equatorialRadius / r); // Sun apparent radius angle[2] = FastMath.asin(Constants.SUN_RADIUS / satSunVector.getNorm()); return angle; }
From source file:org.orekit.frames.Transform.java
/** Get the inverse transform of the instance. * @return inverse transform of the instance *//*from ww w . j ava 2s. c o m*/ public Transform getInverse() { final Rotation r = angular.getRotation(); final Vector3D o = angular.getRotationRate(); final Vector3D oDot = angular.getRotationAcceleration(); final Vector3D rp = r.applyTo(cartesian.getPosition()); final Vector3D rv = r.applyTo(cartesian.getVelocity()); final Vector3D ra = r.applyTo(cartesian.getAcceleration()); final Vector3D pInv = rp.negate(); final Vector3D crossP = Vector3D.crossProduct(o, rp); final Vector3D vInv = crossP.subtract(rv); final Vector3D crossV = Vector3D.crossProduct(o, rv); final Vector3D crossDotP = Vector3D.crossProduct(oDot, rp); final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP); final Vector3D aInv = new Vector3D(-1, ra, 2, crossV, 1, crossDotP, -1, crossCrossP); return new Transform(date, new PVCoordinates(pInv, vInv, aInv), angular.revert()); }
From source file:org.orekit.propagation.analytical.AdapterPropagatorTest.java
@Test public void testLowEarthOrbit() throws OrekitException, ParseException, IOException { Orbit leo = new CircularOrbit( 7200000.0, -1.0e-5, 2.0e-4, FastMath.toRadians(98.0), FastMath.toRadians(123.456), 0.0, PositionAngle.MEAN, FramesFactory.getEME2000(), new AbsoluteDate(new DateComponents(2004, 01, 01), new TimeComponents(23, 30, 00.000), TimeScalesFactory.getUTC()), Constants.EIGEN5C_EARTH_MU); double mass = 5600.0; AbsoluteDate t0 = leo.getDate().shiftedBy(1000.0); Vector3D dV = new Vector3D(-0.1, 0.2, 0.3); double f = 20.0; double isp = 315.0; double vExhaust = Constants.G0_STANDARD_GRAVITY * isp; double dt = -(mass * vExhaust / f) * FastMath.expm1(-dV.getNorm() / vExhaust); BoundedPropagator withoutManeuver = getEphemeris(leo, mass, 5, new LofOffset(leo.getFrame(), LOFType.LVLH), t0, Vector3D.ZERO, f, isp, false, false, null); BoundedPropagator withManeuver = getEphemeris(leo, mass, 5, new LofOffset(leo.getFrame(), LOFType.LVLH), t0, dV, f, isp, false, false, null); // we set up a model that reverts the maneuvers AdapterPropagator adapterPropagator = new AdapterPropagator(withManeuver); AdapterPropagator.DifferentialEffect effect = new SmallManeuverAnalyticalModel( adapterPropagator.propagate(t0), dV.negate(), isp); adapterPropagator.addEffect(effect); adapterPropagator.addAdditionalStateProvider(new AdditionalStateProvider() { public String getName() { return "dummy 3"; }// w ww . j av a 2 s.co m public double[] getAdditionalState(SpacecraftState state) { return new double[3]; } }); // the adapted propagators do not manage the additional states from the reference, // they simply forward them Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 1")); Assert.assertFalse(adapterPropagator.isAdditionalStateManaged("dummy 2")); Assert.assertTrue(adapterPropagator.isAdditionalStateManaged("dummy 3")); for (AbsoluteDate t = t0.shiftedBy(0.5 * dt); t.compareTo(withoutManeuver.getMaxDate()) < 0; t = t .shiftedBy(60.0)) { PVCoordinates pvWithout = withoutManeuver.getPVCoordinates(t, leo.getFrame()); PVCoordinates pvReverted = adapterPropagator.getPVCoordinates(t, leo.getFrame()); double revertError = new PVCoordinates(pvWithout, pvReverted).getPosition().getNorm(); Assert.assertEquals(0, revertError, 0.45); Assert.assertEquals(2, adapterPropagator.propagate(t).getAdditionalState("dummy 1").length); Assert.assertEquals(1, adapterPropagator.propagate(t).getAdditionalState("dummy 2").length); Assert.assertEquals(3, adapterPropagator.propagate(t).getAdditionalState("dummy 3").length); } }