List of usage examples for org.apache.commons.math.geometry Vector3D getY
public double getY()
From source file:magma.agent.behavior.complex.RunToBall.java
@Override public void perform(float intensity) { if (currentBehavior == null || currentBehavior.isFinished()) { double ballAngle = Math.toDegrees(worldModel.getBall().getHorizontalDirection()); logger.log(Level.FINE, "runToBall angle: {0}", ballAngle); if (ballAngle > 40) { currentBehavior = behaviors.get(IBehavior.TURN_LEFT_40); } else if (ballAngle < -40) { currentBehavior = behaviors.get(IBehavior.TURN_RIGHT_40); } else {// www.ja va 2s. c om currentBehavior = behaviors.get(IBehavior.WALK); } // Calculate whether the straight line between player and ball hits the // goal IThisPlayer thisPlayer = worldModel.getThisPlayer(); IMoveableObject ball = worldModel.getBall(); Vector3D ballPos = ball.getPosition(); Vector3D ownPos = thisPlayer.getPosition(); // double goalDistance = thisPlayer.getDistanceTo(worldModel // .getOtherGoalPosition()); // double ownGoalDistance = thisPlayer.getDistanceTo(worldModel // .getOwnGoalPosition()); Angle ballAngle1 = thisPlayer.getBodyDirectionTo(ballPos); // Angle goalAngle1 = thisPlayer.getBodyDirectionTo(worldModel // .getOtherGoalPosition()); // Angle delta = goalAngle1.subtract(ballAngle1); double ballAngle2 = ballAngle1.degrees(); // double goalAngle = goalAngle1.degrees(); // Calculate the position where the straight line through the player // and // the ball will cross the end of the field double m = 0; double b = 0; // Decide on which goal are we playing and calculate the straight line double x = worldModel.getOtherGoalPosition().getX(); if (x < 0) { // Playing to the left goal m = (ballPos.getY() - ownPos.getY()) / (ballPos.getX() - ownPos.getX()); b = ballPos.getY() - m * ownPos.getX(); } else { // Playing to the right goal m = (ownPos.getY() - ballPos.getY()) / (ownPos.getX() - ballPos.getX()); b = ownPos.getY() - m * ballPos.getX(); } // Calculate the Y point of the goal line where the ball will cross the // line if we perform a straight kick double y = m * x + b; // The straight line will not hit the goal logger.log(Level.FINE, "runToBall2 ballAngle2: {0} y:{1}", new Object[] { ballAngle2, y }); if (y > 0.7 && ballAngle2 < 15) { currentBehavior = behaviors.get(IBehavior.TURN_RIGHT_40); } else if (y < -0.7 && ballAngle2 > -15) { currentBehavior = behaviors.get(IBehavior.TURN_LEFT_40); } else { currentBehavior = behaviors.get(IBehavior.WALK); } currentBehavior.init(); } currentBehavior.perform(intensity); }
From source file:magma.agent.behavior.complex.ShootToGoal.java
/** * @return the appropriate behavior for kicking *//*from ww w . j a v a 2s . co m*/ private IBehavior getBehavior() { IBehavior result; IThisPlayer thisPlayer = worldModel.getThisPlayer(); IMoveableObject ball = worldModel.getBall(); Vector3D ballPos = ball.getPosition(); Vector3D ownPos = thisPlayer.getPosition(); double ownDistanceToBall = thisPlayer.getDistanceToXY(ballPos); double goalDistance = thisPlayer.getDistanceTo(worldModel.getOtherGoalPosition()); double ownGoalDistance = thisPlayer.getDistanceTo(worldModel.getOwnGoalPosition()); Angle ballAngle1 = thisPlayer.getBodyDirectionTo(ballPos); Angle goalAngle1 = thisPlayer.getBodyDirectionTo(worldModel.getOtherGoalPosition()); Angle delta = goalAngle1.subtract(ballAngle1); double ballAngle = ballAngle1.degrees(); double goalAngle = goalAngle1.degrees(); // Calculate the position where the straight line through the player and // the ball will cross the end of the field double m = 0; double b = 0; // Decide on which goal are we playing and calculate the straight line double x = worldModel.getOtherGoalPosition().getX(); if (x < 0) { // Playing to the left goal m = (ballPos.getY() - ownPos.getY()) / (ballPos.getX() - ownPos.getX()); b = ballPos.getY() - m * ownPos.getX(); } else { // Playing to the right goal m = (ownPos.getY() - ballPos.getY()) / (ownPos.getX() - ballPos.getX()); b = ownPos.getY() - m * ballPos.getX(); } // Calculate the Y point of the goal line where the ball will cross the // line if we perform a straight kick double y = m * x + b; // LOG logger.log(Level.FINE, "Kick: Distance: {0} BallAngle: {1} goalAngle: {2} " + "ball observed distance: {3} last seen: {4}", new Object[] { ownDistanceToBall, ballAngle, goalAngle, ball.getDistance(), ball.getLastSeenTime() }); logger.log(Level.FINE, "Kick: ballPos: {0};{1} playerPos: {2};{3} ", new Object[] { ballPos.getX(), ballPos.getY(), thisPlayer.getPosition().getX(), thisPlayer.getPosition().getY() }); // if (Math.abs(goalAngle) < 30) { // if your direction is pointed to the ball // and you are able to shoot between the two goalPost // then kick if ((goalDistance < 2 && y < 0.7 && y > -0.7 && Math.abs(ballAngle) < 45) || (goalDistance > 2 && ownGoalDistance > 3 && Math.abs(ballAngle) < 30 && Math.abs(goalAngle) < 30) || (ownGoalDistance < 3 && Math.abs(ballAngle) < 30 && Math.abs(goalAngle) < 80)) { // we are aligned to goal if (thisPlayer.isInsidePolygonXY(ballPos, getBothLegKickRectangle())) { // ball is kickable by both feet if (goalAngle < 0) { // the goal is to the right, kicking with left leg in this // area usually lets the ball drift right of the straight line // from robot through ball is of the left side of the goal center return behaviors.get(IBehavior.SHOOT_LEFT); } else { return behaviors.get(IBehavior.SHOOT_RIGHT); } // ball kickable by single leg? } else if (thisPlayer.isInsidePolygonXY(ballPos, getLeftLegKickRectangle())) { return behaviors.get(IBehavior.SHOOT_LEFT); } else if (thisPlayer.isInsidePolygonXY(ballPos, getRightLegKickRectangle())) { return behaviors.get(IBehavior.SHOOT_RIGHT); } else if (Math.abs(ballAngle) < 20) { // ball is ahead return behaviors.get(IBehavior.FORWARD); } else if (ballAngle > 50 && ballAngle < 90) { // ball is left and in front return behaviors.get(IBehavior.STEP_LEFT); } else if (ballAngle < -50 && ballAngle > -90) { // ball is right and in front return behaviors.get(IBehavior.STEP_RIGHT); } } // not facing to goal direction if (Math.abs(delta.degrees()) < 35) { // ball and goal direction are aligned /* * if (ballAngle > 70) { return * behaviors.get(IBehavior.SIDE_KICK_LEFT); } else if (ballAngle < -70) * { return behaviors.get(IBehavior.SIDE_KICK_RIGHT); } else */ // side kick possible ? if (thisPlayer.isInsidePolygonXY(ballPos, getLeftLegSideKickRectangle())) { return behaviors.get(IBehavior.SIDE_KICK_LEFT); } else if (thisPlayer.isInsidePolygonXY(ballPos, getRightLegSideKickRectangle())) { return behaviors.get(IBehavior.SIDE_KICK_RIGHT); // turn necessary? } else if ((result = getTurnBehavior(ballAngle)) != null) { return result; } else { return behaviors.get(IBehavior.FORWARD); } } else { // ball and goal are not aligned if (Math.abs(ballAngle) < 50) { // ball is in front: move round ball if (goalAngle < 0) { return behaviors.get(IBehavior.INWARD_TURN_LEFT45); } else { return behaviors.get(IBehavior.INWARD_TURN_RIGHT45); } } else if (goalAngle > 0 && delta.degrees() > 0) { // goal is further left than ball return behaviors.get(IBehavior.FORWARD); } else if (goalAngle < 0 && delta.degrees() < 0) { // goal is further right than ball return behaviors.get(IBehavior.FORWARD); } else { // ball angle is bigger than goal angle, so we turn return getTurnBehavior(ballAngle); } } }
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;/*from w w w. j a v a 2s .c om*/ switch (e) { 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)); }
From source file:uk.ac.imperial.presage2.util.location.Location.java
/** * Create a location from an existing {@link Vector3D}. * //from w ww . j a v a 2 s . co m * @param v */ public Location(Vector3D v) { super(v.getX(), v.getY(), v.getZ()); }
From source file:uk.ac.imperial.presage2.util.location.Move.java
/** * Create a move from an existing {@link Vector3D}. * * @param v */ public Move(Vector3D v) { super(v.getX(), v.getY(), v.getZ()); }