Package sk.fiit.jim.math

Examples of sk.fiit.jim.math.Vector3D

70
71
72
73
74
75
76
77
78
79
80
81
82
83
  @Test
  public void products(){
    //the loops is here to measure performance
    for (int i = 0 ; i < 1000 ; i++)
    {
    Vector3D a = cartesian(0, 1, 0);
    Vector3D b = cartesian(1, 0, 0);
    assertEquals(a.dotProduct(a), 1.0, .01);
    assertEquals(a.dotProduct(b), 0.0, .01);
    assertThat(a.crossProduct(b), is(equalTo(cartesian(0, 0, -1))));
    assertThat(a.crossProduct(a), is(equalTo(cartesian(0, 0, 0))));
    assertThat(b.crossProduct(a), is(equalTo(cartesian(0, 0, 1))));
    }
  }
View Full Code Here
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
    }
  }
 
  @Test
  public void setters(){
    Vector3D one = cartesian(1, 1, 1);
    Vector3D eigen = cartesian(0, 1, 0);
    assertThat(one.setX(0), is(equalTo(cartesian(0, 1, 1))));
    assertThat(one.setY(2), is(equalTo(cartesian(1, 2, 1))));
    assertThat(one.setZ(4), is(equalTo(cartesian(1, 1, 4))));
    assertThat(eigen.setR(2), is(equalTo(cartesian(0, 2, 0))));
    assertThat(eigen.setPhi(Math.PI / 2.0), is(equalTo(cartesian(-1, 0, 0))));
    assertThat(eigen.setTheta(Math.PI / 2.0), is(equalTo(cartesian(0, 0, 1))));
    //identity
    assertThat(one, is(one));
    assertThat(eigen, is(eigen));
  }
View Full Code Here
52
53
54
55
56
57
58
59
60
61
62
63
64
      prophecy.setBallPosition(Vector3D.ZERO_VECTOR);
      prophecy.setBallPositionRelativized(agent.relativize(Vector3D.ZERO_VECTOR));
      return;
    }
   
    Vector3D whereItIsNow = ball.getSpeed().multiply(now - ball.getLastTimeSeen()).add(ball.getPosition());
    Vector3D whereWillItBe = ball.getSpeed().multiply(offset).add(whereItIsNow);
    Vector3D whereWillItBeBasedOnRelativeSpeed = ball.getRelativeSpeed().multiply(now - ball.getLastTimeSeen() + offset).add(ball.getRelativePosition());
   
    prophecy.setBallPosition(whereWillItBe);
    prophecy.setBallPositionRelativized(agent.relativize(whereWillItBe));
    prophecy.setBallRelativePosition(whereWillItBeBasedOnRelativeSpeed);
  }
View Full Code Here
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
      ForceReceptor frp = data.forceReceptor == null ? new ForceReceptor() : data.forceReceptor;
      boolean isRight = "rf".equals(frpMatcher.group(1));
      double x = Double.valueOf(frpMatcher.group(2));
      double y = Double.valueOf(frpMatcher.group(3));
      double z = Double.valueOf(frpMatcher.group(4));
      Vector3D center = Vector3D.cartesian(x, y, z);
      x = Double.valueOf(frpMatcher.group(5));
      y = Double.valueOf(frpMatcher.group(6));
      z = Double.valueOf(frpMatcher.group(7));
      Vector3D magnitude = Vector3D.cartesian(x, y, z);
      if (isRight){
        frp.rightFootForce = magnitude;
        frp.rightFootPoint = center;
      }
      else{
View Full Code Here
83
84
85
86
87
88
89
90
91
92
93
  {
    Matcher bodyPartMatcher = bodyPartPattern.matcher(message);
   
    Map<String, Vector3D> bodyParts = new HashMap<String, Vector3D>();
    String bodyPart = null;
    Vector3D where = null;
    while(bodyPartMatcher.find()){
      bodyPart = bodyPartMatcher.group(1);
      double r = Double.valueOf(bodyPartMatcher.group(2));
      double phi = Double.valueOf(bodyPartMatcher.group(3));
      double theta = Double.valueOf(bodyPartMatcher.group(4));
View Full Code Here
43
44
45
46
47
48
49
50
51
52
    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
83
84
85
86
87
88
89
90
91
92
93
  {
    Matcher bodyPartMatcher = bodyPartPattern.matcher(message);

    Map<String, Vector3D> bodyParts = new HashMap<String, Vector3D>();
    String bodyPart = null;
    Vector3D where = null;
    while(bodyPartMatcher.find()){
      bodyPart = bodyPartMatcher.group(1).toLowerCase();
      double r = Double.valueOf(bodyPartMatcher.group(2));
      double phi = Double.valueOf(bodyPartMatcher.group(3));
      double theta = Double.valueOf(bodyPartMatcher.group(4));
View Full Code Here
69
70
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
   * @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);
View Full Code Here
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
  /**
   * 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
92
93
94
95
96
97
98
99
100
    ParsedData data = new ParsedData();
    data.fixedObjects.put(THEIR_UPPER_CORNER, THEIR_UPPER_CORNER.getAbsolutePosition().subtract(ourPosition).rotateOverZ(-rotationZ));
    data.fixedObjects.put(THEIR_LOWER_CORNER, THEIR_LOWER_CORNER.getAbsolutePosition().subtract(ourPosition).rotateOverZ(-rotationZ));
    data.fixedObjects.put(THEIR_LOWER_POST, THEIR_LOWER_POST.getAbsolutePosition().subtract(ourPosition).rotateOverZ(-rotationZ));
    model.processNewServerMessage(data);
    Vector3D diff = model.getPosition().subtract(ourPosition);

    assertThat(diff.getR(), is(closeTo(0, 0.1)));
  }
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.