int oslom_net_global::check_unions_and_overlap(module_collection & mall, bool only_similar) {
mall.put_gaps();
if(mall.effective_groups()==0)
return 0;
cout<<"checking similar modules"<<endl<<endl;
check_existing_unions(mall);
if(only_similar==false) {
if(check_fusion_with_gather(mall))
check_fusion_with_gather(mall);
}
cout<<"checking highly intersecting modules"<<endl<<endl;
check_intersection(mall);
mall.compute_inclusions();
return 0;
}
Rectangle intersection(const Rectangle& a, const Rectangle &b)
{
if(check_intersection(a, b)) {
return Rectangle(
{std::max(a.getBottomLeft().x, b.getBottomLeft().x),
std::max(a.getBottomLeft().y, b.getBottomLeft().y)},
{std::min(a.getTopRight().x, b.getTopRight().x),
std::min(a.getTopRight().y, b.getTopRight().y)});
}
return Rectangle();
}
void sensors1(int s1, int s2)
{
int i, j, in_new_room, object_i, test=0, obst_test;
double final_o_x, final_o_y, object_x, object_y, int_x, int_y, dx, dy;
double dist=0.0;
double f_i, min_angle, max_angle, angle, robot_oo;
double sin_robot_oo_plus_f_i, cos_robot_oo_plus_f_i;
int tried_before[MAX_OBJ];
double sensorScale;
robot_oo = robot_o + torsoDir + sensor_zero;
sensorScale = TWO_PI / ((double) (n_sensors * n_sensors_fact));
for (i = s1*n_sensors_fact; i < (s2+1)*n_sensors_fact; i++) {
f_i = i * sensorScale;
object_i = actual_object;
final_o_x = object_x = robot_x;
final_o_y = object_y = robot_y;
for (j = 0; j < MAX_OBJ; j++) tried_before[j] = 0;
tried_before[actual_object] = 1;
do {
sin_robot_oo_plus_f_i = sin(robot_oo + f_i);
cos_robot_oo_plus_f_i = cos(robot_oo + f_i);
obst_test = check_obstacle(objs, n_obj,
&int_x, &int_y, object_x, object_y,
sin_robot_oo_plus_f_i,
cos_robot_oo_plus_f_i,
objs[object_i].x1, objs[object_i].y1,
objs[object_i].x2, objs[object_i].y2,
0.001);
if (obst_test == 0)
test = check_intersection(&int_x, &int_y, object_x, object_y,
sin_robot_oo_plus_f_i,
cos_robot_oo_plus_f_i,
objs[object_i].x1, objs[object_i].y1,
objs[object_i].x2, objs[object_i].y2);
if (obst_test == 1 || test == 1) {
final_o_x = int_x;
final_o_y = int_y;
dx = final_o_x - robot_x;
dy = final_o_y - robot_y;
dist = sqrt((dx*dx) + (dy*dy));
}
else {
int_x = object_x;
int_y = object_y;
}
in_new_room = 0;
if (obst_test == 0 && dist <= sensors_distance) {
for (j = 0; j < n_obj; j++) {
if ((objs[j].type == 0 || objs[j].type == 2 )&&
in_new_room == 0 && tried_before[j] == 0 &&
int_x >= objs[j].x1 - ALMOST_ZERO &&
int_x <= objs[j].x2 + ALMOST_ZERO &&
int_y >= objs[j].y1 - ALMOST_ZERO &&
int_y <= objs[j].y2 + ALMOST_ZERO) {
object_i = j;
object_x = int_x;
object_y = int_y;
in_new_room = 1;
tried_before[j] = 1;
}
}
}
}
while (in_new_room != 0);
if (dist > sensors_distance) {
final_o_x = robot_x +
((final_o_x - robot_x) / dist * sensors_distance);
final_o_y = robot_y +
((final_o_y - robot_y) / dist * sensors_distance);
dist = sensors_distance;
}
sensor2[i] = dist;
}
for (i = s1; i < s2+1; i ++) {
sensor[i] = 999999999999.9;
min_angle = ((double) i) / ((double) n_sensors) * TWO_PI;
max_angle = min_angle + (sensors_range / 2.0);
min_angle = min_angle - (sensors_range / 2.0);
for (j = s1*n_sensors_fact; j < (s2+1)*n_sensors_fact; j++) {
angle = j * sensorScale;
if (angle >= min_angle && angle <= max_angle &&
sensor[i] >= sensor2[j])
sensor[i] = sensor2[j];
}
/* Should be +- as opposed to just + */
sensor[i] = distrNormal(sensor[i], sensor_noise_level);
}
}
void doChange(double deltaX, double deltaY, double deltaT )
{
int i, space_i, in_new_space, j, test, obst_test;
double dist=0.0, dx, dy, object_x, object_y, d3, d2;
double old_x, old_y, old_o;
double robot2_x=0.0, robot2_y=0.0, robot3_x=0.0, robot3_y=0.0;
double fwdSq, angle=0.0;
int tried_before[MAX_OBJ];
if (actual_freespace < 0)
for (i = 0; i < n_space; i++) {
if (robot_x >= space[i].x1 && robot_x <= space[i].x2 &&
robot_y >= space[i].y1 && robot_y <= space[i].y2)
actual_freespace = i;
}
if (actual_freespace < 0) {
fprintf(stderr, "\nWARNING: robot not in free space...");
}
if (turning_noise_level != 0) {
deltaT = distrNormal(deltaT, deltaT*turning_noise_level);
}
if (forward_noise_level != 0) {
deltaX = distrNormal(deltaX, deltaX*forward_noise_level);
deltaY = distrNormal(deltaY, deltaY*forward_noise_level);
}
fwdSq = (double)(SQR(deltaX) + SQR(deltaY));
if( deltaX == 0 && deltaY == 0 )
angle = robot_o;
else
angle = (double)PI/2-(double)atan2(deltaY,deltaX);
old_o = robot_o;
robot_o = angle;
old_x = robot_x;
old_y = robot_y;
for (; robot_o >= TWO_PI; robot_o -= TWO_PI);
for (; robot_o < 0.0 ; robot_o += TWO_PI);
collision = 0;
if (check_obstacle(objs, n_obj,
&robot_x, &robot_y, old_x, old_y,
sin(robot_o), cos(robot_o), space[actual_freespace].x1,
space[actual_freespace].y1, space[actual_freespace].x2,
space[actual_freespace].y2, robot_r) == 0) {
robot_x = old_x + deltaX;
robot_y = old_y + deltaY;
} else {
dist = (((robot_x - old_x) * (robot_x - old_x)) +
((robot_y - old_y) * (robot_y - old_y)));
if (dist < fwdSq) {
fprintf(stderr, "Collision: At distance %.2f\n", dist);
collision = 1;
}
if (dist >= fwdSq) {
robot_x = old_x + deltaX;
robot_y = old_y + deltaY;
}
}
if (robot_x < space[actual_freespace].x1 ||
robot_x > space[actual_freespace].x2 ||
robot_y < space[actual_freespace].y1 ||
robot_y > space[actual_freespace].y2) { /* robot left last freespace */
robot_x = object_x = old_x;
robot_y = object_y = old_y;
space_i = actual_freespace;
for (i = 0; i < MAX_OBJ; i++) tried_before[i] = 0;
tried_before[space_i] = 1;
in_new_space = 1;
do {
obst_test = check_obstacle(objs, n_obj,
&robot2_x, &robot2_y, object_x, object_y,
sin(robot_o), cos(robot_o),
space[space_i].x1, space[space_i].y1,
space[space_i].x2, space[space_i].y2,
robot_r);
test = check_intersection(&robot3_x, &robot3_y, object_x, object_y,
sin(robot_o), cos(robot_o),
space[space_i].x1, space[space_i].y1,
space[space_i].x2, space[space_i].y2);
if (obst_test == 1 || test == 1) {
d2 = SQR(robot2_x - object_x) + SQR(robot2_y - object_y);
d3 = SQR(robot3_x - object_x) + SQR(robot3_y - object_y);
if (obst_test == 1 && (test == 0 || d2 < d3)) {
robot_x = robot2_x;
robot_y = robot2_y;
} else {
robot_x = robot3_x;
robot_y = robot3_y;
}
dx = robot_x - old_x;
dy = robot_y - old_y;
dist = (dx * dx) + (dy * dy);
if (dist > fwdSq) {
robot_x = old_x + deltaX;
robot_y = old_y + deltaY;
}
dx = robot_x - object_x;
dy = robot_y - object_y;
}
in_new_space = 0;
if (obst_test == 0 && dist <= fwdSq) {
for (j = 0; j < n_space; j++) {
if (tried_before[j] == 0 && in_new_space == 0 &&
robot_x >= space[j].x1 - ALMOST_ZERO &&
robot_x <= space[j].x2 + ALMOST_ZERO &&
robot_y >= space[j].y1 - ALMOST_ZERO &&
robot_y <= space[j].y2 + ALMOST_ZERO) {
space_i = j;
object_x = robot_x;
object_y = robot_y;
in_new_space = 1;
tried_before[space_i] = 1;
}
}
}
}
while (in_new_space != 0);
if (dist < fwdSq) {
fprintf(stderr, "Collision: At distance %.2f\n", dist);
collision = 1;
}
for (i = 0; i < n_space; i++) {
if (robot_x >= space[i].x1 && robot_x <= space[i].x2 &&
robot_y >= space[i].y1 && robot_y <= space[i].y2)
actual_freespace = i;
}
if (robot_x < objs[actual_object].x1 ||
robot_x > objs[actual_object].x2 ||
robot_y < objs[actual_object].y1 ||
robot_y > objs[actual_object].y2) {
actual_object = -1;
for (i = 0; i < n_obj && actual_object == -1; i++)
if (robot_x >= objs[i].x1 &&
robot_x <= objs[i].x2 &&
robot_y >= objs[i].y1 &&
robot_y <= objs[i].y2) {
actual_object = i;
break;
}
}
}
if (collision == 1) {
robot_x -= robot_min_dist * sin(robot_o);
robot_y -= robot_min_dist * cos(robot_o);
}
robot_o = old_o + deltaT;
}
