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

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

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

Introduction

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

Prototype

public double getY()

Source Link

Document

Get the ordinate of the vector.

Usage

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  ww  .  j a  va 2  s  . co  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.worldmodel.impl.MovableObject.java

public void calculateSpeed(float lastSeenTime, float time) {
    // need previousPosition, hence speed is calculated after update
    if (getPreviousPosition() != null) {
        Vector3D checkSpeed = new Vector3D();
        checkSpeed = getPosition().subtract(getPreviousPosition());
        if (time - lastSeenTime > 1)
            checkSpeed = new Vector3D(checkSpeed.getX() / (time - lastSeenTime),
                    checkSpeed.getY() / (time - lastSeenTime), checkSpeed.getZ() / (time - lastSeenTime));

        // we don't set speed for impossible values
        if (checkSpeed.getNorm() < getPossibleSpeed())
            speed = checkSpeed;//from  ww  w .  j a  v a2s  . c  om
    } else
        speed = new Vector3D(0.0, 0.0, 0.0);
}

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