List of usage examples for org.apache.commons.math.geometry Vector3D subtract
public Vector3D subtract(Vector3D v)
From source file:magma.agent.behavior.complex.GetInScorePosition.java
private Vector3D calculateAlternativePosition(Vector3D ballPosition, IVisibleObject thisPlayer, Vector3D scorePosition) { Vector3D intermediatePosition = ballPosition.subtract(thisPlayer.getPosition()).normalize(); // find out which way is shorter Vector3D intermediate1 = new Vector3D(intermediatePosition.getY(), -intermediatePosition.getX(), intermediatePosition.getZ()); Vector3D intermediate2 = new Vector3D(-intermediatePosition.getY(), intermediatePosition.getX(), intermediatePosition.getZ()); intermediate1 = ballPosition.add(new Vector3D(BALL_KEEP_AWAY_DISTANCE, intermediate1)); intermediate2 = ballPosition.add(new Vector3D(BALL_KEEP_AWAY_DISTANCE, intermediate2)); float way1 = (float) (thisPlayer.getDistanceTo(intermediate1) + scorePosition.subtract(intermediate1).getNorm()); float way2 = (float) (thisPlayer.getDistanceTo(intermediate2) + scorePosition.subtract(intermediate2).getNorm()); if (way1 < way2) { scorePosition = intermediate1;//w ww . java 2s . c o m } else { scorePosition = intermediate2; } return (scorePosition); }
From source file:magma.agent.worldmodel.impl.VisibleObject.java
/** * Calculates the global absolute Direction of this visible object to the * passed Vector3D//w ww. java 2 s .c om * @param other the Vector3D to which to calculate direction * @return the global absolute direction (rad) of this visible object to the * passed Vector */ public Angle getDirectionTo(Vector3D other) { return Angle.rad(other.subtract(getPosition()).getAlpha()); }
From source file:magma.agent.believe.impl.BeamTime.java
@Override public float getTruthValue() { String playmode = worldModel.getPlaymode(); if (playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_GOAL_RIGHT) || playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_GOAL_LEFT) || playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_BEFORE_KICK_OFF)) { Vector3D position = worldModel.getThisPlayer().getPosition(); Vector3D homePosition = worldModel.getThisPlayer().getHomePosition(playmode); logger.log(Level.FINE, "position: ({0}, {1}) home position: ({2}, {3})", new Object[] { position.getX(), position.getY(), homePosition.getX(), homePosition.getY() }); double distance = position.subtract(homePosition).getNorm(); if (distance > 0.3) { return 1.0f; }//from w ww . ja v a 2 s. co m } return 0.0f; }
From source file:magma.agent.behavior.complex.GetToInitialPosition.java
/** * calculates a point on a circle from the own goal to which to go *//*from w w w .ja v a2 s .c om*/ 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:orchestration.path.RectShape.java
private static boolean testIntersection(Vector3D p, Vector3D r, Vector3D q, Vector3D s) { double bottomFactor = Vector3D.crossProduct(r, s).getNorm(); if (bottomFactor == 0) return false; // parallel else {//from w w w . j a va 2 s. c o m double t = Vector3D.crossProduct((q.subtract(p)), s).getNorm() / bottomFactor; double u = Vector3D.crossProduct((q.subtract(p)), r).getNorm() / bottomFactor; if (0 <= t && t <= 1 && 0 <= u && u <= 1) return true; else return false; } }
From source file:org.orekit.utils.Line.java
/** Get the abscissa of a point with respect to the line. * <p>The abscissa is 0 if the projection of the point and the * projection of the frame origin on the line are the same * point.</p>// ww w . ja va 2 s . c o m * @param point point to check * @return abscissa of the point */ public double getAbscissa(final Vector3D point) { return Vector3D.dotProduct(point.subtract(zero), direction); }
From source file:org.orekit.utils.Line.java
/** Compute the distance between the instance and a point. * @param p to check//from w w w. ja va 2 s .c om * @return distance between the instance and the point */ public double distance(final Vector3D p) { final Vector3D d = p.subtract(zero); return new Vector3D(1.0, d, -Vector3D.dotProduct(d, direction), direction).getNorm(); }
From source file:uk.ac.imperial.presage2.util.location.area.WrapEdgeHandler.java
@Override public Move getValidMove(final Location loc, Move m) { // get the move up to the edge of the area Move toEdge = super.getValidMove(loc, m); // get remaining move m = new Move(m.subtract(toEdge)); Vector3D edgePoint = loc.add(toEdge); Vector3D targetEdgePoint; switch (e) {/*from w w w . j av a2s .c o m*/ case X_MIN: targetEdgePoint = new Vector3D(x, edgePoint.getY(), edgePoint.getZ()); break; case Y_MIN: targetEdgePoint = new Vector3D(edgePoint.getX(), y, edgePoint.getZ()); break; case Z_MIN: targetEdgePoint = new Vector3D(edgePoint.getX(), edgePoint.getY(), z); break; case X_MAX: targetEdgePoint = new Vector3D(0, edgePoint.getY(), edgePoint.getZ()); break; case Y_MAX: targetEdgePoint = new Vector3D(edgePoint.getX(), 0, edgePoint.getZ()); break; case Z_MAX: targetEdgePoint = new Vector3D(edgePoint.getX(), edgePoint.getY(), 0); break; default: throw new RuntimeException("Edge not initialised!"); } // construct move return new Move(targetEdgePoint.subtract(loc).add(m)); }