-
Notifications
You must be signed in to change notification settings - Fork 0
/
swarm.pde
200 lines (167 loc) · 7.54 KB
/
swarm.pde
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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
class Swarm {
//Variables
ArrayList<Bot> bots;
PVector move = new PVector();
PVector mousePos = new PVector();
int botcount = 0;
//Constructor
Swarm(int botcount_) {
botcount=botcount_;
}
public void Init() {
bots = new ArrayList<Bot>();
for ( int i = 0; i<botcount; i++) {
int formationWidth = 2;
float startX = width - 400 + i%(formationWidth)*bot_Size;
float startY = 100 + bot_Size * floor(i/(formationWidth));
addBot(i, startX, startY);
}
}
public void Loop() {
updateSwarm();
mousePos.set(mouseX, mouseY);
}
public void updateSwarm() {
for (int i = 0; i<bots.size(); i++) {
Bot bot = bots.get(i);
if (i==0) {
if (mousePressed && (mouseButton == RIGHT)) {
bot.setPos(mousePos);
}
}
bot.setSize(bot_Size);
//Assign new target to bot
if(bot.needNewTarget){
updateTarget(bot, i);
bot.needNewTarget = false;
bot.needNewPath = true;
}
//Send target to bot
bot.goal_pos = goal_Pos[i];
//Path Planning
if(bot.needNewPath && bot.pos.x !=0 && bot.pos.y !=0 && millis()>bot.nextLoop){
bot.nextLoop = millis()+int(random(200,400));
bot.waypoints.clear();
recalculatePath(bot);
for (int j = path.size()-1; j >=0 ; j--) {
float x = path.get(j).pos.x*cellSize;
float y = path.get(j).pos.y*cellSize;
bot.waypoints.add(new PVector(x,y));
}
}
bot.Loop();
checkIntersection(bot.depthCamera, i, true);
checkIntersection(bot.ultrasonic, i, false);
checkIntersection(bot.leftInfrared, i, false);
checkIntersection(bot.rightInfrared, i, false);
}
}
void checkIntersection(Sensor sensor, int i, boolean discover){
//Detect depth camera ray intersection with bots and walls
PVector p1 = sensor.sensorPos;
PVector closestIntersectionPoint = new PVector(10000, 10000);
PVector distanceToIntersection = new PVector();
for (int k=0; k<sensor.numberOfBeams; k++) {
boolean wallintersectionExists = false;
boolean botintersectionExists = false;
PVector p2 = new PVector(sensor.beamEndPoints[k].x, sensor.beamEndPoints[k].y);
PVector sub = PVector.sub(p2, p1);
// y = a * x + b
float a = sub.y / sub.x;
float b = p1.y - a * p1.x;
closestIntersectionPoint = new PVector(10000, 10000);
//Check for beam collision with other bots
for (int ii = 0; ii<bots.size(); ii++) {
Bot targetBot = bots.get(ii);
//do not check self
if (!(ii == i)) {
float A = (1 + a * a);
float B = (2 * a *( b - targetBot.pos.y) - 2 * targetBot.pos.x);
float C = (targetBot.pos.x * targetBot.pos.x + (b - targetBot.pos.y) * (b - targetBot.pos.y)) - (targetBot.botSizePixels/2 * targetBot.botSizePixels/2);
float delta = B * B - 4 * A * C;
if (delta >= 0) {
float x1 = (-B - sqrt(delta)) / (2 * A);
float y1 = a * x1 + b;
float x2 = (-B + sqrt(delta)) / (2 * A);
float y2 = a * x2 + b;
if ((x1 > min(p1.x, p2.x)) && (x1 < max(p1.x, p2.x)) && (y1 > min(p1.y, p2.y)) && (y1 < max(p1.y, p2.y))||(x2 > min(p1.x, p2.x)) && (x2 < max(p1.x, p2.x)) && (y2 > min(p1.y, p2.y)) && (y2 < max(p1.y, p2.y))) {
botintersectionExists = true;
PVector closestIntersection = PVector.sub(p1, closestIntersectionPoint);
PVector intersectionPoint1 = new PVector(x1, y1);
PVector intersectionPoint2 = new PVector(x2, y2);
PVector.sub(p1, intersectionPoint1, distanceToIntersection);
if (distanceToIntersection.mag()<closestIntersection.mag()) {
closestIntersectionPoint.set(intersectionPoint1);
closestIntersection = PVector.sub(p1, closestIntersectionPoint);
}
PVector.sub(p1, intersectionPoint2, distanceToIntersection);
if (distanceToIntersection.mag()<closestIntersection.mag()) {
closestIntersectionPoint.set(intersectionPoint2);
}
p2 = new PVector(closestIntersectionPoint.x, closestIntersectionPoint.y);
}
}
}
}
//Check for line intersection
if (edgePool.size()>0) {
for (int j=0; j<edgePool.size(); j++) {
PVector p3 = new PVector(edgePool.get(j).sx, edgePool.get(j).sy);
PVector p4 = new PVector(edgePool.get(j).ex+1, edgePool.get(j).ey+1);
PVector sub1 = PVector.sub(p4, p3);
float a1 = sub1.y / sub1.x;
float b1 = p3.y - a1 * p3.x;
float x = (b1 - b) / (a - a1);
float y = a * x + b;
if ((x > min(p1.x, p2.x)) && (x < max(p1.x, p2.x)) && (y > min(p1.y, p2.y)) && (y < max(p1.y, p2.y))
&& (x > min(p3.x, p4.x)) && (x < max(p3.x, p4.x)) && (y > min(p3.y, p4.y)) && (y < max(p3.y, p4.y))) {
wallintersectionExists = true;
PVector closestIntersection = PVector.sub(p1, closestIntersectionPoint);
PVector intersectionPoint = new PVector(x+random(-sensor.noise, sensor.noise), y+random(-sensor.noise, sensor.noise));
PVector.sub(p1, intersectionPoint, distanceToIntersection);
if (distanceToIntersection.mag()<closestIntersection.mag()) {
closestIntersectionPoint.set(intersectionPoint);
}
}
}
if (wallintersectionExists&&(closestIntersectionPoint.x<width)&&(closestIntersectionPoint.y<height)) {
fill(255, 0, 0);
noStroke();
sensor.beamEndPointsIntersect[k].set(closestIntersectionPoint);
} else {
sensor.beamEndPointsIntersect[k].set(sensor.beamEndPoints[k]);
}
}
if (botintersectionExists&&(closestIntersectionPoint.x<width)&&(closestIntersectionPoint.y<height)) {
sensor.beamEndPointsIntersect[k].set(closestIntersectionPoint);
}
if (PVector.sub(sensor.beamEndPointsIntersect[k], sensor.sensorPos).mag()>PVector.sub(sensor.beamStartPoints[k], sensor.sensorPos).mag() && PVector.sub(sensor.beamEndPointsIntersect[k], sensor.sensorPos).mag()>0) {
PVector start = sensor.beamStartPoints[k];
PVector end = sensor.beamEndPointsIntersect[k];
PVector diff = PVector.sub(end, start);
float xLast = 0;
float yLast = 0;
for (float m=0; m<=1.1; m+=(cellSize)/(diff.mag()*2)) {
PVector currentCell = PVector.add(start, PVector.mult(diff, m));
if(currentCell.x != xLast && currentCell.y != yLast){
if(discover){
if((wallintersectionExists || botintersectionExists) && (m>=0.95-(cellSize)/(diff.mag()*3))){
updateCell(currentCell,0.0, 0.03);
// updateCell(PVector.add(start, PVector.mult(diff, m)),0.0);
}else{
updateCell(currentCell,1.0, 0.03);
// updateCell(PVector.add(start, PVector.mult(diff, m)),1.0);
}
}
}
xLast = currentCell.x;
yLast = currentCell.y;
}
}
}
}
public void addBot(int id_, float x_, float y_) {
PVector setPos = new PVector(0, 0);
bots.add(new Bot(botcount, bots, setPos.set(x_, y_), id_));
}
}