Package sk.fiit.jim.math

Examples of sk.fiit.jim.math.Vector3D

30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
  }
 
  public void updatePosition(ParsedData data){
    if(data.fixedObjects == null || data.fixedObjects.size() < 1)
      return;
    Vector3D accumulator = Vector3D.cartesian(0.0, 0.0, 0.0);
   
    for(Entry<FixedObject, Vector3D> flag : data.fixedObjects.entrySet()){
      Vector3D absolute = flag.getKey().getAbsolutePosition();
      Vector3D seen = flag.getValue();
      //the order of rotations is SIGNIFICANT! This order is the most suitable for a standing agent, it differs the least from the real position
      Vector3D ourPosition = seen.rotateOverY(agent.rotationY).rotateOverZ(agent.rotationZ).rotateOverX(agent.rotationX).negate();
      ourPosition = ourPosition.add(absolute);
      accumulator = accumulator.add(ourPosition);
    }
    agent.position = accumulator.divide(data.fixedObjects.size());
   
    Log.log(AGENT_MODEL, "My position: [%.2f,%.2f,%.2f]", agent.position.getX(), agent.position.getY(), agent.position.getZ());
View Full Code Here
42
43
44
45
46
47
48
49
50
51
    assertGoodRotation();
    assertGoodPosition();
  }

  private void assertGoodPosition(){
    Vector3D expected = Vector3D.cartesian(-2.0, 0.0, 0.54);
    Vector3D diff = agent.getPosition().subtract(expected);
    assertThat(diff.getR(), is(lessThan(0.1)));
    System.out.println(agent.getPosition());
  }
View Full Code Here
38
39
40
41
42
43
44
45
    reportBallRelativePosition(data.ballRelativePosition, data.SIMULATION_TIME);
  }
 
  public void reportBallRelativePosition(Vector3D ballRelativePosition, double simulationTime){
    ball.setRelativePosition(ballRelativePosition, simulationTime);
    Vector3D absolutePosition = agentModel.globalize(ballRelativePosition);
    ball.setPosition(absolutePosition, simulationTime);
  }
View Full Code Here
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
   * @param absolute Global positions of seen flags
   * @param seen Perceived positions of seen flags
   */
  private void calculateRotation(List<Vector3D> absolute, List<Vector3D> seen){
    int[] order = orderToFormAxes(absolute);
    Vector3D y1 = seen.get(order[0]);
    Vector3D y2 = seen.get(order[1]);
    Vector3D y2MinusY1 = y2.subtract(y1);
    Vector3D yAxis = y2MinusY1.negate().toUnitVector();
   
    Vector3D z1 = seen.get(order[2]);
    //this.agent is how we get closest point on a line [Y1, Y2] to a point Z_known
    //
    //                    (Y_2 - Y_1) x (Z_known - Y_1) x (Y_2 - Y_1)
    //    Z_needed = Z_known -  --------------------------------------------------
    //                        (Y_2 - Y_1) . (Y_2 - Y_1)
    Vector3D z2 = z1.subtract(y2MinusY1.crossProduct(z1.subtract(y1)).crossProduct(y2MinusY1).divide(y2MinusY1.dotProduct(y2MinusY1)));
    Vector3D zAxis = z1.subtract(z2).toUnitVector();
   
    if (absolute.get(order[2]).getZ() == 0.0)
      zAxis = zAxis.negate();
     
    Vector3D xAxis = yAxis.crossProduct(zAxis).toUnitVector();
   
    double rotationX = asin(-xAxis.getZ());
    double rotationZ = atan2( -xAxis.rotateOverX(rotationX).getY() / cos(rotationX), xAxis.rotateOverX(rotationX).getX() / cos(rotationX));
    double rotationY = atan2( -yAxis.rotateOverX(rotationX).getZ() / cos(rotationX), zAxis.rotateOverX(rotationX).getZ() / cos(rotationX));
    this.agent.rotationX = Angles.normalize(rotationX);
    this.agent.rotationY = Angles.normalize(rotationY);
    this.agent.rotationZ = Angles.normalize(rotationZ);
    Log.log(AGENT_MODEL, "My rotation: [%.2f,%.2f,%.2f]", rotationX, rotationY, rotationZ);
View Full Code Here
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
  /**
   * Of 3 flags, determine which two of them to use to form Y axis (0. and 1. member)
   * and which one is a complementary point to form Z axis perpendicular to Y
   */
  private int[] orderToFormAxes(List<Vector3D> absolute){
    Vector3D first = absolute.get(0);
    Vector3D second = absolute.get(1);
    Vector3D third = absolute.get(2);
    if(first.getZ() == second.getZ()){
      if (first.getY() > second.getY())
        return new int[]{0, 1, 2};
      return new int[]{1, 0, 2};
    }
    if(first.getZ() == third.getZ()){
      if (first.getY() > third.getY())
        return new int[]{0, 2, 1};
      return new int[]{2, 0, 1};
    }
   
    if (second.getY() > third.getY())
      return new int[]{1, 2, 0};
    return new int[]{2, 1, 0};
  }
View Full Code Here
97
98
99
100
101
102
103
104
  }
 
  @Test
  public void globalizePositions(){
    model.position = Vector3D.cartesian(-1.0, 0.0, 0.0);
    Vector3D centerOfThePitch = model.globalize(Vector3D.cartesian(0.0, 1.0, 0.0));
    assertThat(centerOfThePitch, is(Vector3D.cartesian(0, 0, 0)));
  }
