List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D crossProduct
public static Vector3D crossProduct(final Vector3D v1, final Vector3D v2)
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 a v a 2 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)); } }
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 {// w w w.j a v a 2 s . c om 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.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; /*from ww w . j av a2 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:org.orekit.attitudes.CelestialBodyPointed.java
/** {@inheritDoc} */ public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException { final PVCoordinates satPV = pvProv.getPVCoordinates(date, celestialFrame); // compute celestial references at the specified date final PVCoordinates bodyPV = pointedBody.getPVCoordinates(date, celestialFrame); final PVCoordinates pointing = new PVCoordinates(satPV, bodyPV); final Vector3D pointingP = pointing.getPosition(); final double r2 = Vector3D.dotProduct(pointingP, pointingP); // evaluate instant rotation axis due to sat and body motion only (no phasing yet) final Vector3D rotAxisCel = new Vector3D(1 / r2, Vector3D.crossProduct(pointingP, pointing.getVelocity())); // fix instant rotation to take phasing constraint into account // (adding a rotation around pointing axis ensuring the motion of the phasing axis // is constrained in the pointing-phasing plane) final Vector3D v1 = Vector3D.crossProduct(rotAxisCel, phasingCel); final Vector3D v2 = Vector3D.crossProduct(pointingP, phasingCel); final double compensation = -Vector3D.dotProduct(v1, v2) / v2.getNormSq(); final Vector3D phasedRotAxisCel = new Vector3D(1.0, rotAxisCel, compensation, pointingP); // compute transform from celestial frame to satellite frame final Rotation celToSatRotation = new Rotation(pointingP, phasingCel, pointingSat, phasingSat); // build transform combining rotation and instant rotation axis Transform transform = new Transform(date, celToSatRotation, celToSatRotation.applyTo(phasedRotAxisCel)); if (frame != celestialFrame) { // prepend transform from specified frame to celestial frame transform = new Transform(date, frame.getTransformTo(celestialFrame, date), transform); }/*w ww . java 2s .c o m*/ // build the attitude return new Attitude(date, frame, transform.getRotation(), transform.getRotationRate(), transform.getRotationAcceleration()); }
From source file:org.orekit.attitudes.CelestialBodyPointingTest.java
@Test public void testSunPointing() throws OrekitException { PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); final Frame frame = FramesFactory.getGCRF(); AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01), new TimeComponents(3, 25, 45.6789), TimeScalesFactory.getTAI()); AttitudeProvider sunPointing = new CelestialBodyPointed(frame, sun, Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_K);/* w w w . j av a2 s .co m*/ PVCoordinates pv = new PVCoordinates(new Vector3D(28812595.32120171334, 5948437.45881852374, 0.0), new Vector3D(0, 0, 3680.853673522056)); Orbit orbit = new KeplerianOrbit(pv, frame, date, 3.986004415e14); Attitude attitude = sunPointing.getAttitude(orbit, date, frame); Vector3D xDirection = attitude.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D zDirection = attitude.getRotation().applyInverseTo(Vector3D.PLUS_K); Assert.assertEquals(0, Vector3D.dotProduct(zDirection, Vector3D.crossProduct(xDirection, Vector3D.PLUS_K)), 1.0e-15); // the following statement checks we take parallax into account // Sun-Earth-Sat are in quadrature, with distance (Earth, Sat) == distance(Sun, Earth) / 5000 Assert.assertEquals(FastMath.atan(1.0 / 5000.0), Vector3D.angle(xDirection, sun.getPVCoordinates(date, frame).getPosition()), 1.0e-15); double h = 0.1; Attitude aMinus = sunPointing.getAttitude(orbit.shiftedBy(-h), date.shiftedBy(-h), frame); Attitude a0 = sunPointing.getAttitude(orbit, date, frame); Attitude aPlus = sunPointing.getAttitude(orbit.shiftedBy(h), date.shiftedBy(h), frame); // check spin is consistent with attitude evolution double errorAngleMinus = Rotation.distance(aMinus.shiftedBy(h).getRotation(), a0.getRotation()); double evolutionAngleMinus = Rotation.distance(aMinus.getRotation(), a0.getRotation()); Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus); double errorAnglePlus = Rotation.distance(a0.getRotation(), aPlus.shiftedBy(-h).getRotation()); double evolutionAnglePlus = Rotation.distance(a0.getRotation(), aPlus.getRotation()); Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus); }
From source file:org.orekit.attitudes.LofOffsetTest.java
private void checkSatVector(Orbit o, Attitude a, Vector3D satVector, double expectedX, double expectedY, double expectedZ, double threshold) { Vector3D zLof = o.getPVCoordinates().getPosition().normalize().negate(); Vector3D yLof = o.getPVCoordinates().getMomentum().normalize().negate(); Vector3D xLof = Vector3D.crossProduct(yLof, zLof); Assert.assertTrue(Vector3D.dotProduct(xLof, o.getPVCoordinates().getVelocity()) > 0); Vector3D v = a.getRotation().applyInverseTo(satVector); Assert.assertEquals(expectedX, Vector3D.dotProduct(v, xLof), 1.0e-8); Assert.assertEquals(expectedY, Vector3D.dotProduct(v, yLof), 1.0e-8); Assert.assertEquals(expectedZ, Vector3D.dotProduct(v, zLof), 1.0e-8); }
From source file:org.orekit.attitudes.YawCompensation.java
/** {@inheritDoc} */ @Override//from w ww.j a v a2 s. co m public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException { final Transform bodyToRef = getBodyFrame().getTransformTo(frame, date); // compute sliding target ground point final PVCoordinates slidingRef = getTargetPV(pvProv, date, frame); final PVCoordinates slidingBody = bodyToRef.getInverse().transformPVCoordinates(slidingRef); // compute relative position of sliding ground point with respect to satellite final PVCoordinates relativePosition = new PVCoordinates(pvProv.getPVCoordinates(date, frame), slidingRef); // compute relative velocity of fixed ground point with respect to sliding ground point // the velocity part of the transformPVCoordinates for the sliding point ps // from body frame to reference frame is: // d(ps_ref)/dt = r(d(ps_body)/dt + dq/dt) - ps_ref // where r is the rotation part of the transform, is the corresponding // angular rate, and dq/dt is the derivative of the translation part of the // transform (the translation itself, without derivative, is hidden in the // ps_ref part in the cross product). // The sliding point ps is co-located to a fixed ground point pf (i.e. they have the // same position at time of computation), but this fixed point as zero velocity // with respect to the ground. So the velocity part of the transformPVCoordinates // for this fixed point can be computed using the same formula as above: // from body frame to reference frame is: // d(pf_ref)/dt = r(0 + dq/dt) - pf_ref // so remembering that the two points are at the same location at computation time, // i.e. that at t=0 pf_ref=ps_ref, the relative velocity between the fixed point // and the sliding point is given by the simple expression: // d(ps_ref)/dt - d(pf_ref)/dt = r(d(ps_body)/dt) // the acceleration is computed by differentiating the expression, which gives: // d(ps_ref)/dt - d(pf_ref)/dt = r(d(ps_body)/dt) - [d(ps_ref)/dt - d(pf_ref)/dt] final Vector3D v = bodyToRef.getRotation().applyTo(slidingBody.getVelocity()); final Vector3D a = new Vector3D(+1, bodyToRef.getRotation().applyTo(slidingBody.getAcceleration()), -1, Vector3D.crossProduct(bodyToRef.getRotationRate(), v)); final PVCoordinates relativeVelocity = new PVCoordinates(v, a, Vector3D.ZERO); final PVCoordinates relativeNormal = PVCoordinates.crossProduct(relativePosition, relativeVelocity) .normalize(); // attitude definition : // . Z satellite axis points to sliding target // . target relative velocity is in (Z,X) plane, in the -X half plane part return new Attitude(frame, new TimeStampedAngularCoordinates(date, relativePosition.normalize(), relativeNormal.normalize(), PLUS_K, PLUS_J, 1.0e-9)); }
From source file:org.orekit.attitudes.YawSteering.java
/** Creates a new instance. * @param groundPointingLaw ground pointing attitude provider without yaw compensation * @param sun sun motion model//from w ww.jav a2 s .co m * @param phasingAxis satellite axis that must be roughly in Sun direction * (if solar arrays rotation axis is Y, then this axis should be either +X or -X) * @deprecated as of 7.1, replaced with {@link #YawSteering(Frame, GroundPointing, PVCoordinatesProvider, Vector3D)} */ @Deprecated public YawSteering(final GroundPointing groundPointingLaw, final PVCoordinatesProvider sun, final Vector3D phasingAxis) { super(groundPointingLaw.getBodyFrame()); this.groundPointingLaw = groundPointingLaw; this.sun = sun; this.phasingNormal = new PVCoordinates(Vector3D.crossProduct(Vector3D.PLUS_K, phasingAxis).normalize(), Vector3D.ZERO, Vector3D.ZERO); }
From source file:org.orekit.attitudes.YawSteering.java
/** Creates a new instance. * @param inertialFrame frame in which orbital velocities are computed * @param groundPointingLaw ground pointing attitude provider without yaw compensation * @param sun sun motion model/*www . j av a 2s . co m*/ * @param phasingAxis satellite axis that must be roughly in Sun direction * (if solar arrays rotation axis is Y, then this axis should be either +X or -X) * @exception OrekitException if the frame specified is not a pseudo-inertial frame * @since 7.1 */ public YawSteering(final Frame inertialFrame, final GroundPointing groundPointingLaw, final PVCoordinatesProvider sun, final Vector3D phasingAxis) throws OrekitException { super(inertialFrame, groundPointingLaw.getBodyFrame()); this.groundPointingLaw = groundPointingLaw; this.sun = sun; this.phasingNormal = new PVCoordinates(Vector3D.crossProduct(Vector3D.PLUS_K, phasingAxis).normalize(), Vector3D.ZERO, Vector3D.ZERO); }
From source file:org.orekit.bodies.Ellipsoid.java
/** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane. * @param planePoint point belonging to the plane, in the ellipsoid frame * @param planeNormal normal of the plane, in the ellipsoid frame * @return plane section or null if there are no intersections *///from w w w.ja v a 2s. c om public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal) { // we define the points Q in the plane using two free variables and as: // Q = P + u + v // where u and v are two unit vectors belonging to the plane // Q belongs to the 3D ellipsoid so: // (xQ / a) + (yQ / b) + (zQ / c) = 1 // combining both equations, we get: // (xP + 2 xP ( xU + xV) + ( xU + xV)) / a // + (yP + 2 yP ( yU + yV) + ( yU + yV)) / b // + (zP + 2 zP ( zU + zV) + ( zU + zV)) / c // = 1 // which can be rewritten: // + + 2 + 2 + 2 + = 0 // with // = xU / a + yU / b + zU / c > 0 // = xV / a + yV / b + zV / c > 0 // = xU xV / a + yU yV / b + zU zV / c // = xP xU / a + yP yU / b + zP zU / c // = xP xV / a + yP yV / b + zP zV / c // = xP / a + yP / b + zP / c - 1 // this is the equation of a conic (here an ellipse) // Of course, we note that if the point P belongs to the ellipsoid // then = 0 and the equation holds at point P since = 0 and = 0 final Vector3D u = planeNormal.orthogonal(); final Vector3D v = Vector3D.crossProduct(planeNormal, u).normalize(); final double xUOa = u.getX() / a; final double yUOb = u.getY() / b; final double zUOc = u.getZ() / c; final double xVOa = v.getX() / a; final double yVOb = v.getY() / b; final double zVOc = v.getZ() / c; final double xPOa = planePoint.getX() / a; final double yPOb = planePoint.getY() / b; final double zPOc = planePoint.getZ() / c; final double alpha = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc; final double beta = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc; final double gamma = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc); final double delta = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc); final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc); final double zeta = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1); // reduce the general equation + + 2 + 2 + 2 + = 0 // to canonical form (/l) + (/m) = 1 // using a coordinates change // = C + cos - sin // = C + sin + cos // or equivalently // = ( - C) cos + ( - C) sin // = - ( - C) sin + ( - C) cos // C and C are the coordinates of the 2D ellipse center with respect to P // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest) // is the angle of the 2D ellipse axis corresponding to axis with length 2l // choose in order to cancel the coupling term in // expanding the general equation, we get: A + B + C + D + E + F = 0 // with C = 2[( - ) cos sin + (cos - sin)] // hence the term is cancelled when = arctan(t), with t + ( - ) t - = 0 // As the solutions of the quadratic equation obey t?t = -1, they correspond to // angles in quadrature to each other. Selecting one solution or the other simply // exchanges the principal axes. As we don't care about which axis we want as the // first one, we select an arbitrary solution final double tanTheta; if (FastMath.abs(gamma) < Precision.SAFE_MIN) { tanTheta = 0.0; } else { final double bMA = beta - alpha; tanTheta = (bMA >= 0) ? (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) : (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))); } final double tan2 = tanTheta * tanTheta; final double cos2 = 1 / (1 + tan2); final double sin2 = tan2 * cos2; final double cosSin = tanTheta * cos2; final double cos = FastMath.sqrt(cos2); final double sin = tanTheta * cos; // choose C and C in order to cancel the linear terms in and // expanding the general equation, we get: A + B + C + D + E + F = 0 // with D = 2[ ( C + C + ) cos + ( C + C + ) sin] // E = 2[-( C + C + ) sin + ( C + C + ) cos] // can be eliminated by combining the equations // D cos - E sin = 2[ C + C + ] // E cos + D sin = 2[ C + C + ] // hence the terms D and E are both cancelled (regardless of ) when // C = ( - ) / ( - ) // C = ( - ) / ( - ) final double denom = MathArrays.linearCombination(gamma, gamma, -alpha, beta); final double tauC = MathArrays.linearCombination(beta, delta, -gamma, epsilon) / denom; final double nuC = MathArrays.linearCombination(alpha, epsilon, -gamma, delta) / denom; // compute l and m // expanding the general equation, we get: A + B + C + D + E + F = 0 // with A = cos + sin + 2 cos sin // B = sin + cos - 2 cos sin // F = C + C + 2 C C + 2 C + 2 C + // hence we compute directly l = (-F/A) and m = (-F/B) final double twogcs = 2 * gamma * cosSin; final double bigA = alpha * cos2 + beta * sin2 + twogcs; final double bigB = alpha * sin2 + beta * cos2 - twogcs; final double bigF = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC + (beta * nuC + 2 * epsilon) * nuC + zeta; final double l = FastMath.sqrt(-bigF / bigA); final double m = FastMath.sqrt(-bigF / bigB); if (Double.isNaN(l + m)) { // the plane does not intersect the ellipsoid return null; } if (l > m) { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(cos, u, sin, v), new Vector3D(-sin, u, cos, v), l, m, frame); } else { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(sin, u, -cos, v), new Vector3D(cos, u, sin, v), m, l, frame); } }