root / branches / colonetmk2 / code / projects / swarm / BOMPosRender / bom algo 1 save.txt @ 1456
History | View | Annotate | Download (1.48 KB)
1 | 1456 | rcahoon | for(int source=0; source<16; source++) |
---|---|---|---|
2 | { |
||
3 | for(int dest=0; dest<source; dest++) |
||
4 | { |
||
5 | if (sensors[source][dest]==null) |
||
6 | { |
||
7 | if (sensors[dest][source]!=null) |
||
8 | { |
||
9 | sensors[source][dest] = new SensorReading(Float.NaN, sensors[dest][source].dist); |
||
10 | } |
||
11 | } |
||
12 | else if (sensors[dest][source]==null) |
||
13 | { |
||
14 | sensors[dest][source] = new SensorReading(Float.NaN, sensors[source][dest].dist); |
||
15 | } |
||
16 | else |
||
17 | { |
||
18 | int avgdist = (sensors[source][dest].dist + sensors[dest][source].dist)/2; |
||
19 | sensors[source][dest].dist = avgdist; |
||
20 | sensors[dest][source].dist = avgdist; |
||
21 | } |
||
22 | } |
||
23 | } |
||
24 | |||
25 | boolean firstbot = true; |
||
26 | |||
27 | for(int i=0; i<16; i++) |
||
28 | { |
||
29 | for(int j=0; j<i; j++) |
||
30 | { |
||
31 | if (sensors[i][j]==null) continue; |
||
32 | if (positions[j]!=null) continue; |
||
33 | if (firstbot) |
||
34 | { |
||
35 | firstbot = false; |
||
36 | positions[i] = new R2(0, 0, 0); |
||
37 | } |
||
38 | if (positions[i]==null) continue; |
||
39 | if (Float.isNaN(positions[i].th)) continue; |
||
40 | positions[j] = new R2((int) (positions[i].x + sensors[i][j].dist*Math.cos(positions[i].th + sensors[i][j].dir)), |
||
41 | (int) (positions[i].y + sensors[i][j].dist*Math.sin(positions[i].th + sensors[i][j].dir)), |
||
42 | (float)(/*robostat[i].th + sensors[i][j].dir +*/ sensors[j][i].dir /*- Math.PI*/)); |
||
43 | } |
||
44 | } |
||
45 | |||
46 | /*for(int i=0; i<16; i++) |
||
47 | { |
||
48 | for(int j=0; j<16; j++) |
||
49 | { |
||
50 | for(int k=0; k<16; k++) |
||
51 | { |
||
52 | if (sensors[i][j]!=null && |
||
53 | sensors[i][k]!=null && |
||
54 | sensors[j][k]!=null) |
||
55 | { |
||
56 | |||
57 | } |
||
58 | } |
||
59 | } |
||
60 | }*/ |