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

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;//from  w ww. ja va2s  . c  om
    } else {
        scorePosition = intermediate2;
    }
    return (scorePosition);
}

From source file:magma.agent.worldmodel.impl.VisibleObject.java

/**
 * @param other the point to which to calculate
 * @return the distance projection to the x y plane between this object and
 *         the passed coordinate//from   w w w  .j a v  a  2s  .c o m
 */
public double getDistanceToXY(Vector3D other) {
    Vector3D delta = getPosition().subtract(other);
    return new Vector3D(delta.getX(), delta.getY(), 0).getNorm();
}

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

@Override
public void perform(float intensity) {
    if (currentBehavior == null || currentBehavior.isFinished()) {

        side = worldModel.getThisPlayer().getSide();
        rightSide = side == IMagmaConstants.RIGHT_SIDE;
        leftSide = side == IMagmaConstants.LEFT_SIDE;
        ballPosition = worldModel.getBall().getPosition();
        zAngle = 0.0;//  w w  w . j  a  va  2  s  .co m

        Vector3D newPosition = calculateNewXY();
        if (rightSide)// invert values for playing from right side
        {
            newPosition = new Vector3D(-newPosition.getX(), -newPosition.getY(), 0);
            zAngle = 180.0;
        }

        if (!checkAngle(zAngle)) { // player is facing somewhere other than
            // opponent's goal
            turnPlayer(zAngle);
        }

        else if (thisPlayer.getPosition().getY() - newPosition.getY() < -0.3) {
            // move left
            currentBehavior = behaviors.get(STEP_LEFT);
        }

        else if (thisPlayer.getPosition().getY() - newPosition.getY() > 0.3) {
            // move right
            currentBehavior = behaviors.get(STEP_RIGHT);
        }

        else if (thisPlayer.getPosition().getX() - newPosition.getX() < -1.5) {
            // move forward
            currentBehavior = behaviors.get(STEP_FORWARD);
        }

        else if (thisPlayer.getPosition().getX() - newPosition.getX() > 1.5) {
            // move backward
            currentBehavior = behaviors.get(STEP_BACKWARD);
        }

        else
            currentBehavior = behaviors.get(NONE);

        currentBehavior.init();
    }
    currentBehavior.perform(intensity);
}

From source file:magma.agent.perception.impl.ServerMessageParserTest.java

@Test
public void testGyroPerceptor() {
    String msg = "(GYR (n torso) (rt 0.01 0.07 0.46))";

    try {/*from w w  w  . jav  a  2  s .c o  m*/
        List<Perceptor> list = testee.parseString(msg);

        assertEquals(list.size(), 1);

        Perceptor perceptor = list.get(0);
        assertTrue(perceptor instanceof GyroPerceptor);

        GyroPerceptor gyro = (GyroPerceptor) perceptor;

        assertEquals(gyro.getName(), "torso");

        Vector3D rotation = gyro.getGyro();
        assertEquals(rotation.getX(), 0.01f, 0.0001);
        assertEquals(rotation.getY(), 0.07f, 0.0001);
        assertEquals(rotation.getZ(), 0.46f, 0.0001);
    } catch (Exception e) {
        fail("No exception expected");
    }
}

From source file:magma.agent.perception.impl.ServerMessageParserTest.java

@Test
public void testForceResistancePerceptor() {
    String msg = "(FRP (n lf) (c -0.14 0.08 -0.05) (f 1.12 -0.26 13.07))";

    try {/*w ww  .j a v a2  s  .  c  o  m*/
        List<Perceptor> list = testee.parseString(msg);

        assertEquals(list.size(), 1);

        Perceptor perceptor = list.get(0);
        assertTrue(perceptor instanceof ForceResistancePerceptor);

        ForceResistancePerceptor frp = (ForceResistancePerceptor) perceptor;

        assertEquals(frp.getName(), "lf");

        Vector3D origin = frp.getForceOrigin();
        Vector3D force = frp.getForce();

        assertEquals(origin.getX(), -0.14f, 0.0001);
        assertEquals(origin.getY(), 0.08f, 0.0001);
        assertEquals(origin.getZ(), -0.05f, 0.0001);
        assertEquals(force.getX(), 1.12f, 0.0001);
        assertEquals(force.getY(), -0.26f, 0.0001);
        assertEquals(force.getZ(), 13.07f, 0.0001);
    } catch (Exception e) {
        fail("No exception expected");
    }
}

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  . j  av 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:magma.agent.behavior.complex.GoalieBehavior.java

