List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getZ
public double getZ()
From source file:edu.mit.fss.hla.FSScartesianVector.java
/** * Sets this vector from a {@link Vector3D} object. * * @param value the new value/*from w w w . j a v a 2 s . c o m*/ */ public void setValue(Vector3D value) { this.x.setValue(value.getX()); this.y.setValue(value.getY()); this.z.setValue(value.getZ()); }
From source file:jtrace.object.Cube.java
@Override public double getFirstCollisionObjectFrame(Ray ray) { // Cube edges and vertices are the intersections of 6 planes. // Need to find points of intersection with each of these planes. double alpha = Double.POSITIVE_INFINITY; Ray collisionNormal = new Ray(); for (int i = 0; i < 6; i++) { double thisAlpha = normals[i].direction.dotProduct(normals[i].origin.subtract(ray.origin)) / normals[i].direction.dotProduct(ray.direction); if (thisAlpha <= 0) continue; Vector3D collisionLocation = ray.origin.add(thisAlpha, ray.direction); Vector3D delta = collisionLocation.subtract(normals[i].origin); if (Math.abs(delta.getX()) > 0.5 || Math.abs(delta.getY()) > 0.5 || Math.abs(delta.getZ()) > 0.5) { continue; }//w ww . j a v a 2 s . c o m if (thisAlpha < alpha) { alpha = thisAlpha; collisionNormal.origin = collisionLocation; collisionNormal.direction = normals[i].direction; } } if (alpha < Double.POSITIVE_INFINITY) { incidentRay = ray; normalRay = collisionNormal; } return alpha; }
From source file:edu.stanford.cfuller.imageanalysistools.filter.WatershedFilter.java
/** * Gets the seed Image for the watershed segmentation. If no seed Image has been set externally, one is created from * the set of pixels at the lowest greylevel. If a seed Image has been set externally, the seedImage retrieved is the external one. * /*from ww w.j av a 2s. c om*/ * @param greylevelLookup A hashtable mapping greylevel values in the Image to coordinates in the Image. * @param im The Image being segmented. * @param h A Histogram of the Image being segmented, constructed before the beginning of segmentation. * @return An Image containing the seeds for the watershed algorithm. */ protected WritableImage getSeedImage(java.util.Hashtable<Double, java.util.Vector<Vector3D>> greylevelLookup, Image im, Histogram h) { if (!(this.seedImage == null)) { return ImageFactory.createWritable(this.seedImage); } WritableImage tempSeed = null; if (this.seedImage == null) { tempSeed = ImageFactory.createWritable(im.getDimensionSizes(), 0.0f); } else { tempSeed = ImageFactory.createWritable(this.seedImage); } double minValue = h.getMinValue(); java.util.Vector<Vector3D> minPoints = greylevelLookup.get(minValue); ImageCoordinate ic = ImageCoordinate.createCoordXYZCT(0, 0, 0, 0, 0); for (Vector3D v : minPoints) { ic.set(ImageCoordinate.X, (int) (v.getX())); ic.set(ImageCoordinate.Y, (int) (v.getY())); ic.set(ImageCoordinate.Z, (int) (v.getZ())); tempSeed.setValue(ic, 1); } ic.recycle(); LabelFilter lf = new LabelFilter(); lf.apply(tempSeed); if (this.seedImage != null) this.seedImage = tempSeed; return tempSeed; }
From source file:edu.stanford.cfuller.imageanalysistools.filter.WatershedFilter.java
/** * Applies the WatershedFilter to the specified Image, segmenting it. * @param im The Image to be segmented. *//*from w ww. jav a 2 s . c o m*/ @Override public void apply(WritableImage im) { WritableImage imCopy = ImageFactory.createWritable(im); InversionFilter invf = new InversionFilter(); invf.apply(imCopy); Histogram h = new Histogram(imCopy); java.util.Hashtable<Double, java.util.Vector<Vector3D>> greylevelLookup = new Hashtable<Double, java.util.Vector<Vector3D>>(); for (ImageCoordinate ic : imCopy) { double value = imCopy.getValue(ic); if (greylevelLookup.get(value) == null) { greylevelLookup.put(value, new java.util.Vector<Vector3D>()); } greylevelLookup.get(value).add( new Vector3D(ic.get(ImageCoordinate.X), ic.get(ImageCoordinate.Y), ic.get(ImageCoordinate.Z))); } WritableImage processing = getSeedImage(greylevelLookup, imCopy, h); Histogram hSeed = new Histogram(processing); int nextLabel = hSeed.getMaxValue() + 1; ImageCoordinate ic = ImageCoordinate.createCoordXYZCT(0, 0, 0, 0, 0); for (int i = h.getMinValue() + 1; i < h.getMaxValue(); i++) { //java.util.logging.Logger.getLogger("edu.stanford.cfuller.imageanalysistools").info("processing greylevel: " + i); if (h.getCounts(i) == 0) continue; for (Vector3D v : greylevelLookup.get((double) i)) { int x = (int) v.getX(); int y = (int) v.getY(); int z = (int) v.getZ(); ic.set(ImageCoordinate.X, x); ic.set(ImageCoordinate.Y, y); ic.set(ImageCoordinate.Z, z); int label = getCorrectLabel(ic, processing, nextLabel); processing.setValue(ic, label); if (label == nextLabel) nextLabel++; } } ic.recycle(); MaskFilter mf = new MaskFilter(); mf.setReferenceImage(processing); mf.apply(im); LabelFilter lf = new LabelFilter(); lf.apply(im); }
From source file:Engine.WorldMap.java
public double GetGroundPosition(Vector3D position, WorldMap worldMap) { return GetGroundPosition(new double[] { position.getX(), position.getY(), position.getZ() }, worldMap); }
From source file:Engine.WorldMap.java
public double GetGroundPosition(double[] position, WorldMap worldMap) { double retX = 0; double retY = 0; double x = (int) position[0] / GenerateTerrain.Size; double y = (int) position[1] / GenerateTerrain.Size; double[] p11 = { x, y, (double) worldMap.Map[(int) x][(int) y] }; double[] p12 = { x, y + 1, (double) worldMap.Map[(int) x][(int) y + 1] }; double[] p21 = { x + 1, y, (double) worldMap.Map[(int) x + 1][(int) y] }; double[] p22 = { x + 1, y + 1, (double) worldMap.Map[(int) x + 1][(int) y + 1] }; if (PointInTriangle(position, p11, p12, p22)) { Vector3D v1 = new Vector3D(p12[0] - p11[0], p12[1] - p11[1], p12[2] - p11[2]); Vector3D v2 = new Vector3D(p22[0] - p11[0], p22[1] - p11[1], p22[2] - p11[2]); Vector3D n = Vector3D.crossProduct(v1, v2); double z = ((n.getX() * x) + (n.getY() * y) + ((-n.getX() * p11[0]) + (-n.getY() * p11[1]) + (-n.getZ() * p11[2]))) / (-n.getZ()); return z; //return (z3(x-x1)(y-y2) + z1(x-x2)(y-y3) + z2(x-x3)(y-y1) - z2(x-x1)(y-y3) - z3(x-x2)(y-y1) - z1(x-x3)(y-y2)) // / ( (x-x1)(y-y2) + (x-x2)(y-y3) + (x-x3)(y-y1) - (x-x1)(y-y3) - (x-x2)(y-y1) - (x-x3)(y-y2)); } else {//from w w w.java 2 s .c o m Vector3D v1 = new Vector3D(p21[0] - p11[0], p21[1] - p11[1], p21[2] - p11[2]); Vector3D v2 = new Vector3D(p22[0] - p11[0], p22[1] - p11[1], p22[2] - p11[2]); Vector3D n = Vector3D.crossProduct(v1, v2); double z = ((n.getX() * x) + (n.getY() * y) + ((-n.getX() * p11[0]) + (-n.getY() * p11[1]) + (-n.getZ() * p11[2]))) / (-n.getZ()); return z; } //screen.worldMap.Map[x][y]; }
From source file:Engine.Projectile.java
public Projectile(Vector3D[] forces, Vector3D size, Vector3D intialPosition, Vector3D velocities[], boolean affectedByGravity, Screen screen, IProjectileFunction projectileFunction, double magicMultiplayer, Object[] ownserId) { this.ownerId = ownserId; this.magicMultiplayer = magicMultiplayer; this.intialPosition = new Vector3D(intialPosition.getX(), intialPosition.getY(), intialPosition.getZ()); this.actualPosition = new Vector3D(intialPosition.getX(), intialPosition.getY(), intialPosition.getZ()); //this. boundingSphere = new BoundingSphere(actualPosition.getX(),actualPosition.getY(), actualPosition.getZ(), Math.max(Math.max(size.getX(), size.getY()), size.getZ())); this.boundingBox = new BoundingBox(intialPosition.getX() - (size.getX() / 2.0), intialPosition.getY() - (size.getY() / 2.0), intialPosition.getZ() - (size.getZ() / 2.0), intialPosition.getX() + (size.getX() / 2.0), intialPosition.getY() + (size.getY() / 2.0), intialPosition.getZ() + (size.getZ() / 2.0), true); this.screen = screen; this.size = size; this.projectileFunction = projectileFunction; if (velocities != null) { this.velocities = new Vector3D[velocities.length]; for (int a = 0; a < velocities.length; a++) this.velocities[a] = new Vector3D(velocities[a].getX(), velocities[a].getY(), velocities[a].getZ()); }/* w ww.j av a 2s . c o m*/ int forcesCount = 0; if (forces != null) { forcesCount += forces.length; if (affectedByGravity) forcesCount++; this.forces = new Vector3D[forcesCount]; for (int a = 0; a < forces.length; a++) this.forces[a] = new Vector3D(forces[a].getX(), forces[a].getY(), forces[a].getZ()); if (affectedByGravity) this.forces[forcesCount - 1] = new Vector3D(gravity.getX(), gravity.getY(), gravity.getZ()); } else { if (affectedByGravity) { forcesCount++; this.forces = new Vector3D[forcesCount]; this.forces[forcesCount - 1] = new Vector3D(gravity.getX(), gravity.getY(), gravity.getZ()); } } }
From source file:Engine.WorldMap.java
public Vector3D CorrectPosition(Vector3D position, Screen screen) { double x = (double) (position.getX()); double y = (double) (position.getY()); x = DoubleModulo(x, (screen.worldMap.mapSize - 1) * GenerateTerrain.Size); y = DoubleModulo(y, (screen.worldMap.mapSize - 1) * GenerateTerrain.Size); return new Vector3D(x, y, position.getZ()); }
From source file:Engine.Player.java
public ArrayList<DPolygon> GetPolygons(double x, double y) { /*Cube head = new Cube(- halfHeadSize + x, - halfHeadSize + y, ViewFrom[2] - halfHeadSize + epsBody, halfHeadSize * 2.0, halfHeadSize * 2.0, halfHeadSize * 2.0, Color.YELLOW, PlayerId); head.Polys[2].DrawablePolygon.texture = GenerateTerrain.img; /* w ww. ja v a 2 s .c o m*/ Cube neck = new Cube(- halfNeckSize + x, - halfNeckSize + y, ViewFrom[2] - headSize + halfNeckSize, halfNeckSize * 2.0, halfNeckSize * 2.0, halfNeckSize, Color.PINK, PlayerId); Cube corpus = new Cube(- halfCorupsXSize + x, - halfCorupsYSize + y, ViewFrom[2] - corpusZSize - headSize + halfNeckSize - epsBody, halfCorupsXSize * 2.0, halfCorupsYSize * 2.0, 2 * halfCorpusZSize, Color.BLUE, PlayerId); Cube legs = new Cube(- halfLegSizeX + x, - halfLegSizeY + y, ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, 2 * halfLegSizeX, 2 * halfLegSizeY, legZSize, Color.MAGENTA, PlayerId); Cube hands = new Cube(- halfHandsXSize + x, - halfHandsYSize + y, ViewFrom[2] - handsZSize - headSize + halfNeckSize - epsBody, halfHandsXSize * 2.0, halfHandsYSize * 2.0, handsZSize, Color.CYAN, PlayerId); ArrayList<DPolygon> all = new ArrayList<DPolygon>(); rot += 0.01; head.rotation = rot; head.setRotAdd(); head.updatePoly(); neck.rotation = rot; neck.updatePoly(); corpus.rotation = rot; corpus.updatePoly(); legs.rotation = rot; legs.updatePoly(); hands.rotation = rot; hands.updatePoly(); AddArrayToArrayList(all, head.GetPolygons()); AddArrayToArrayList(all, neck.GetPolygons()); AddArrayToArrayList(all, corpus.GetPolygons()); AddArrayToArrayList(all, hands.GetPolygons()); AddArrayToArrayList(all, legs.GetPolygons());*/ //rot = Math.PI*0.75; //rot += 0.01; /*if (MoveDirection != null) { //0 if (Math.abs(MoveDirection.getX()) < 0.1 && MoveDirection.getY() > 0.1) rot = 0; //1/4 if (MoveDirection.getX() > 0.1 && MoveDirection.getY() > 0.1) rot = -Math.PI / 4.0; //1/2 if (MoveDirection.getX() > 0.1 && Math.abs(MoveDirection.getY()) < 0.1) rot = -Math.PI / 2.0; //3/4 if (MoveDirection.getX() > 0.1 && MoveDirection.getY() < 0.1) rot = -3.0 * Math.PI / 4.0; //1 if (Math.abs(MoveDirection.getX()) < 0.1 && MoveDirection.getY() < 0.1) rot = -Math.PI; //5/4 if (MoveDirection.getX() < 0.1 && MoveDirection.getY() < 0.1) rot = -5.0 * Math.PI / 4.0; //3/2 if (MoveDirection.getX() < 0.1 && Math.abs(MoveDirection.getY()) < 0.1) rot = -3.0 * Math.PI / 2.0; //7/4 if (MoveDirection.getX() < 0.1 && MoveDirection.getY() > 0.1) rot = -7.0 * Math.PI / 4.0; rot += 2 * Math.PI / 4; }*/ if (PositionIFace != null) { rot = Math.PI / 4.0; Vector3D v1 = new Vector3D(PositionIFace.getX(), PositionIFace.getY(), 0); Vector3D v2 = new Vector3D(ViewFrom[0], ViewFrom[1], 0); Vector3D v3 = (v1.subtract(v2)).normalize(); Vector3D cross = Vector3D.crossProduct(v3, new Vector3D(1, 0, 0)); double dot = Vector3D.dotProduct(v3, new Vector3D(1, 0, 0)); double angle = Math.atan2(cross.getZ(), dot); rot += -angle; } double xx = -halfHeadSize + x; double yy = -halfHeadSize + y; double[] r = rotatePoint(xx, yy, xx, yy, rot - Math.PI * 0.75); Cube head = new Cube(r[0], r[1], ViewFrom[2] - halfHeadSize + epsBody, halfHeadSize * 2.0, halfHeadSize * 2.0, halfHeadSize * 2.0, Color.YELLOW, PlayerId); head.Polys[2].DrawablePolygon.texture = GenerateTerrain.img; Cube neck = new Cube(-halfNeckSize + x, -halfNeckSize + y, ViewFrom[2] - headSize + halfNeckSize, halfNeckSize * 2.0, halfNeckSize * 2.0, halfNeckSize, Color.PINK, PlayerId); Cube corpus = new Cube(-halfCorupsXSize + x, -halfCorupsYSize + y, ViewFrom[2] - corpusZSize - headSize + halfNeckSize - epsBody, halfCorupsXSize * 2.0, halfCorupsYSize * 2.0, 2 * halfCorpusZSize, Color.BLUE, PlayerId); Cube legs = new Cube(-halfLegSizeX + x, -halfLegSizeY + y, ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, 2 * halfLegSizeX, 2 * halfLegSizeY, legZSize, Color.MAGENTA, PlayerId); xx = -halfHandsXSize + x; yy = -halfHandsYSize + y;//- halfHandsYSize + (halfHandsYSize / 2.0) + y; r = rotatePoint(xx, yy, x - 0.5, y - halfHandsYSize, rot - Math.PI * 0.75); Cube handLeft = new Cube(r[0], r[1], ViewFrom[2] - handsZSize - headSize + halfNeckSize - epsBody, 1, halfHandsYSize * 2.0, handsZSize, Color.CYAN, PlayerId); xx = halfHandsXSize + x - 1; yy = -halfHandsYSize + y;//- halfHandsYSize + (halfHandsYSize / 2.0) + y; r = rotatePoint(xx, yy, x - 0.5, y - halfHandsYSize, rot - Math.PI * 0.75); Cube handRight = new Cube(r[0], r[1], ViewFrom[2] - handsZSize - headSize + halfNeckSize - epsBody, 1, halfHandsYSize * 2.0, handsZSize, Color.CYAN, PlayerId); xx = -2 + x; yy = -halfLegSizeY + y;//- halfHandsYSize + (halfHandsYSize / 2.0) + y; r = rotatePoint(xx, yy, x - 0.75, y - halfLegSizeY, rot - Math.PI * 0.75); Cube legLeft = new Cube(r[0], r[1], ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, 1.5, halfLegSizeY * 2.0, legZSize, Color.CYAN, PlayerId); xx = 2 + x - 1.5; yy = -halfLegSizeY + y;//- halfHandsYSize + (halfHandsYSize / 2.0) + y; r = rotatePoint(xx, yy, x - 0.75, y - halfLegSizeY, rot - Math.PI * 0.75); Cube legRight = new Cube(r[0], r[1], ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, 1.5, halfLegSizeY * 2.0, legZSize, Color.CYAN, PlayerId); //Cube legs = new Cube(- halfLegSizeX + x, - halfLegSizeY + y, ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, // 2 * halfLegSizeX, 2 * halfLegSizeY, legZSize, Color.MAGENTA, PlayerId); ArrayList<DPolygon> all = new ArrayList<DPolygon>(); //rot += 0.00; head.rotation = rot; head.setRotAdd(); head.updatePoly(); neck.rotation = rot; neck.updatePoly(); corpus.rotation = rot; corpus.updatePoly(); legs.rotation = rot; legs.updatePoly(); handLeft.rotation = rot; handLeft.updatePoly(); handRight.rotation = rot; handRight.updatePoly(); legLeft.rotation = rot; legLeft.updatePoly(); legRight.rotation = rot; legRight.updatePoly(); AddArrayToArrayList(all, head.GetPolygons()); AddArrayToArrayList(all, neck.GetPolygons()); AddArrayToArrayList(all, corpus.GetPolygons()); AddArrayToArrayList(all, handLeft.GetPolygons()); AddArrayToArrayList(all, handRight.GetPolygons()); AddArrayToArrayList(all, legLeft.GetPolygons()); AddArrayToArrayList(all, legRight.GetPolygons()); /* //rot = Math.PI*0.75; rot += 0.01; double xx = - halfHeadSize + x; double yy = - halfHeadSize + y; double[]r = rotatePoint(xx, yy, xx, yy, rot - Math.PI*0.75); Cube head = new Cube(r[0], r[1], ViewFrom[2] - halfHeadSize + epsBody, halfHeadSize * 2.0, halfHeadSize * 2.0, halfHeadSize * 2.0, Color.YELLOW, PlayerId, x, y, rot - Math.PI*0.75); head.Polys[2].DrawablePolygon.texture = GenerateTerrain.img; Cube neck = new Cube(- halfNeckSize + x, - halfNeckSize + y, ViewFrom[2] - headSize + halfNeckSize, halfNeckSize * 2.0, halfNeckSize * 2.0, halfNeckSize, Color.PINK, PlayerId, x, y, rot - Math.PI*0.75); Cube corpus = new Cube(- halfCorupsXSize + x, - halfCorupsYSize + y, ViewFrom[2] - corpusZSize - headSize + halfNeckSize - epsBody, halfCorupsXSize * 2.0, halfCorupsYSize * 2.0, 2 * halfCorpusZSize, Color.BLUE, PlayerId, x, y, rot - Math.PI*0.75); Cube legs = new Cube(- halfLegSizeX + x, - halfLegSizeY + y, ViewFrom[2] - legZSize - corpusZSize - headSize + halfNeckSize - epsBody - epsBody, 2 * halfLegSizeX, 2 * halfLegSizeY, legZSize, Color.MAGENTA, PlayerId, x, y, rot - Math.PI*0.75); //xx = - halfHandsXSize + 0.5 + x; //yy = - halfHandsYSize + (halfHandsYSize / 2.0) + y; Cube hands = new Cube(- halfHandsXSize + x, - halfHandsYSize + y, ViewFrom[2] - handsZSize - headSize + halfNeckSize - epsBody, 1, halfHandsYSize * 2.0, handsZSize, Color.CYAN, PlayerId, x, y, rot - Math.PI*0.75); ArrayList<DPolygon> all = new ArrayList<DPolygon>(); AddArrayToArrayList(all, head.GetPolygons()); AddArrayToArrayList(all, neck.GetPolygons()); AddArrayToArrayList(all, corpus.GetPolygons()); AddArrayToArrayList(all, hands.GetPolygons()); AddArrayToArrayList(all, legs.GetPolygons());*/ return all; }
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//w w w . j ava2 s . 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)); } }