Example usage for org.apache.commons.math.geometry Vector3D getX

List of usage examples for org.apache.commons.math.geometry Vector3D getX

  1. HOME
  2. Java
  3. org.apache.commons
  4. org.apache.commons.math.geometry.*
  5. Vector3D
  6. getX

Introduction

In this page you can find the example usage for org.apache.commons.math.geometry Vector3D getX.

Prototype

public double getX()

Source Link

Document

Get the abscissa of the vector.

Usage

From source file:magma.agent.behavior.complex.GetInScorePosition.java

double[] getPosition() {
    Vector3D ballPosition = worldModel.getBall().getPosition();
    IThisPlayer thisPlayer = worldModel.getThisPlayer();

    Vector3D shootVector = worldModel.getOtherGoalPosition().subtract(ballPosition);
    Vector3D scorePosition = ballPosition.add(-0.25, shootVector.normalize());

    if (thisPlayer.getDistanceTo(ballPosition) < thisPlayer.getDistanceTo(scorePosition)) {
        Angle delta1 = thisPlayer.getDirectionTo(ballPosition)
                .subtract(thisPlayer.getDirectionTo(scorePosition));
        double delta = Math.abs(delta1.degrees());

        if (delta < 90) {
            // We have to run around the ball
            scorePosition = calculateAlternativePosition(ballPosition, thisPlayer, scorePosition);
        } else if (delta < 140) {
            scorePosition = ballPosition.add(-BALL_KEEP_AWAY_DISTANCE, shootVector.normalize());
        }//from w w w  .j  a  v  a  2 s.c  o  m
    }
    return new double[] { scorePosition.getX(), scorePosition.getY(), shootVector.getAlpha() };
}

From source file:magma.agent.agentmodel.impl.AgentModel.java

/**
 * updates all known ForceResistances from perception
 * @param perception the result from server message parsing
 *//*from www  . j  a  v  a  2  s  . c o  m*/
protected void updateForceResistance(IPerception perception) {
    Vector3D force;
    Vector3D forceOrigin;

    for (int i = 0; i < IServerConfigFilesConstants.FORCERESISTANCE_NAMES.length; i++) {
        // Fetch new Force Values
        force = perception
                .getForceResistancePerceptor(IServerConfigFilesConstants.FORCERESISTANCE_PERCEPTORS[i])
                .getForce();
        forceOrigin = perception
                .getForceResistancePerceptor(IServerConfigFilesConstants.FORCERESISTANCE_PERCEPTORS[i])
                .getForceOrigin();

        // Set new Force Values
        getForceResistance(IServerConfigFilesConstants.FORCERESISTANCE_NAMES[i])
                .setForce(new Vector3D(force.getX(), force.getY(), force.getZ()));
        getForceResistance(IServerConfigFilesConstants.FORCERESISTANCE_NAMES[i])
                .setForceOrigin(new Vector3D(forceOrigin.getX(), forceOrigin.getY(), forceOrigin.getZ()));
    }
}

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 {//from   w  w  w  .j a  va2 s  . co m
            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
 *///ww w  . ja v a2  s .  c  om
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  av a2s .  c o m
    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   www . 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());
}