Project

General

Profile

Statistics
| Revision:

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
		}*/