List of usage examples for org.apache.commons.math.geometry Vector3D Vector3D
public Vector3D(double a, Vector3D u)
From source file:magma.agent.behavior.complex.GetToInitialPosition.java
/** * calculates a point on a circle from the own goal to which to go *//* w ww . ja v a2 s . co m*/ private boolean getCurrentBehavior() { IThisPlayer thisPlayer = worldModel.getThisPlayer(); Vector3D home = thisPlayer.getHomePosition("doesNotMatter"); Vector3D ourPos = thisPlayer.getPosition(); Vector3D ownGoal = worldModel.getOwnGoalPosition(); Vector3D ball = worldModel.getBall().getPosition(); double distanceX = IServerConfigFilesConstants.FIELD_LENGTH - Math.abs(home.getX()); // destination position is certain distance from goal in line with ball Vector3D goalBallDir = new Vector3D(distanceX, ball.subtract(ownGoal).normalize()); Vector3D goalBall = ownGoal.add(goalBallDir); setPosition((float) goalBall.getX(), (float) goalBall.getY(), (float) goalBall.getAlpha()); double directionToTarget = thisPlayer.getBodyDirectionTo(position).degrees(); rotation = (float) directionToTarget; float distanceToTarget = (float) thisPlayer.getDistanceToXY(position); if (distanceToTarget < DISTANCE_PRECISION) { directionToTarget = thisPlayer.getBodyDirectionTo(ball).degrees(); rotation = (float) directionToTarget; } logger.log(Level.FINE, "initial pos: ({0},{1}) distanceToTarget: {2} directionToTarget: {3} " + "ourpos: ({4},{5})", new Object[] { goalBall.getX(), goalBall.getY(), distanceToTarget, directionToTarget, ourPos.getX(), ourPos.getY() }); if (distanceToTarget < DISTANCE_PRECISION) { // we are at the desired position // make sure that we are also in desired direction if (!turnedLeftBefore && directionToTarget > ANGULAR_PRECISION && directionToTarget <= 90) { turnLeft(directionToTarget); return true; } if (!turnedLeftBefore && directionToTarget < -ANGULAR_PRECISION && directionToTarget >= -90) { turnRight(directionToTarget - 180); return true; } if (!turnedLeftBefore && directionToTarget > 90 && directionToTarget < (180 - ANGULAR_PRECISION)) { turnRight(directionToTarget - 180); return true; } if (!turnedRightBefore && directionToTarget < -90 && directionToTarget > -180 + ANGULAR_PRECISION) { turnLeft(180 + directionToTarget); return true; } } if (directionToTarget > 50 && directionToTarget < 130) { // position is left currentBehavior = behaviors.get(IBehavior.STEP_LEFT); return true; } if (directionToTarget < -50 && directionToTarget > -130) { // position is right currentBehavior = behaviors.get(IBehavior.STEP_RIGHT); return true; } if (directionToTarget > 90 || directionToTarget < -90) { // or do we prefer to turn if (!turnedLeftBefore && directionToTarget > 0 && directionToTarget < (180 - ANGULAR_PRECISION)) { turnRight(directionToTarget - 180); return true; } if (!turnedRightBefore && directionToTarget < -ANGULAR_PRECISION) { turnLeft(180 + directionToTarget); return true; } currentBehavior = behaviors.get(IBehavior.STEP_BACKWARD); return true; } return false; }
From source file:magma.agent.perception.impl.ServerMessageParser.java
/** * Parse object coordinates/*from w w w. j ava 2s. c o m*/ * * @param node * @return * @throws PerceptorConversionException */ private Vector3D parsePol(SymbolNode node) throws PerceptorConversionException { try { assert node.childCount() == 4 : "Malformed node: " + node.toString(); assert node.getChild(0).content().equals("pol") : "Expecting a pol object: " + node.toString(); float val1 = Float.parseFloat(node.getChild(1).content()); float val2 = Float.parseFloat(node.getChild(2).content()); float val3 = Float.parseFloat(node.getChild(3).content()); return new Vector3D(val1, new Vector3D(Math.toRadians(val2), Math.toRadians(val3))); } catch (IndexOutOfBoundsException e) { throw new PerceptorConversionException("Malformed node: " + node.toString()); } }
From source file:org.orekit.frames.LocalOrbitalFrame.java
/** {@inheritDoc} */ protected void updateFrame(final AbsoluteDate date) throws OrekitException { // get position/velocity with respect to parent frame final PVCoordinates pv = provider.getPVCoordinates(date, getParent()); final Vector3D p = pv.getPosition(); final Vector3D v = pv.getVelocity(); final Vector3D momentum = pv.getMomentum(); // compute the translation part of the transform final Transform translation = new Transform(p.negate(), v.negate()); // compute the rotation part of the transform final Rotation r = new Rotation((type == LOFType.TNW) ? v : p, momentum, Vector3D.PLUS_I, Vector3D.PLUS_K); final Transform rotation = new Transform(r, new Vector3D(1.0 / p.getNormSq(), r.applyTo(momentum))); // update the frame defining transform setTransform(new Transform(translation, rotation)); }
From source file:org.orekit.frames.PEFFrame.java
/** Update the frame to the given date. * <p>The update considers the earth rotation from IERS data.</p> * @param date new value of the date//from w w w.j ava2 s .c o m * @exception OrekitException if the nutation model data embedded in the * library cannot be read */ protected void updateFrame(final AbsoluteDate date) throws OrekitException { if ((cachedDate == null) || !cachedDate.equals(date)) { // offset from J2000.0 epoch final double tts = date.durationFrom(AbsoluteDate.J2000_EPOCH); // evaluate the nutation elements setInterpolatedNutationElements(tts); // offset from J2000 epoch in julian centuries final double ttc = tts / Constants.JULIAN_CENTURY; // compute the mean obliquity of the ecliptic moe = ((MOE_3 * ttc + MOE_2) * ttc + MOE_1) * ttc + MOE_0; final double eqe = getNewEquationOfEquinoxes(date, ttc); // offset in julian centuries from J2000 epoch (UT1 scale) final double dtai = date.durationFrom(GMST_REFERENCE); final double dutc = TimeScalesFactory.getUTC().offsetFromTAI(date); final double dut1 = eopHistory.getUT1MinusUTC(date); final double tut1 = dtai + dutc + dut1; final double tt = tut1 / Constants.JULIAN_CENTURY; // Seconds in the day, adjusted by 12 hours because the // UT1 is supplied as a Julian date beginning at noon. final double sd = (tut1 + Constants.JULIAN_DAY / 2.) % Constants.JULIAN_DAY; // compute Greenwich mean sidereal time, in radians final double gmst = (((GMST_3 * tt + GMST_2) * tt + GMST_1) * tt + GMST_0 + sd) * RADIANS_PER_SECOND; // compute Greenwich apparent sidereal time, in radians final double gast = gmst + eqe; // compute true angular rotation of Earth, in rad/s final double lod = ((TEMEFrame) getParent()).isEOPCorrectionApplied() ? eopHistory.getLOD(date) : 0.0; final double omp = AVE * (1 - lod / Constants.JULIAN_DAY); final Vector3D rotationRate = new Vector3D(omp, Vector3D.PLUS_K); // set up the transform from parent TEME setTransform(new Transform(new Rotation(Vector3D.PLUS_K, -gast), rotationRate)); cachedDate = date; } }
From source file:org.orekit.frames.TIRF2000Frame.java
/** Update the frame to the given date. * <p>The update considers the earth rotation from IERS data.</p> * @param date new value of the date//from w w w .j av a 2 s . com * @exception OrekitException if the nutation model data embedded in the * library cannot be read */ protected void updateFrame(final AbsoluteDate date) throws OrekitException { if ((cachedDate == null) || !cachedDate.equals(date)) { // compute Earth Rotation Angle using Nicole Capitaine model (2000) final double tidalDtu1 = (tidalCorrection == null) ? 0 : tidalCorrection.getDUT1(date); final double dtu1 = eopHistory.getUT1MinusUTC(date); final double utcMinusTai = TimeScalesFactory.getUTC().offsetFromTAI(date); final double tu = (date.durationFrom(ERA_REFERENCE) + utcMinusTai + dtu1 + tidalDtu1) / Constants.JULIAN_DAY; era = ERA_0 + ERA_1A * tu + ERA_1B * tu; era -= TWO_PI * Math.floor((era + Math.PI) / TWO_PI); // set up the transform from parent CIRF final Vector3D rotationRate = new Vector3D((ERA_1A + ERA_1B) / Constants.JULIAN_DAY, Vector3D.PLUS_K); setTransform(new Transform(new Rotation(Vector3D.PLUS_K, -era), rotationRate)); cachedDate = date; } }
From source file:org.orekit.frames.VEISFrame.java
/** Update the frame to the given date. * @param date new value of the date/*from w w w .j a v a 2 s.com*/ * @exception OrekitException if data embedded in the library cannot be read */ protected void updateFrame(final AbsoluteDate date) throws OrekitException { if ((cachedDate == null) || !cachedDate.equals(date)) { // offset from FIFTIES epoch (UT1 scale) final double dtai = date.durationFrom(VST_REFERENCE); final double dutc = TimeScalesFactory.getUTC().offsetFromTAI(date); final double dut1 = eopHistory.getUT1MinusUTC(date); final double tut1 = dtai + dutc + dut1; final double ttd = tut1 / Constants.JULIAN_DAY; final double rdtt = ttd - (int) ttd; // compute Veis sidereal time, in radians final double vst = (VST0 + VST1 * ttd + TWO_PI * rdtt) % TWO_PI; // compute angular rotation of Earth, in rad/s final Vector3D rotationRate = new Vector3D(-VSTD, Vector3D.PLUS_K); // set up the transform from parent PEF setTransform(new Transform(new Rotation(Vector3D.PLUS_K, vst), rotationRate)); cachedDate = date; } }
From source file:org.orekit.propagation.Ephemeris.java
/** Get the interpolated Attitude. * @param date interpolation date//from w ww . j a v a 2 s .c o m * @param tp time in seconds since previous date * @param tn time in seconds until next date * @return the new attitude kinematics */ private Attitude interpolatedAttitude(final AbsoluteDate date, final double tp, final double tn) { final double dt = tp + tn; final Transform prevToNext = new Transform(new Transform(previous.getAttitude().getRotation().revert()), new Transform(next.getAttitude().getRotation())); final Rotation newRot = new Rotation(prevToNext.getRotation().getAxis(), tp * prevToNext.getRotation().getAngle() / dt); Vector3D newInstRotAxis; if (prevToNext.getRotationRate().getNorm() != 0) { newInstRotAxis = new Vector3D(tp * prevToNext.getRotationRate().getNorm() / dt, prevToNext.getRotationRate().normalize()); } else { newInstRotAxis = Vector3D.ZERO; } final Transform newTrans = new Transform(new Transform(previous.getAttitude().getRotation()), new Transform(newRot, newInstRotAxis)); return new Attitude(date, previous.getFrame(), newTrans.getRotation(), newTrans.getRotationRate()); }