double ConvexHullProjectionConstraint::computeMargin(const Vector2& posIn2D)
{
bool isInside = true;
// First check if the point is inside the convex hull or not
iDynTree::VectorDynSize Ax(A.rows());
toEigen(Ax) = toEigen(A)*toEigen(posIn2D);
// If even one of the constraint is violated, then the point is outside the convex hull
for (int i=0; i < Ax.size(); i++)
{
if (!(Ax(i) <= b(i)))
{
isInside = false;
break;
}
}
// Then, compute the distance between each segment of the convex hull and the point :
// the minimum one is the distance of the point from the convex hull
// We start from the last segment that do not follow the segment[i],segment[i+1] structure
double distanceWithoutSign = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(projectedConvexHull.getNrOfVertices()-1),
projectedConvexHull(0));
// Find the minimum distance
for (int i=0; i < projectedConvexHull.getNrOfVertices()-1; i++)
{
double candidateDistance = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(i),
projectedConvexHull(i+1));
if (candidateDistance < distanceWithoutSign)
{
distanceWithoutSign = candidateDistance;
}
}
double margin;
// If the point is inside the convex hull, we return the distance, otherwise the negated distance
if (isInside)
{
margin = distanceWithoutSign;
}
else
{
margin = -distanceWithoutSign;
}
return margin;
}
int save_pgm(Drawing D, int pix_per_unit, int sample_type, int sample_size, FILE *outfile) {
List *layer;
LineSegment *ls;
Circle *cr;
double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0, x_ij = 0.0, y_ij = 0.0;
double dist, val;
int H, W, i, imax, j, jmax, k, l, n;
int **matrix, *content;
if (D->firstLayer == NULL) {
return EMPTY_IMAGE_ERROR;
}
layer = D->firstLayer;
while(layer != NULL) {
ls = layer->lineSegment;
cr = layer->circle;
if (ls != NULL) {
// Normalize units
ls->x1 *= pix_per_unit;
ls->y1 *= pix_per_unit;
ls->x2 *= pix_per_unit;
ls->y2 *= pix_per_unit;
ls->thickness *= pix_per_unit;
if ((min(ls->x1, ls->x2) - ls->thickness) < x1)
x1 = min(ls->x1, ls->x2) - ls->thickness;
if ((min(ls->y1, ls->y2) - ls->thickness) < y1)
y1 = min(ls->y1, ls->y2) - ls->thickness;
if ((max(ls->x1, ls->x2) + ls->thickness) > x2)
x2 = max(ls->x1, ls->x2) + ls->thickness;
if ((max(ls->y1, ls->y2) + ls->thickness) > y2)
y2 = max(ls->y1, ls->y2) + ls->thickness;
} else {
// Normalize units
cr->x *= pix_per_unit;
cr->y *= pix_per_unit;
cr->radius *= pix_per_unit;
cr->thickness *= pix_per_unit;
if (cr->x - cr->radius - cr->thickness < x1)
x1 = cr->x - cr->radius - cr->thickness;
if (cr->y - cr->radius - cr->thickness < y1)
y1 = cr->y - cr->radius - cr->thickness;
if (cr->x + cr->radius + cr->thickness > x2)
x2 = cr->x + cr->radius + cr->thickness;
if (cr->y + cr->radius + cr->thickness > y2)
y2 = cr->y + cr->radius + cr->thickness;
}
layer = layer->next;
}
W = ceil(x2 - x1);
H = ceil(y2 - y1);
matrix = malloc(H * sizeof(int *));
if (!matrix) {
printf("Erro ao alocar matriz enquanto tentava salvar o PGM.\n");
return MEMORY_ERROR;
}
content = malloc(H * W * sizeof(int));
if (!content) {
printf("Erro ao alocar vetor de conteúdo para a matriz enquanto tentava salvar o PGM.\n");
return MEMORY_ERROR;
}
for (int i = 0; i < H; i++) {
matrix[i] = &(content[i * W]);
for (int j = 0; j < W; j++) {
matrix[i][j] = 255;
}
}
layer = D->firstLayer;
while(layer != NULL) {
ls = layer->lineSegment;
cr = layer->circle;
if (ls != NULL) {
ls->x1 -= x1;
ls->y1 -= y1;
ls->x2 -= x1;
ls->y2 -= y1;
i = floor(min(ls->x1, ls->x2) - ls->thickness);
imax = floor(max(ls->x1, ls->x2) + ls->thickness);
jmax = floor(max(ls->y1, ls->y2) + ls->thickness);
for (; i < imax; i++) {
for (j = floor(min(ls->y1, ls->y2) - ls->thickness); j < jmax; j++) {
n = 0;
if (sample_type == GRID_SAMPLE) {
for (k = 1; k <= sample_size; k++) {
y_ij = j + 1.0 * k / (sample_size + 1.0);
for (l = 1; l <= sample_size; l++) {
x_ij = i + 1.0 * l / (sample_size + 1.0);
dist = distanceBetweenPointAndSegment(x_ij, y_ij, ls->x1, ls->y1, ls->x2, ls->y2);
printf("Distancia: %lf <= %lf\n", dist, ls->thickness);
if (dist <= ls->thickness)
n++;
}
}
} else {
for (k = 0; k < sample_size; k++) {
y_ij = j + (double)rand() / (double)((unsigned)RAND_MAX + 1);
x_ij = i + (double)rand() / (double)((unsigned)RAND_MAX + 1);
dist = distanceBetweenPointAndSegment(x_ij, y_ij, ls->x1, ls->y1, ls->x2, ls->y2);
if (dist <= ls->thickness)
n++;
}
}
/* NOTE QUE O MODO BORRACHA FOI INSERIDO POR CONTA PRÓPRIA DOS ALUNOS,
CONFORME AUTORIZADO PELO PROFESSOR NO ENUNCIADO DO EXERCICIO-PROGRAMA */
if (ls->eraseMode) {
if (sample_type == GRID_SAMPLE)
val = (1.0 * n) / (sample_size * sample_size);
else
val = (1.0 * n) / (sample_size);
matrix[j][i] = floor(max(1.0 * matrix[j][i], 255.0 * val));
} else {
if (sample_type == GRID_SAMPLE)
val = 1.0 - (1.0 * n) / (sample_size * sample_size);
else
val = 1.0 - (1.0 * n) / (sample_size);
matrix[j][i] = floor(min(1.0 * matrix[j][i], 255.0 * val));
}
}
}
} else {
cr->x -= x1;
cr->y -= y1;
i = floor(cr->x - cr->thickness - cr->radius);
imax = floor(cr->x + cr->thickness + cr->radius);
jmax = floor(cr->y + cr->thickness + cr->radius);
for (; i < imax; i++) {
for (j = floor(cr->y - cr->thickness - cr->radius); j < jmax; j++) {
n = 0;
if (sample_type == GRID_SAMPLE) {
for (k = 1; k <= sample_size; k++) {
y_ij = j + 1.0 * k / (sample_size + 1.0);
for (l = 1; l <= sample_size; l++) {
x_ij = i + 1.0 * l / (sample_size + 1.0);
dist = distance(x_ij, y_ij, cr->x, cr->y);
if ((cr->filled && dist <= (cr->radius + cr->thickness)) ||
(!cr->filled && dist >= (cr->radius - cr->thickness) && dist <= (cr->radius + cr->thickness)))
n++;
}
}
} else {
for (k = 1; k <= sample_size; k++) {
y_ij = j + (double)rand() / (double)((unsigned)RAND_MAX + 1);
x_ij = i + (double)rand() / (double)((unsigned)RAND_MAX + 1);
dist = distance(x_ij, y_ij, cr->x, cr->y);
if ((cr->filled && dist <= (cr->radius + cr->thickness)) ||
(!cr->filled && dist >= (cr->radius - cr->thickness) && dist <= (cr->radius + cr->thickness)))
n++;
}
}
if (sample_type == GRID_SAMPLE)
val = 1.0 - (1.0 * n) / (1.0 * sample_size * sample_size);
else
val = 1.0 - (1.0 * n) / (1.0 * sample_size);
matrix[j][i] = floor(min(1.0 * matrix[j][i], 255.0 * val));
}
}
}
layer = layer->next;
}
/*
/* REDEFINIR OS LIMITES DA IMAGEM É NECESSÁRIO DEVIDO A OPERAÇÃO DE BORRACHA/
for (y1 = 0; y1 < H; y1++) {
for (j = 0; j < W; j++)
if (matrix[(int)y1][j] != 255) break;
if (j < W && matrix[(int)y1][j] != 255) break;
}
for (y2 = H - 1; y2 >= 0; y2--) {
for (j = 0; j < W; j++)
if (matrix[(int)y2][j] != 255) break;
if (j < W && matrix[(int)y2][j] != 255) break;
}
for (x1 = 0; x1 < W; x1++) {
for (j = 0; j < H; j++)
if (matrix[j][(int)x1] != 255) break;
if (j < H && matrix[j][(int)x1] != 255) break;
}
for (x2 = W - 1; x2 >= 0; x2--) {
for (j = 0; j < H; j++)
if (matrix[j][(int)x2] != 255) break;
if (j < H && matrix[j][(int)x2] != 255) break;
}
*/
fprintf(outfile, "P2\n");
fprintf(outfile, "%d %d\n", W, H);
fprintf(outfile, "255\n");
for(i = 0; i < H; i++) {
for (j = 0; j < W; j++) {
fprintf(outfile, "%d ", matrix[i][j]);
}
fprintf(outfile, "\n");
}
return 0;
}