@Override
public void perform(float intensity) {
    if (currentBehavior == null || currentBehavior.isFinished()) {

        ballPosition = worldModel.getBall().getPosition();
        goalie = worldModel.getThisPlayer();
        leftSide = goalie.getSide() == IMagmaConstants.LEFT_SIDE;
        rightSide = goalie.getSide() == IMagmaConstants.RIGHT_SIDE;
        boolean none = false;

        float newY = calculateNewGoaliePos(ballPosition.getX(), ballPosition.getY());
        if (newY == 0.7f || newY == -0.7f)
            none = true;//  w w  w . j av  a  2s .  c  o  m
        zAngle = 0.0;
        if (rightSide)// inverting y values for right side.
        {
            newY = -newY;
            zAngle = 180.0;
        }
        Vector3D newPosition = new Vector3D(goalie.getPosition().getX(), newY, 0.0);

        // log
        logger.log(Level.FINE, "Goalie behavior. new position - x,y,z : ({0}, {1}, {2})",
                new Object[] { newPosition.getX(), newPosition.getY(), newPosition.getZ() });

        // ball kick-able and isSafeToKick(when kicked will not be self goal)
        if (isSafeToKick()) {
            // kick with right or left foot
            currentBehavior = behaviors.get(IBehavior.SHOOT_TO_GOAL);

        } else if (!checkAngle(zAngle)) { // player is facing somewhere other
            // than
            // opponent's goal
            turnPlayer(zAngle);
        } else if (Geometry.isInsidePolygon(ballPosition, getBallToBeKickedRectangle())) {
            currentBehavior = behaviors.get(IBehavior.GET_IN_SCORE_POSITION);
        } else if (!none && goalie.getPosition().getY() - newPosition.getY() < -0.2) {
            // move left
            currentBehavior = behaviors.get(STEP_LEFT);
        } else if (!none && goalie.getPosition().getY() - newPosition.getY() > 0.2) {
            // move right
            currentBehavior = behaviors.get(STEP_RIGHT);
        }
        // else
        // currentBehavior = behaviors.get(NONE);
        //
        // // distance to newPosition is greater than distance to ball
        // // TODO also check if opponent is closer than you
        // else if (FuzzyCompare.gt((float) worldModel.getThisPlayer()
        // .getDistanceTo(newPosition), (float) worldModel.getThisPlayer()
        // .getDistanceTo(worldModel.getBall()), 0.3f)) {
        // // run to ball
        // // TODO change run_to_ball to something else
        // System.out
        // .println("----------------------------------------********----------run to ball");
        // currentBehavior = behaviors.get(IBehavior.RUN_TO_BALL);
        // }
        // // distance to position smaller
        // else if (FuzzyCompare.lt((float) worldModel.getThisPlayer()
        // .getDistanceTo(newPosition), (float) worldModel.getThisPlayer()
        // .getDistanceTo(worldModel.getBall()), 0.3f)) {
        // // run to position
        // System.out
        // .println("----------------------------------------********----------run to p");
        // /** ====> */
        // System.out.println("my x=" + thisPlayer.getPosition().getX()
        // + " y=" + thisPlayer.getPosition().getY() + " z="
        // + thisPlayer.getPosition().getZ());
        //
        // System.out.println("x=" + newPosition.getX() + " y="
        // + newPosition.getY() + " z=" + newPosition.getZ());
        //
        // System.out.println("new ops="
        // + worldModel.getThisPlayer().getDistanceTo(newPosition)
        // + " ball="
        // + worldModel.getThisPlayer().getDistanceTo(
        // worldModel.getBall()));
        // currentBehavior = behaviors.get(IBehavior.RUN_TO_POSITION);
        // ((RunToPosition) currentBehavior).setPosition(newPosition,
        // calculateRot());
        //
        // }
        else {
            currentBehavior = behaviors.get(IBehavior.NONE);
        }
        currentBehavior.init();
    }
    currentBehavior.perform(intensity);

}

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

/**
 * updates all known GyroRates from perception
 * @param perception the result from server message parsing
 *//*  w w  w .  ja va2 s.co m*/
protected void updateGytoRate(IPerception perception) {
    Vector3D rate;

    for (int i = 0; i < IServerConfigFilesConstants.GYRORATE_NAMES.length; i++) {
        rate = perception.getGyroRatePerceptor(IServerConfigFilesConstants.GYRORATE_PERCEPTORS[i]).getGyro();
        getGyroRate(IServerConfigFilesConstants.GYRORATE_NAMES[i])
                .setGyro(new Vector3D(rate.getX(), rate.getY(), rate.getZ()));
    }
}

From source file:magma.agent.worldmodel.impl.VisibleObject.java

public Vector3D getGlobalFromLocalPosition(Vector3D localPos, Vector3D playerPosition, Angle horizontalAngle,
        Angle verticalAngle, double neckPitchAngle, double neckYawAngle) {
    // Rotation around z axis with angle1
    double angle1 = (float) -neckPitchAngle;
    double sinus = sin(angle1);
    double cosinus = cos(angle1);
    double newX = localPos.getX() * cosinus + localPos.getZ() * sinus;
    double newY = localPos.getY();
    double newZ = localPos.getX() * (-sinus) + localPos.getZ() * cosinus;

    // Rotation around z axis with angle2
    double angle2 = horizontalAngle.add(neckYawAngle).radians();
    sinus = sin(angle2);//from   w w w  .  ja v a2  s .c  o  m
    cosinus = cos(angle2);
    double newX2 = newX * cosinus - newY * sinus;
    double newY2 = newX * sinus + newY * cosinus;
    double newZ2 = newZ;

    // Rotation around x axis with angle b:
    // x' = x
    // y' = y * cos(b) - z * sin(b)
    // z' = y * sin(b) + z * cos(b)

    // translation
    return new Vector3D(newX2, newY2, newZ2).add(playerPosition);
}

From source file:magma.agent.worldmodel.impl.GlobalMap.java

@Override
public boolean isInOtherHalf(Vector3D position) {

    char side = getThisPlayer().getSide();
    double ballX = position.getX();

    return ((side == IMagmaConstants.LEFT_SIDE && ballX > 0)
            || (side == IMagmaConstants.RIGHT_SIDE && ballX < 0));
}