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

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

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

Introduction

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

Prototype

public double getZ()

Source Link

Document

Get the height of the vector.

Usage

From source file:at.tuwien.ifs.commons.util.MathUtils.java

/**
 * Squared euclidean distance in 3D space
 * NOTE: apache commons math3 has this method already. But for simplicity I implemented it here.
 *       remote it in the future/* w ww . j a  v  a  2  s .c  o  m*/
 * @param v from
 * @param u to
 * @return the distance^2
 */
public static double euclidean2(Vector3D v, Vector3D u) {
    double x = (v.getX() - u.getX());
    double y = (v.getY() - u.getY());
    double z = (v.getZ() - u.getZ());
    return (x * x + y * y + z * z);
}

From source file:magma.util.FuzzyCompare.java

/**
 * Equal ( == )/*  ww w. jav  a 2 s  .  co  m*/
 * 
 * @param first first vector to compare
 * @param second second vector to compare
 * @param range The range within the vector values are equal
 * @return true if the both values are within the same range
 */
public static boolean eq(Vector3D first, Vector3D second, float range) {
    if (first == null && second == null) {
        return true;
    }
    if (first == null || second == null) {
        return false;
    }

    if (!eq((float) first.getX(), (float) second.getX(), range)) {
        return false;
    }
    if (!eq((float) first.getY(), (float) second.getY(), range)) {
        return false;
    }
    if (!eq((float) first.getZ(), (float) second.getZ(), range)) {
        return false;
    }
    return true;
}

From source file:magma.agent.believe.impl.LayBase.java

protected boolean laying() {
    Vector3D gyro = agent.getGyroRate("TorsoGyro").getTranslation();
    if (gyro.getZ() < 0.5) {
        return true;
    }/* w  w w.java2 s  .  c  o  m*/
    return false;
}

From source file:magma.agent.behavior.basic.BeamHomeBehavior.java

@Override
public void perform(float intensity) {
    // the position is based on if we play left to right or right to left
    Vector3D position = worldModel.getThisPlayer().getHomePosition(worldModel.getPlaymode());
    // the z component is the rotation of the player
    setPos((float) position.getX(), (float) position.getY(), (float) position.getZ());

    super.perform(1.0f);

    // after beaming we init gyro
    agentModel.getGyroRate(IServerConfigFilesConstants.GYRORATE_NAMES[0]).initialize();
    logger.log(Level.FINER, "init gyro");
}

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

public void setGyro(Vector3D gyro) {
    this.gyro = gyro;

    double x, y, z;

    x = Math.toRadians(gyro.getX() * 0.02);
    y = Math.toRadians(gyro.getY() * 0.02);
    z = Math.toRadians(gyro.getZ() * 0.02);

    double[] rotationValues = new double[9];

    rotationValues[0] = cos(y) * cos(z) + sin(x) * sin(y) * sin(z);
    rotationValues[1] = sin(z) * cos(x);
    rotationValues[2] = -sin(y) * cos(z) + sin(x) * cos(y) * sin(z);
    rotationValues[3] = -cos(y) * sin(z) + sin(x) * sin(y) * cos(z);
    rotationValues[4] = cos(x) * cos(z);
    rotationValues[5] = sin(z) * sin(z) + sin(x) * cos(y) * cos(z);
    rotationValues[6] = cos(x) * sin(y);
    rotationValues[7] = -sin(x);// w  w w.  j a v  a  2 s  .co  m
    rotationValues[8] = cos(x) * cos(y);

    Matrix3d M = new Matrix3d(rotationValues);

    M.mul(identity);

    // Error correction
    // N = M / sqrt(MT x M)
    // N - closest rotation result without errors
    // M - with errors
    // MT - transpose of M
    // sqrt of matrix - done by Cholesky Algorithm in MatrixUtil.java

    Matrix3d MT = new Matrix3d(M.m00, M.m10, M.m20, M.m01, M.m11, M.m21, M.m02, M.m12, M.m22);

    MT.mul(M);

    // call cholesky for sqrt(matrix)
    Matrix3d sqrt = MatrixUtil.cholesky(MT);

    sqrt.invert();
    M.mul(sqrt);

    // setting identity as result of error correction
    identity = M;

    logger.log(Level.FINER, "gyro update: ({0}, {1}, {2})",
            new Object[] { identity.m02, identity.m12, identity.m22 });
}

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. j  av a 2  s  .co m
    } else {
        scorePosition = intermediate2;
    }
    return (scorePosition);
}

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  av  a  2s .c om
    } else
        speed = new Vector3D(0.0, 0.0, 0.0);
}

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   ww w  .  ja v a 2 s.co 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.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 va2 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
 */// w  w w .j a  va  2 s.c  om
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()));
    }
}