List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getNorm
public double getNorm()
From source file:fr.cs.examples.frames.Frames3.java
public static void main(String[] args) { try {//w w w . ja v a 2 s. co m // configure Orekit and printing format Autoconfiguration.configureOrekit(); // Initial state definition : // ========================== // Date // **** AbsoluteDate initialDate = new AbsoluteDate(new DateComponents(1970, 04, 07), TimeComponents.H00, TimeScalesFactory.getUTC()); // Orbit // ***** // The Sun is in the orbital plane for raan ~ 202 double mu = 3.986004415e+14; // gravitation coefficient Frame eme2000 = FramesFactory.getEME2000(); // inertial frame Orbit orbit = new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(220.), FastMath.toRadians(5.300), PositionAngle.MEAN, eme2000, initialDate, mu); // Attitude laws // ************* // Earth Frame earthFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, earthFrame); // Target pointing attitude provider over satellite nadir at date, without yaw compensation NadirPointing nadirLaw = new NadirPointing(eme2000, earth); // Target pointing attitude provider with yaw compensation final PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); YawSteering yawSteeringLaw = new YawSteering(eme2000, nadirLaw, sun, Vector3D.MINUS_I); // Propagator : Eckstein-Hechler analytic propagator Propagator propagator = new EcksteinHechlerPropagator(orbit, yawSteeringLaw, Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS, Constants.EIGEN5C_EARTH_MU, Constants.EIGEN5C_EARTH_C20, Constants.EIGEN5C_EARTH_C30, Constants.EIGEN5C_EARTH_C40, Constants.EIGEN5C_EARTH_C50, Constants.EIGEN5C_EARTH_C60); // Let's write the results in a file in order to draw some plots. propagator.setMasterMode(10, new OrekitFixedStepHandler() { PrintStream out = null; public void init(SpacecraftState s0, AbsoluteDate t) throws PropagationException { try { File file = new File(System.getProperty("user.home"), "XYZ.dat"); System.out.println("Results written to file: " + file.getAbsolutePath()); out = new PrintStream(file); out.println("#time X Y Z Wx Wy Wz"); } catch (IOException ioe) { throw new PropagationException(ioe, LocalizedFormats.SIMPLE_MESSAGE, ioe.getLocalizedMessage()); } } public void handleStep(SpacecraftState currentState, boolean isLast) throws PropagationException { try { // get the transform from orbit/attitude reference frame to spacecraft frame Transform inertToSpacecraft = currentState.toTransform(); // get the position of the Sun in orbit/attitude reference frame Vector3D sunInert = sun.getPVCoordinates(currentState.getDate(), currentState.getFrame()) .getPosition(); // convert Sun position to spacecraft frame Vector3D sunSat = inertToSpacecraft.transformPosition(sunInert); // and the spacecraft rotational rate also Vector3D spin = inertToSpacecraft.getRotationRate(); // Lets calculate the reduced coordinates double sunX = sunSat.getX() / sunSat.getNorm(); double sunY = sunSat.getY() / sunSat.getNorm(); double sunZ = sunSat.getZ() / sunSat.getNorm(); out.format(Locale.US, "%s %12.3f %12.3f %12.3f %12.7f %12.7f %12.7f%n", currentState.getDate(), sunX, sunY, sunZ, spin.getX(), spin.getY(), spin.getZ()); if (isLast) { out.close(); } } catch (OrekitException oe) { throw new PropagationException(oe); } } }); System.out.println("Running..."); propagator.propagate(initialDate.shiftedBy(6000)); } catch (OrekitException oe) { System.err.println(oe.getMessage()); } }
From source file:gentracklets.conversions.java
public static double[] geo2radec(PVCoordinates obj, TopocentricFrame staF, Frame inertialFrame, AbsoluteDate epoch) {//from w ww. ja va2 s . c o m Vector3D rho = new Vector3D(0, 0, 0); try { rho = obj.getPosition().subtract(staF.getPVCoordinates(epoch, inertialFrame).getPosition()); } catch (OrekitException ex) { Logger.getLogger(conversions.class.getName()).log(Level.SEVERE, null, ex); } double rho_mag = rho.getNorm(); double DEC = FastMath.asin(rho.getZ() / rho_mag); double cosRA = 0.0; double sinRA = 0.0; double RA = 0.0; Vector3D v_site = new Vector3D(0, 0, 0); try { v_site = staF.getPVCoordinates(epoch, inertialFrame).getVelocity(); } catch (OrekitException ex) { Logger.getLogger(conversions.class.getName()).log(Level.SEVERE, null, ex); } Vector3D rhoDot = obj.getVelocity().subtract(v_site); if (FastMath.sqrt(FastMath.pow(rho.getX(), 2) + FastMath.pow(rho.getY(), 2)) != 0) { cosRA = rho.getX() / FastMath.sqrt(FastMath.pow(rho.getX(), 2) + FastMath.pow(rho.getY(), 2)); sinRA = rho.getY() / FastMath.sqrt(FastMath.pow(rho.getX(), 2) + FastMath.pow(rho.getY(), 2)); RA = FastMath.atan2(sinRA, cosRA); if (RA <= 0) { RA = RA + 2 * FastMath.PI; } } else { sinRA = rhoDot.getY() / FastMath.sqrt(FastMath.pow(rhoDot.getX(), 2) + FastMath.pow(rhoDot.getY(), 2)); cosRA = rhoDot.getX() / FastMath.sqrt(FastMath.pow(rhoDot.getX(), 2) + FastMath.pow(rhoDot.getY(), 2)); RA = FastMath.atan2(sinRA, cosRA); if (RA <= 0) { RA = RA + 2 * FastMath.PI; } } double rhoDot_mag = rho.dotProduct(rhoDot) / rho_mag; double RAdot = (rhoDot.getX() * rho.getY() - rhoDot.getY() * rho.getX()) / (-1 * FastMath.pow(rho.getY(), 2) - FastMath.pow(rho.getX(), 2)); double DECdot = (rhoDot.getZ() - rhoDot_mag * FastMath.sin(DEC)) / FastMath.sqrt(FastMath.pow(rho.getX(), 2) + FastMath.pow(rho.getY(), 2)); double[] out = { RA, RAdot, DEC, DECdot, rho_mag, rhoDot_mag }; return out; }
From source file:com.mapr.synth.drive.GeoPoint.java
public Vector3D east(Vector3D r) { Vector3D ux = r.crossProduct(Z); if (ux.getNorm() < 1e-4) { // near the poles (i.e. < 640 meters from them), the definition of east is difficult ux = this.r.crossProduct(X); }/* w w w . j a va2 s.c o m*/ ux = ux.normalize(); return ux; }
From source file:jat.core.cm.TwoBodyAPL.java
@Override public void computeDerivatives(double t, double[] y, double[] yDot) { Vector3D r = new Vector3D(y[0], y[1], y[2]); double rnorm = r.getNorm(); double r3 = rnorm * rnorm * rnorm; double k = -1. * this.mu / r3; yDot[0] = y[3];// w w w . j ava 2 s.c om yDot[1] = y[4]; yDot[2] = y[5]; yDot[3] = k * y[0]; yDot[4] = k * y[1]; yDot[5] = k * y[2]; }
From source file:jtrace.texture.TransparentFinish.java
/** * Obtain ray refracted according to Snell's law. * //from ww w. jav a 2 s .c o m * @param incidentRay * @param normalRay * @return refracted ray */ Ray getRefractedRay(Ray incidentRay, Ray normalRay) { // Check direction of incident ray (inside to out or outside to in) // and adjust ior accordingly: double snellFactor; Vector3D axis; if (incidentRay.direction.dotProduct(normalRay.direction) < 0.0) { snellFactor = 1.0 / ior; axis = incidentRay.direction.crossProduct(normalRay.direction); } else { snellFactor = ior; axis = normalRay.direction.crossProduct(incidentRay.direction); } if (axis.getNorm() > 0) { axis = axis.normalize(); } else { return new Ray(normalRay.getOrigin(), incidentRay.direction); } double angle = Math .asin((incidentRay.direction.normalize().crossProduct(normalRay.direction).normalize()).getNorm()); double newAngle = Math.asin(snellFactor * Math.sin(angle)); Rotation rotation = new Rotation(axis, newAngle - angle); Vector3D refractedDir = rotation.applyTo(incidentRay.direction); return new Ray(normalRay.getOrigin(), refractedDir); }
From source file:magma.tools.benchmark.model.proxy.BenchmarkAgentProxy.java
/** * Called before a message from the server was forwarded to the client * @param msg the message received from the server *///from w ww .jav a 2s .com @Override protected byte[] onNewServerMessage(byte[] msg) { if (isGazebo) { msg = extractGazeboGroundTruth(new String(msg)).getBytes(); } double leftPressure = 0; double rightPressure = 0; String message = new String(msg); if (isGazebo) { extractTime(message); } while (message.contains("(FRP (n lf") || message.contains("(FRP (n rf")) { Vector3D leftForce = getForce(message, "(FRP (n lf"); if (leftForce == null) { return msg; } Vector3D rightForce = getForce(message, "(FRP (n rf"); if (rightForce == null) { return msg; } leftPressure += leftForce.getNorm(); rightPressure += rightForce.getNorm(); message = message.replaceFirst("\\(FRP \\(n lf", ""); message = message.replaceFirst("\\(FRP \\(n rf", ""); } if (startCount && !stopCount) { if (leftPressure < 0.01 && rightPressure < 0.01) { bothLegsOffGround++; if (showMessages) { System.out.println("off ground"); } } else { legOnGround++; if (leftPressure >= 0.01 && rightPressure >= 0.01) { noLegOffGround++; } else { oneLegOffGround++; } } } return msg; }
From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java
private void handleGyroBias(ImuData imuData) { /*//from ww w . ja v a 2 s . c o m * Shouldn't need to do any of this if we call dmp_set_orientation... // do axis rotation if (axisRotation != null) { // need to do an axis rotation byte[][] matrix = axisRotation.getOrientationMatrix(); IMUData tempIMU = imuData; // do new x value if (matrix[0][0] != 0) { imuData.gyro.setX(tempIMU.gyro.x() * matrix[0][0]); imuData.accel.setX(tempIMU.accel.x() * matrix[0][0]); imuData.compass.setX(tempIMU.compass.x() * matrix[0][0]); } else if (matrix[0][1] != 0) { imuData.gyro.setX(tempIMU.gyro.y() * matrix[0][1]); imuData.accel.setX(tempIMU.accel.y() * matrix[0][1]); imuData.compass.setX(tempIMU.compass.y() * matrix[0][1]); } else if (matrix[0][2] != 0) { imuData.gyro.setX(tempIMU.gyro.z() * matrix[0][2]); imuData.accel.setX(tempIMU.accel.z() * matrix[0][2]); imuData.compass.setX(tempIMU.compass.z() * matrix[0][2]); } // do new y value if (matrix[1][0] != 0) { imuData.gyro.setY(tempIMU.gyro.x() * matrix[1][0]); imuData.accel.setY(tempIMU.accel.x() * matrix[1][0]); imuData.compass.setY(tempIMU.compass.x() * matrix[1][0]); } else if (matrix[1][1] != 0) { imuData.gyro.setY(tempIMU.gyro.y() * matrix[1][1]); imuData.accel.setY(tempIMU.accel.y() * matrix[1][1]); imuData.compass.setY(tempIMU.compass.y() * matrix[1][1]); } else if (matrix[1][2] != 0) { imuData.gyro.setY(tempIMU.gyro.z() * matrix[1][2]); imuData.accel.setY(tempIMU.accel.z() * matrix[1][2]); imuData.compass.setY(tempIMU.compass.z() * matrix[1][2]); } // do new z value if (matrix[2][0] != 0) { imuData.gyro.setZ(tempIMU.gyro.x() * matrix[2][0]); imuData.accel.setZ(tempIMU.accel.x() * matrix[2][0]); imuData.compass.setZ(tempIMU.compass.x() * matrix[2][0]); } else if (matrix[2][1] != 0) { imuData.gyro.setZ(tempIMU.gyro.y() * matrix[2][1]); imuData.accel.setZ(tempIMU.accel.y() * matrix[2][1]); imuData.compass.setZ(tempIMU.compass.y() * matrix[2][1]); } else if (matrix[2][2] != 0) { imuData.gyro.setZ(tempIMU.gyro.z() * matrix[2][2]); imuData.accel.setZ(tempIMU.accel.z() * matrix[2][2]); imuData.compass.setZ(tempIMU.compass.z() * matrix[2][2]); } } */ Vector3D deltaAccel = previousAccel.subtract(imuData.getAccel()); // compute difference previousAccel = imuData.getAccel(); if ((deltaAccel.getNorm() < RTIMU_FUZZY_ACCEL_ZERO) && (imuData.getGyro().getNorm() < RTIMU_FUZZY_GYRO_ZERO)) { // What we are seeing on the gyros should be bias only so learn from this if (gyroSampleCount < (5 * sampleRate)) { gyroBias = new Vector3D( (1.0 - gyroLearningAlpha) * gyroBias.getX() + gyroLearningAlpha * imuData.getGyro().getX(), (1.0 - gyroLearningAlpha) * gyroBias.getY() + gyroLearningAlpha * imuData.getGyro().getY(), (1.0 - gyroLearningAlpha) * gyroBias.getZ() + gyroLearningAlpha * imuData.getGyro().getZ()); gyroSampleCount++; if (gyroSampleCount == (5 * sampleRate)) { // this could have been true already of course gyroBiasValid = true; //saveSettings(); } } else { gyroBias = new Vector3D( (1.0 - gyroContinuousAlpha) * gyroBias.getX() + gyroContinuousAlpha * imuData.getGyro().getX(), (1.0 - gyroContinuousAlpha) * gyroBias.getY() + gyroContinuousAlpha * imuData.getGyro().getY(), (1.0 - gyroContinuousAlpha) * gyroBias.getZ() + gyroContinuousAlpha * imuData.getGyro().getZ()); } } imuData.setGyro(imuData.getGyro().subtract(gyroBias)); }
From source file:edu.stanford.cfuller.imageanalysistools.filter.ConvexHullByLabelFilter.java
/** * Applies the convex hull filter to the supplied mask. * @param im The Image to process-- a mask whose regions will be replaced by their filled convex hulls. *//*from www. j a va2 s. c om*/ @Override public void apply(WritableImage im) { RelabelFilter RLF = new RelabelFilter(); RLF.apply(im); Histogram h = new Histogram(im); java.util.Hashtable<Integer, java.util.Vector<Integer>> xLists = new java.util.Hashtable<Integer, java.util.Vector<Integer>>(); java.util.Hashtable<Integer, java.util.Vector<Integer>> yLists = new java.util.Hashtable<Integer, java.util.Vector<Integer>>(); java.util.Vector<Integer> minValues = new java.util.Vector<Integer>(h.getMaxValue() + 1); java.util.Vector<Integer> minIndices = new java.util.Vector<Integer>(h.getMaxValue() + 1); for (int i = 0; i < h.getMaxValue() + 1; i++) { minValues.add(im.getDimensionSizes().get(ImageCoordinate.X)); minIndices.add(0); } for (ImageCoordinate i : im) { int value = (int) im.getValue(i); if (value == 0) continue; if (!xLists.containsKey(value)) { xLists.put(value, new java.util.Vector<Integer>()); yLists.put(value, new java.util.Vector<Integer>()); } xLists.get(value).add(i.get(ImageCoordinate.X)); yLists.get(value).add(i.get(ImageCoordinate.Y)); if (i.get(ImageCoordinate.X) < minValues.get(value)) { minValues.set(value, i.get(ImageCoordinate.X)); minIndices.set(value, xLists.get(value).size() - 1); } } java.util.Vector<Integer> hullPointsX = new java.util.Vector<Integer>(); java.util.Vector<Integer> hullPointsY = new java.util.Vector<Integer>(); ImageCoordinate ic = ImageCoordinate.createCoordXYZCT(0, 0, 0, 0, 0); for (int k = 1; k < h.getMaxValue() + 1; k++) { hullPointsX.clear(); hullPointsY.clear(); java.util.Vector<Integer> xList = xLists.get(k); java.util.Vector<Integer> yList = yLists.get(k); int minIndex = (int) minIndices.get(k); //start at the leftmost point int currentIndex = minIndex; int currentX = xList.get(currentIndex); int currentY = yList.get(currentIndex); hullPointsX.add(currentX); hullPointsY.add(currentY); org.apache.commons.math3.linear.RealVector angles = new org.apache.commons.math3.linear.ArrayRealVector( xList.size()); Vector3D currentVector = new Vector3D(0, -1, 0); java.util.HashSet<Integer> visited = new java.util.HashSet<Integer>(); do { visited.add(currentIndex); int maxIndex = 0; double maxAngle = -2 * Math.PI; double dist = Double.MAX_VALUE; for (int i = 0; i < xList.size(); i++) { if (i == currentIndex) continue; Vector3D next = new Vector3D(xList.get(i) - xList.get(currentIndex), yList.get(i) - yList.get(currentIndex), 0); double angle = Vector3D.angle(currentVector, next); angles.setEntry(i, angle); if (angle > maxAngle) { maxAngle = angle; maxIndex = i; dist = next.getNorm(); } else if (angle == maxAngle) { double tempDist = next.getNorm(); if (tempDist < dist) { dist = tempDist; maxAngle = angle; maxIndex = i; } } } currentX = xList.get(maxIndex); currentY = yList.get(maxIndex); currentVector = new Vector3D(xList.get(currentIndex) - currentX, yList.get(currentIndex) - currentY, 0); hullPointsX.add(currentX); hullPointsY.add(currentY); currentIndex = maxIndex; } while (!visited.contains(currentIndex)); //hull vertices have now been determined .. need to fill in the lines //between them so I can apply a fill filter //approach: x1, y1 to x0, y0: //start at min x, min y, go to max x, max y // if x_i, y_i = x0, y0 + slope to within 0.5 * sqrt(2), then add to hull double eps = Math.sqrt(2); for (int i = 0; i < hullPointsX.size() - 1; i++) { int x0 = hullPointsX.get(i); int y0 = hullPointsY.get(i); int x1 = hullPointsX.get(i + 1); int y1 = hullPointsY.get(i + 1); int xmin = (x0 < x1) ? x0 : x1; int ymin = (y0 < y1) ? y0 : y1; int xmax = (x0 > x1) ? x0 : x1; int ymax = (y0 > y1) ? y0 : y1; x1 -= x0; y1 -= y0; double denom = (x1 * x1 + y1 * y1); for (int x = xmin; x <= xmax; x++) { for (int y = ymin; y <= ymax; y++) { int rel_x = x - x0; int rel_y = y - y0; double projLength = (x1 * rel_x + y1 * rel_y) / denom; double projPoint_x = x1 * projLength; double projPoint_y = y1 * projLength; if (Math.hypot(rel_x - projPoint_x, rel_y - projPoint_y) < eps) { ic.set(ImageCoordinate.X, x); ic.set(ImageCoordinate.Y, y); im.setValue(ic, k); } } } } } ic.recycle(); FillFilter ff = new FillFilter(); ff.apply(im); }
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 a v a 2 s .c o 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:org.hbird.business.navigation.processors.orekit.EclipseCalculator.java
public static double calculateEclipse(SpacecraftState s) throws OrekitException { double occultedRadius = Constants.EQUATORIAL_RADIUS_OF_SUN; double occultingRadius = Constants.EQUATORIAL_RADIUS_OF_THE_EARTH; final Vector3D pted = CelestialBodyFactory.getSun().getPVCoordinates(s.getDate(), s.getFrame()) .getPosition();//from w ww .j a v a 2 s .c om final Vector3D ping = CelestialBodyFactory.getEarth().getPVCoordinates(s.getDate(), s.getFrame()) .getPosition(); final Vector3D psat = s.getPVCoordinates().getPosition(); final Vector3D ps = pted.subtract(psat); final Vector3D po = ping.subtract(psat); final double angle = Vector3D.angle(ps, po); final double rs = Math.asin(occultedRadius / ps.getNorm()); final double ro = Math.asin(occultingRadius / po.getNorm()); return angle - ro - rs; }