-
Notifications
You must be signed in to change notification settings - Fork 0
/
Position.java
119 lines (96 loc) · 3.37 KB
/
Position.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
package aig;
import java.util.Arrays;
import java.awt.Color;
import java.awt.Graphics2D;
import java.awt.geom.Point2D;
class Position {
private double[] X, Y, H; // last three known positions
private long[] T;
private static long[] map = {800,800};
public Position(double x, double y, double h, long time) {
X = new double[3];
Y = new double[3];
H = new double[3];
T = new long[3];
Arrays.fill(X, x);
Arrays.fill(Y, y);
Arrays.fill(H, h);
T[0] = time;
T[1] = -1;
T[2] = -1;
}
public void update(Point2D pos, double h, long time) {
int i = 3;
while (--i > 0) {
X[i] = X[i-1];
Y[i] = Y[i-1];
H[i] = H[i-1];
T[i] = T[i-1];
}
X[i] = x;
Y[i] = y;
H[i] = h;
T[i] = time;
}
public double guessX(long time) {
return guessPos(time)[0];
}
public double guessY(long time) {
return guessPos(time)[1];
}
public double deltHeading() {
return (H[0]-H[1]) / (T[0]-T[1]);
}
public double guessShotTime(double x, double y) {
double dist = Helper.calcDistanceFromPosition(x, y, X[0], Y[0]);
double[] np = guessPos(T[0]+1);
double dDist = Helper.calcDistanceFromPosition(x, y, np[0], np[1])-dist;
double dB = 11;
return (dist / (dB - dDist)) + 1;
}
public double guessSpeed() {
return Helper.calcDistanceFromPosition(X[1], Y[1], X[0], Y[0])
/ (T[0]-T[1]);
}
public double[] guessPos(long time) {
double[] pos = new double[2];
if (T[1] < 0) { // not enough data
pos[0] = X[0];
pos[1] = Y[0];
return pos;
}
double dHeading = deltHeading() * (time - T[0]);
double gHeading = H[0] + (dHeading/2);
double errHeading = Math.abs(Helper.plusMinusPI(
gHeading -
Helper.calcHeadingFromPosition(X[1], Y[1], X[0], Y[0])));
if (errHeading > (Math.PI/2)) {
gHeading = Helper.plusMinusPI(gHeading + Math.PI);
}
double gDistance = guessSpeed() * (time - T[0]);
double x = Helper.calcXFromHeadingDistance(
gHeading, gDistance, X[0]);
double y = Helper.calcYFromHeadingDistance(
gHeading, gDistance, Y[0]);
pos[0] = (x < 0) ? 0 : ((x > map[0]) ? map[0] : x);
pos[1] = (y < 0) ? 0 : ((y > map[1]) ? map[1] : y);
return pos;
}
public void paint(Graphics2D g, long time, long shotTime, boolean target) {
g.setColor(new Color(0x40, 0xff, 0x60, 0xbb));
g.drawArc((int) X[0]-15, (int) Y[0]-15, 30, 30, 0, 360);
g.drawArc((int) X[1]-10, (int) Y[1]-10, 20, 20, 0, 360);
g.drawArc((int) X[2]-5, (int) Y[2]-5, 10, 10, 0, 360);
if (target) {
g.setColor(new Color(0xcc, 0xcc, 0x10, 0xbb));
g.fillArc((int) guessX(time)-20, (int) guessY(time)-20,
40, 40, 0, 360);
g.setColor(new Color(0xff, 0x10, 0x10, 0xff));
int shotX = (int) guessX(time+shotTime);
int shotY = (int) guessY(time+shotTime);
g.drawArc(shotX-15, shotY-15, 30, 30, 0, 360);
g.drawLine(shotX-20, shotY, shotX+20, shotY);
g.drawLine(shotX, shotY-20, shotX, shotY+20);
}
}
}