void CRobotVisualisation::visualise_paint_agent(struct sAgentInterface agent_interface)
{
float x = agent_interface.x;
float y = agent_interface.y;
float z = agent_interface.z;
float yaw = agent_interface.yaw;
float width = agent_interface.size*cm_size*0.1;
float depth = agent_interface.size*cm_size*0.1;
float height = agent_interface.size*cm_size*0.1;
struct sBotColor color = bot_to_color(agent_interface);
float r = color.r*agent_interface.color_intensity;
float g = color.g*agent_interface.color_intensity;
float b = color.b*agent_interface.color_intensity;
float ax, ay;
float bx, by;
float cx, cy;
float dx, dy;
point_rotate(&ax, &ay, -width/2.0, -depth/2.0, yaw);
point_rotate(&bx, &by, -width/2.0, depth/2.0, yaw);
point_rotate(&cx, &cy, width/2.0, depth/2.0, yaw);
point_rotate(&dx, &dy, width/2.0, -depth/2.0, yaw);
glColor3f(r, g, b);
glBegin(GL_QUADS);
glVertex3f(x + ax, y + ay, z + 0.001);
glVertex3f(x + bx, y + by, z + 0.001);
glVertex3f(x + cx, y + cy, z + 0.001);
glVertex3f(x + dx, y + dy, z + 0.001);
/*
glVertex3f(x + ax, y + ay, z + height);
glVertex3f(x + bx, y + by, z + height);
glVertex3f(x + cx, y + cy, z + height);
glVertex3f(x + dx, y + dy, z + height);
*/
glEnd();
glBegin(GL_LINES);
glVertex3f(x, y, z + 0.001);
glVertex3f(x, y, z + 0.001 + height);
glEnd();
/*
char str[256];
// sprintf(str,"%i %i", agent_interface.id, agent_interface.value);
sprintf(str,"g%i", agent_interface.value);
struct sBotColor font_color = uint_to_color(agent_interface.value, 8);
print(x, y, font_color.r, font_color.g, font_color.b, GLUT_BITMAP_8_BY_13, str);
*/
}
void point_min_area_rectangle(point_t *corners, point_t *new_corners, int corners_len) // Corners need to be sorted!
{
int i_min = 0;
int i_min_area = INT_MAX;
int i_x0 = 0, i_y0 = 0;
int i_x1 = 0, i_y1 = 0;
int i_x2 = 0, i_y2 = 0;
int i_x3 = 0, i_y3 = 0;
float i_r = 0;
// This algorithm aligns the 4 edges produced by the 4 corners to the x axis and then computes the
// min area rect for each alignment. The smallest rect is choosen and then re-rotated and returned.
for (int i = 0; i < corners_len; i++) {
int16_t x0 = corners[i].x, y0 = corners[i].y;
int x_diff = corners[(i+1)%corners_len].x - corners[i].x;
int y_diff = corners[(i+1)%corners_len].y - corners[i].y;
float r = -fast_atan2f(y_diff, x_diff);
int16_t x1[corners_len-1];
int16_t y1[corners_len-1];
for (int j = 0, jj = corners_len - 1; j < jj; j++) {
point_rotate(corners[(i+j+1)%corners_len].x, corners[(i+j+1)%corners_len].y, r, x0, y0, x1 + j, y1 + j);
}
int minx = x0;
int maxx = x0;
int miny = y0;
int maxy = y0;
for (int j = 0, jj = corners_len - 1; j < jj; j++) {
minx = MIN(minx, x1[j]);
maxx = MAX(maxx, x1[j]);
miny = MIN(miny, y1[j]);
maxy = MAX(maxy, y1[j]);
}
int area = (maxx - minx + 1) * (maxy - miny + 1);
if (area < i_min_area) {
i_min = i;
i_min_area = area;
i_x0 = minx, i_y0 = miny;
i_x1 = maxx, i_y1 = miny;
i_x2 = maxx, i_y2 = maxy;
i_x3 = minx, i_y3 = maxy;
i_r = r;
}
}
point_rotate(i_x0, i_y0, -i_r, corners[i_min].x, corners[i_min].y, &new_corners[0].x, &new_corners[0].y);
point_rotate(i_x1, i_y1, -i_r, corners[i_min].x, corners[i_min].y, &new_corners[1].x, &new_corners[1].y);
point_rotate(i_x2, i_y2, -i_r, corners[i_min].x, corners[i_min].y, &new_corners[2].x, &new_corners[2].y);
point_rotate(i_x3, i_y3, -i_r, corners[i_min].x, corners[i_min].y, &new_corners[3].x, &new_corners[3].y);
}
void CRobotVisualisation::visualise_paint_robot(struct sRobotData robot_data)
{
float x = robot_data.x_pos;
float y = robot_data.y_pos;
float z = robot_data.z_pos;
float yaw = robot_data.yaw;
float width = robot_data.width*cm_size*0.1;
float depth = robot_data.depth*cm_size*0.1;
float height = robot_data.height*cm_size*0.1;
float r = robot_data.color_r;
float g = robot_data.color_g;
float b = robot_data.color_b;
float ax, ay;
float bx, by;
float cx, cy;
float dx, dy;
point_rotate(&ax, &ay, -width/2.0, -depth/2.0, yaw);
point_rotate(&bx, &by, -width/2.0, depth/2.0, yaw);
point_rotate(&cx, &cy, width/2.0, depth/2.0, yaw);
point_rotate(&dx, &dy, width/2.0, -depth/2.0, yaw);
glColor3f(r, g, b);
glBegin(GL_QUADS);
glVertex3f(x + ax, y + ay, z + 0.001);
glVertex3f(x + bx, y + by, z + 0.001);
glVertex3f(x + cx, y + cy, z + 0.001);
glVertex3f(x + dx, y + dy, z + 0.001);
/*
glVertex3f(x + ax, y + ay, z + height);
glVertex3f(x + bx, y + by, z + height);
glVertex3f(x + cx, y + cy, z + height);
glVertex3f(x + dx, y + dy, z + height);
*/
glEnd();
glBegin(GL_LINES);
glVertex3f(x, y, z + 0.001);
glVertex3f(x, y, z + 0.001 + 100.0);
glEnd();
}
/** Dibuja una arista muy linda de un nodo a otro */
static void draw_fancy_edge(cairo_t* cr, Building* start, Building* end)
{
/* Si ya se dibujó en esta pasada, nada que hacer aqui */
if(end -> drawn == draw_count) return;
/* El nodo inicial manda */
Zone* cell = start -> zone;
/* El centro de la celda */
double cx = CELL_SIZE / 2 + cell -> x * CELL_SIZE/2;
double cy = CELL_SIZE / 2 + cell -> y * CELL_SIZE/2;
/* Es una arista dentro del la misma zona */
if(start -> zone == end -> zone)
{
/* Los indices de los nodos */
uint8_t i1 = start -> direction;
uint8_t i2 = end -> direction;
/* Numero para identificar que clase de arista es */
uint8_t code = abs(i2 - i1);
/* TODO agregar el delta para que no se vea feo */
/* Se trata de una recta que cruza la celda*/
if(code == cell -> sides / 2)
{
/** Coordenadas de la recta */
double xi,yi,theta;
/* Celda de 4 */
if(cell -> sides == 4)
{
double side = CELL_SIZE * tan(G_PI/8);
/* Punto inicial */
xi = side / 2 / sqrt(2);
yi = side / 2 / sqrt(2);
/* Rotacion de la recta */
theta = center_rotation[i1 * 2][i2 * 2] * G_PI / 4;
}
/* Celda de 8 */
else
{
/* Punto inicial */
xi = CELL_SIZE / 2;
yi = 0;
/* Rotacion de la recta */
theta = center_rotation[i1][i2] * G_PI / 4;
}
/* Punto final */
double xf = -xi;
double yf = -yi;
/* Rotar la recta */
point_rotate(&xi, &yi, theta);
point_rotate(&xf, &yf, theta);
/* Trazarla */
cairo_move_to(cr,cx + xi,cy - yi);
cairo_line_to(cr,cx + xf,cy - yf);
}
/* Una hermosa curva que va de un lado a otro */
else
{
/* Parametros del arco */
double arcx, arcy, radius, arci, arcf, center_rotate;
if(cell -> sides == 4)
{
double side = CELL_SIZE * tan(G_PI/8);
arcx = 0;
arcy = sqrt(2) * side / 2;
radius = side / 2;
center_rotate = center_rotation[i1 * 2][i2 * 2] / 2;
center_rotate *= G_PI / 2;
arci = starting_angles[i1 * 2][i2 * 2] * G_PI / 4;
arci += G_PI / 4;
arcf = arci + G_PI / 2;
}
/* Arista en una celda de 8 */
else
{
uint8_t delta;
/* Arista que va del nodo i al i + 3 */
if(code == 3 || code == 5)
{
arcx = CELL_SIZE / 2;
arcy = CELL_SIZE * (tan(G_PI/4) + tan(G_PI/8) / 2);
delta = 1;
}
/* Arista que va del nodo i al i + 2 */
else if(code == 2 || code == 6)
{
arcx = CELL_SIZE / 2;
arcy = CELL_SIZE / 2;
delta = 2;
}
/* Solo queda una opcion: arista que va del nodo i al i + 1 */
else
{
arcx = CELL_SIZE / 2;
arcy = arcx * tan(G_PI / 8);
delta = 3;
}
radius = arcy;
center_rotate = center_rotation[i1][i2];
center_rotate *= G_PI / 4;
arci = starting_angles[i1][i2] * G_PI / 4;
arcf = arci + delta * G_PI / 4;
}
point_rotate(&arcx, &arcy, center_rotate);
draw_arc(cr, cx + arcx, cy - arcy, radius, arci, arcf);
}
}
/* El otro nodo es un core */
else if(end -> zone -> core)
{
if(start -> zone -> core)
{
double r1 = core_radius(start -> zone) + CURVE_SIZE / 2;
double r2 = core_radius(end -> zone) + CURVE_SIZE / 2;
link_circles(cr, start -> x, start -> y, r1, end -> x, end -> y, r2);
}
else
{
/* Obtenemos el radio de ese core */
double r = core_radius(end -> zone) + CURVE_SIZE / 2;
/* Coordenadas correspondientes al nodo */
double startx, starty, theta;
if(start -> zone -> sides == 8)
{
startx = CELL_SIZE / 2;
starty = 0;
theta = start -> direction * G_PI / 4;
}
else
{
double side = CELL_SIZE * tan(G_PI/8);
startx = side / 2 / sqrt(2);
starty = side / 2 / sqrt(2);
theta = start -> direction * G_PI / 2;
}
point_rotate(&startx, &starty, theta);
/* Se conectan ambos */
link_circles(cr, cx + startx, cy - starty, 0, end -> x, end -> y, r);
}
}
}
