Package sk.fiit.jim.math

Examples of sk.fiit.jim.math.Vector3D

100
101
102
103
104
105
106
107
  }

  @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
107
108
109
110
111
112
113
114
115
  }

  @Test
  public void shouldGlobalizeAndRelativizesToSamePosition(){
    Random random = new Random();
    Vector3D point = Vector3D.cartesian(random.nextDouble(), random.nextDouble(), random.nextDouble());
    assertThat(model.globalize(model.relativize(point)), is(equalTo(point)));
    assertThat(model.relativize(model.globalize(point)), is(equalTo(point)));
  }
View Full Code Here
51
52
53
54
55
56
57
58
59
60
61
62
63
      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 = applyFriction(ball.getSpeed(), offset).add(whereItIsNow);
    Vector3D whereWillItBeBasedOnRelativeSpeed = applyFriction(ball.getRelativeSpeed(), now - ball.getLastTimeSeen() + offset).add(ball.getRelativePosition());
   
    prophecy.setBallPosition(whereWillItBe);
    prophecy.setBallPositionRelativized(agent.relativize(whereWillItBe));
    prophecy.setBallRelativePosition(whereWillItBeBasedOnRelativeSpeed);
  }
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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
      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
39
40
41
42
43
44
45
46
47
48
49
  public void updatePosition(ParsedData data){
    if(data.fixedObjects == null || data.fixedObjects.size() < 1)
      return;

    Vector3D result = useApproximation(data.fixedObjects);
//    Vector3D result = null;
//    if (data.fixedObjects.size() >= 3)
//      result = useSophisticatedCalculation(data.fixedObjects);
//    if (result == null || result.getR() == Double.NaN)
//      result = useSimpleCalculation(data.fixedObjects);
View Full Code Here
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
    Log.log(AGENT_MODEL, "My position: [%.2f,%.2f,%.2f]", agent.position.getX(), agent.position.getY(), agent.position.getZ());
  }

  //do not delete yet, may come handy if simpleCalculation does not yield good results
  private Vector3D useSophisticatedCalculation(Map<FixedObject, Vector3D> fixedObjects){
    Vector3D min = intersection(fixedObjects, 0.0);
    double MAX_NAO_HEIGHT = 0.7;
    Vector3D max = intersection(fixedObjects, MAX_NAO_HEIGHT);
    Vector3D increment = min.subtract(max).divide(APPROXIMATION_ALGORITHM_STEPS);
    double minError = Double.POSITIVE_INFINITY;
    Vector3D bestResult = max;
    double lastError = Double.POSITIVE_INFINITY;

    for (int i = 0; i < APPROXIMATION_ALGORITHM_STEPS ; i++){
      Vector3D tested = min.subtract(increment.multiply(i));
      double error = 0.0;
      for (FixedObject flag : fixedObjects.keySet()){
        double seenDistance = fixedObjects.get(flag).getR();
        double realDistance = flag.getAbsolutePosition().subtract(tested).getR();
        error += pow(seenDistance - realDistance, 2.0);
View Full Code Here
84
85
86
87
88
89
90
91
92
93
94
95
  private Vector3D intersection(Map<FixedObject, Vector3D> fixedObjects, double z)
  {
    List<FixedObject> flags = new ArrayList<FixedObject>(fixedObjects.keySet());
    FixedObject first = flags.get(0);
    FixedObject second = flags.get(1);
    Vector3D firstSeen = fixedObjects.get(first);
    Vector3D secondSeen = fixedObjects.get(second);
    double R0 = getR(first, firstSeen, z);
    double R1 = getR(second, secondSeen, z);
    double denominator = -2.0*(first.getAbsolutePosition().getY() - second.getAbsolutePosition().getY());
    double y = (R0 - R1) / denominator;
    double C = y*y - 2*y*first.getAbsolutePosition().getY() - R0;
View Full Code Here
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
    real.getAbsolutePosition().getY()*real.getAbsolutePosition().getY()-
    (z - real.getAbsolutePosition().getZ())*(z - real.getAbsolutePosition().getZ());
  }

  private Vector3D useApproximation(Map<FixedObject, Vector3D> fixedObjects){
    Vector3D position = agent.position;
    int currentlyProcessedFlag = 0;
    List<FixedObject> flags = new ArrayList<FixedObject>(fixedObjects.keySet());
    int failSafe = 0;

    while (tooFar(position, fixedObjects) && failSafe++ < 50){
      FixedObject flag = flags.get(currentlyProcessedFlag++ % flags.size());
      double seenDistance = fixedObjects.get(flag).getR();
      Vector3D flagsPosition = flag.getAbsolutePosition();
      position = (position.subtract(flagsPosition)).setR(seenDistance).add(flagsPosition);
      if (position.getZ() < 0.0)
        position = position.setZ(-position.getZ());
    }

View Full Code Here
128
129
130
131
132
133
134
135
136
137
138
139
  }

  private boolean tooFar(Vector3D position, Map<FixedObject,Vector3D> fixedObjects) {
    for (Entry<FixedObject, Vector3D> entry : fixedObjects.entrySet()){
      FixedObject key = entry.getKey();
      Vector3D value = entry.getValue();
      double calculatedDiff = position.subtract(key.getAbsolutePosition()).getR();
      if (abs(calculatedDiff - value.getR()) > 0.1)
        return true;
    }
    return false;
  }
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.