tLine generate_draw_line(cv::Mat& image, const tPoint& prev)
{
tLine line(DRAW, prev, prev);
tPoint pos = line.start;
// Get initial direction
Direction dir = find_direction(image, line.start);
// Mark current position as visited
image.at<uchar>(pos.y, pos.x) = BLACK;
if(dir == NONE) {
return line;
}
// Keep moving while there are white pixels in the direction of movement
while(valid_direction(image, pos, dir)) {
pos = increment_position(dir, pos);
image.at<uchar>(pos.y, pos.x) = BLACK;
}
line.end = pos;
return line;
}
/*
Find direction for next movement.
*/
Direction find_direction(const cv::Mat& image, const tPoint& start)
{
for(Direction dir : DIRECTIONS) {
if(valid_direction(image, start, dir)) {
return dir;
}
}
return NONE;
}
static double valid_distance(DPOINT *a, DPOINT *b, double max,
int symmetric, DATA *d1, DATA *d2, GRIDMAP *map) {
double ddist, dX, dX2, inprod;
DPOINT p;
int mode = 0, i;
unsigned int row, col;
assert(a != NULL);
assert(b != NULL);
assert(d1 != NULL);
assert(d2 != NULL);
mode = d1->mode & d2->mode;
/*
* even if modes don't correspond, valid_direction() will
* calculate valid distances
*/
p.x = a->x - b->x;
p.y = a->y - b->y;
p.z = a->z - b->z;
if (map) {
/* transform here p to allow directional 2d cuts in a 3d world */
if (map_xy2rowcol(map, p.x, p.y, &row, &col))
return -1.0;
else
ddist = (1.0 * row) * map->cols + col + 0.5;
} else {
if (p.x > max || p.y > max || p.z > max)
return -1.0;
/* Changed K.M. Fri Feb 27 15:56:57 1998 */
/* if ddist < 0.0 then we don't need to check for dX! */
if ((ddist = valid_direction(&p, symmetric, d1)) > max || ddist < 0.0)
return -1.0;
}
dX = MIN(d1->dX, d2->dX);
if (dX < DBL_MAX) {
dX2 = dX * dX;
/* allow only points for which 2-norm ||x_i-x_j|| < dX */
if (d1->n_X != d2->n_X)
ErrMsg(ER_IMPOSVAL, "valid_distance(): d1->n_X != d2->n_X");
for (i = 0, inprod = 0.0; i < d1->n_X; i++)
inprod += SQR(a->X[i] - b->X[i]);
if (inprod > dX2)
ddist = -1.0;
}
/*
if (d1->coln_id > 0 && d2->coln_id > 0 && strcmp())
return -1.0;
*/
return ddist;
}