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

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

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

Introduction

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

Prototype

public Vector3D(double x, double y, double z)

Source Link

Document

Simple constructor.

Usage

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;/*from w  w w  .jav  a2 s.  com*/

        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.worldmodel.impl.MovableObjectTest.java

/**
 * Test method for/*from   w ww  .j  a v a  2s.c o  m*/
 * {@link magma.agent.worldmodel.impl.MovableObject#getSpeed()}.
 */
@Test
public void testGetSpeedForPlayers() {
    /** Tests for Players */

    VisibleObjectPerceptor vision = new VisibleObjectPerceptor("test", new Vector3D(2.0, 0.0, 0.0));

    // first-time perception, no prevPosition
    thisPlayer.setPosition(new Vector3D(1.0, 0.0, 0.0));
    testee.update(vision, 1.0f, thisPlayer);
    testVector(new Vector3D(0.0, 0.0, 0.0), testee.getSpeed());

    // speed < 2
    vision.setPosition(new Vector3D(3.0, 0.0, 0.0));
    testee.update(vision, 2.0f, thisPlayer);
    testVector(new Vector3D(1.0, 0.0, 0.0), testee.getSpeed());

    // speed > 2
    vision.setPosition(new Vector3D(6.0, 0.0, 0.0));
    testee.update(vision, 3.0f, thisPlayer);
    testVector(new Vector3D(1.0, 0.0, 0.0), testee.getSpeed());

    // speed < 2, time gap > 1
    vision.setPosition(new Vector3D(7.0, 0.0, 0.0));
    testee.update(vision, 5.0f, thisPlayer);
    testVector(new Vector3D(0.5, 0.0, 0.0), testee.getSpeed());
}

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

/**
 * Test method for//from   w w  w  .  ja  v  a2  s . com
 * {@link magma.agent.worldmodel.impl.VisibleObject#update(magma.agent.perception.impl.VisibleObjectPerceptor, float, ThisPlayer)}
 * .
 */
@Test
public void testUpdateZ() {
    VisibleObjectPerceptor vision = new VisibleObjectPerceptor("test", new Vector3D(0.0, 0.0, 2.0));
    thisPlayer.setPosition(new Vector3D(0.0, 0.0, 1.0));

    testee.update(vision, 1.0f, thisPlayer);
    Vector3D position = testee.getPosition();
    testVector(new Vector3D(0.0, 0.0, 3.0), position);
}

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

@Test
public void testPerformMoveAroundAnywhere() {
    thisPlayer.setPosition(new Vector3D(-2, -2.5, 0));
    expect(worldModelMock.getBall()).andReturn(ballMock);
    expect(worldModelMock.getThisPlayer()).andReturn(thisPlayer);
    expect(worldModelMock.getOtherGoalPosition()).andReturn(new Vector3D(6.0, 0.0, 0.0));
    Vector3D ballPosition = new Vector3D(-3, -1, 0.0);
    expect(ballMock.getPosition()).andReturn(ballPosition);
    // this is the expected position
    replay(worldModelMock, ballMock);//from www .  j  ava 2s.  com

    double[] result = testee.getPosition();

    assertEquals(-3.5824, result[0], 0.001);
    assertEquals(-1.3882, result[1], 0.001);
    assertEquals(0.11065, result[2], 0.001);
}

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

/**
 * Test method for//  ww w.j a  v  a2  s  .c o m
 * {@link magma.agent.worldmodel.impl.VisibleObject#update(magma.agent.perception.impl.VisibleObjectPerceptor, float, ThisPlayer)}
 * .
 */
@Test
public void testUpdateXYZ() {
    VisibleObjectPerceptor vision = new VisibleObjectPerceptor("test", new Vector3D(1.0, 2.0, 3.0));
    thisPlayer.setPosition(new Vector3D(4.0, -2.0, 1.0));

    testee.update(vision, 1.0f, thisPlayer);
    Vector3D position = testee.getPosition();
    testVector(new Vector3D(5.0, 0.0, 4.0), position);
}

From source file:magma.agent.worldmodel.localizer.impl.LocalizerTriangulationTest.java

@Test
public void testLocalizeMoreFlagsRight() {
    expect(F1R.getDistance()).andStubReturn(7.21110);
    expect(F1R.getHorizontalDirection()).andStubReturn(toRadians(33.6900));

    expect(F2R.getDistance()).andStubReturn(7.21110);
    expect(F2R.getHorizontalDirection()).andStubReturn(toRadians(-33.6900));

    expect(G1R.getDistance()).andStubReturn(6.22);
    expect(G1R.getHorizontalDirection()).andStubReturn(toRadians(15.94));

    expect(G2R.getDistance()).andStubReturn(6.22);
    expect(G2R.getHorizontalDirection()).andStubReturn(toRadians(-15.94));

    expect(F1R.compareTo((ILocalizationFlag) anyObject())).andStubReturn(-1);
    expect(G1R.compareTo(F1R)).andStubReturn(1);
    expect(G1R.compareTo(G2R)).andStubReturn(-1);
    expect(G1R.compareTo(F2R)).andStubReturn(-1);
    expect(G2R.compareTo(F1R)).andStubReturn(1);
    expect(G2R.compareTo(G1R)).andStubReturn(1);
    expect(G2R.compareTo(F2R)).andStubReturn(-1);
    expect(F2R.compareTo((ILocalizationFlag) anyObject())).andStubReturn(1);

    replay(F1R, F2R, G1R, G2R);//from  w  ww .  jav  a2s . c  o m

    // player is at position -4,-1 looking 45 deg
    flags.put("G1R", G1R);
    flags.put("G2R", G2R);
    flags.put("F1R", F1R);
    flags.put("F2R", F2R);

    PositionOrientation localizeInfo = testee.localize(flags, 0, 0, Vector3D.zero);
    assertNotNull(localizeInfo);
    assertTrue(FuzzyCompare.eq(new Vector3D(-0.149, -0.001, 0), localizeInfo.getPosition(), 0.01f));
    assertEquals(-1.592629, localizeInfo.getOrientationX().degrees(), 0.001);
}

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

public Vector3D getTranslation() {
    return new Vector3D(identity.m02, identity.m12, identity.m22);
}

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

/**
 * Test method for//  ww  w .  j  av a  2s .co m
 * {@link magma.agent.worldmodel.impl.MovableObject#getSpeed()}.
 */
@Test
public void testGetSpeedForBall() {

    /** Tests for Ball */
    testee = new Ball();

    VisibleObjectPerceptor vision = new VisibleObjectPerceptor("test", new Vector3D(2.0, 0.0, 0.0));
    // thisPlayer.setPosition(new Vector3D(0.0, 0.0, 0.0));

    // first-time perception, initial position 0,0
    testee.update(vision, 1.0f, thisPlayer);
    testVector(new Vector3D(2.0, 0.0, 0.0), testee.getSpeed());

    // speed < 6
    vision.setPosition(new Vector3D(3.0, 2.0, 0.0));
    testee.update(vision, 2.0f, thisPlayer);
    testVector(new Vector3D(1.0, 2.0, 0.0), testee.getSpeed());

    // speed > 6
    vision.setPosition(new Vector3D(6.0, -6.0, 0.0));
    testee.update(vision, 3.0f, thisPlayer);
    testVector(new Vector3D(1.0, 2.0, 0.0), testee.getSpeed());

    // speed < 6, time gap > 1
    vision.setPosition(new Vector3D(4.0, -4.0, 0.0));
    testee.update(vision, 5.0f, thisPlayer);
    testVector(new Vector3D(-1.0, 1.0, 0.0), testee.getSpeed());
}

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

private void setHomePosition() {

    if (side == ' ') {
        // we still have no side information from server
        return;/*ww w.  j  av a 2 s  .co m*/
    }

    int id = getID();
    float[] startPos = { 0.0f, 0.0f, 0.0f };
    if (id >= 0) {
        if (side == IMagmaConstants.LEFT_SIDE) {
            startPos = IMagmaConstants.startPositionsOwnKickoff[id];
        } else if (side == IMagmaConstants.RIGHT_SIDE) {
            startPos = IMagmaConstants.startPositionsOtherKickoff[id];
        }
    }
    homePosition = new Vector3D(startPos[0], startPos[1], startPos[2]);
}

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