root / branches / colonetmk2 / code / projects / swarm / BOMPosRender / bom algo 1 save.txt @ 1456
History | View | Annotate | Download (1.5 KB)
| 1 | 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 | }*/ |