View Full Code Here
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
*/
public class Vector3DTest{
 
  @Test
  public void basicOperations(){
    Vector3D one = cartesian(1, 1, 1);
    assertThat(one.add(cartesian(0, 0, 0)), is(equalTo(one)));
    assertThat(one.add(cartesian(1, 0, 0)), is(equalTo(cartesian(2, 1, 1))));
    assertThat(one.multiply(5.0), is(cartesian(5, 5, 5)));
    assertThat(one.divide(0.2), is(equalTo(cartesian(5, 5, 5))));
    assertThat(one.subtract(cartesian(0, 1, 0)), is(equalTo(cartesian(1, 0, 1))));
    assertThat(one.negate(), is(equalTo(cartesian(-1, -1, -1))));
    assertThat(one.addX(1.0), is(equalTo(cartesian(2, 1, 1))));
    assertThat(one.addY(-1.0), is(equalTo(cartesian(1, 0, 1))));
    assertThat(one.addZ(0.0), is(one));
//    original vector should remain the same
    assertThat(one, is(one));
  }
View Full Code Here
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
    assertThat(one, is(one));
  }
 
  @Test
  public void conversions(){
    Vector3D one = cartesian(1, 1, 1);
    Vector3D two = cartesian(0, 1, 1);
    Vector3D right = cartesian(-1.0, 1.0, 0.0);
   
    Vector3D cartesian = cartesian(Math.random(), Math.random(), Math.random());
    Vector3D spherical = spherical(cartesian.getR(), cartesian.getPhi(), cartesian.getTheta());
    Vector3D spherical2 = spherical(Math.random(), Math.random(), Math.random());
    Vector3D cartesian2 = cartesian(spherical2.getX(), spherical2.getY(), spherical2.getZ());
   
    assertThat(right.getPhi(), is(equalTo(Math.PI / 4.0)));
    assertEquals(one.getR(), Math.sqrt(3.0), .01);
    assertEquals(two.getPhi(), 0.0, .01);
    assertEquals(two.getTheta(), Math.PI / 4.0, .01);
View Full Code Here
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
  @Test
  public void rotations(){
    //the loops is here to measure performance
    for (int i = 0 ; i < 1000 ; i++)
    {
    Vector3D oneAhead = cartesian(0, 1, 0);
    //45 degrees to left
    Vector3D bitToLeft = oneAhead.rotateOverZ(Math.PI / 4.0);
    assertEquals(Math.PI / 4.0, bitToLeft.getPhi(), .01);
    assertEquals(oneAhead.getR(), bitToLeft.getR(), .01);
    Vector3D turnUp = oneAhead.rotateOverX(Math.PI / 2.0);
    assertThat(turnUp, is(cartesian(0, 0, 1)));
    Vector3D left = turnUp.rotateOverY(Math.PI / 2.0);
    assertThat(left, is(cartesian(1, 0, 0)));
    }
  }
View Full Code Here
68
69
70
71
72
73
74
75
76
77
      FixedObject flag = entry.getKey();
      if (isObsolete(fixedObjectSeenTimes.get(flag)) || fixedObjectsKalmans.get(flag) == null)
        fixedObjectsKalmans.put(flag, freshKalman());
     
      KalmanForVector kalman = fixedObjectsKalmans.get(flag);
      Vector3D newValue = kalman.update(entry.getValue());
      fixedObjects.put(flag, newValue);
      fixedObjectSeenTimes.put(flag, now);
    }
  }
View Full Code Here
TOP

Related Classes of sk.fiit.jim.math.Vector3D

  • sk.fiit.jim.agent.models.AgentModelTest
  • sk.fiit.jim.agent.models.AgentPositionCalculator
  • sk.fiit.jim.agent.models.AgentRotationCalculator
  • sk.fiit.jim.agent.models.KalmanAdjuster
  • sk.fiit.jim.agent.models.prediction.Prophet
  • sk.fiit.jim.agent.models.WorldModel
  • sk.fiit.jim.agent.ParserToAgentModelIntegrationTest
  • sk.fiit.jim.agent.parsing.Perceptors
  • sk.fiit.jim.agent.parsing.SeePerceptor
  • sk.fiit.jim.agent.skills.KickTargetEvaluator
    • Copyright © 2018 www.massapicom. All rights reserved.
    All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.