void CollisionFilter::filter()
{
find_points();
for (const auto& point : points)
{
if (collision(point.second))
{
ROS_INFO_STREAM("Removing " << object_frame << "->" << point.first << " frame");
server->remove(object_frame, point.first);
}
}
}
void ValidStateFilter::filter()
{
find_points();
for (const auto& point : points)
{
if (!is_valid(point.second))
{
server->remove(object_frame, point.first);
}
}
ROS_INFO("Ended");
}
void SkDrawPath::parseSVG() {
fPath.reset();
const char* data = d.c_str();
SkPoint f = {0, 0};
SkPoint c = {0, 0};
SkPoint lastc = {0, 0};
SkPoint points[3];
char op = '\0';
char previousOp = '\0';
bool relative = false;
do {
data = skip_ws(data);
if (data[0] == '\0')
break;
char ch = data[0];
if (is_digit(ch) || ch == '-' || ch == '+') {
if (op == '\0')
return;
}
else {
op = ch;
relative = false;
if (islower(op)) {
op = (char) toupper(op);
relative = true;
}
data++;
data = skip_sep(data);
}
switch (op) {
case 'M':
data = find_points(data, points, 1, relative, &c);
fPath.moveTo(points[0]);
op = 'L';
c = points[0];
break;
case 'L':
data = find_points(data, points, 1, relative, &c);
fPath.lineTo(points[0]);
c = points[0];
break;
case 'H': {
SkScalar x;
data = find_scalar(data, &x, relative, c.fX);
fPath.lineTo(x, c.fY);
c.fX = x;
}
break;
case 'V': {
SkScalar y;
data = find_scalar(data, &y, relative, c.fY);
fPath.lineTo(c.fX, y);
c.fY = y;
}
break;
case 'C':
data = find_points(data, points, 3, relative, &c);
goto cubicCommon;
case 'S':
data = find_points(data, &points[1], 2, relative, &c);
points[0] = c;
if (previousOp == 'C' || previousOp == 'S') {
points[0].fX -= lastc.fX - c.fX;
points[0].fY -= lastc.fY - c.fY;
}
cubicCommon:
// if (data[0] == '\0')
// return;
#if QUADRATIC_APPROXIMATION
quadApprox(fPath, points[0], points[1], points[2]);
#else //this way just does a boring, slow old cubic
fPath.cubicTo(points[0], points[1], points[2]);
#endif
//if we are using the quadApprox, lastc is what it would have been if we had used
//cubicTo
lastc = points[1];
c = points[2];
break;
case 'Q': // Quadratic Bezier Curve
data = find_points(data, points, 2, relative, &c);
goto quadraticCommon;
case 'T':
data = find_points(data, &points[1], 1, relative, &c);
points[0] = points[1];
if (previousOp == 'Q' || previousOp == 'T') {
points[0].fX = c.fX * 2 - lastc.fX;
points[0].fY = c.fY * 2 - lastc.fY;
}
quadraticCommon:
fPath.quadTo(points[0], points[1]);
lastc = points[0];
c = points[1];
break;
case 'Z':
fPath.close();
#if 0 // !!! still a bug?
if (fPath.isEmpty() && (f.fX != 0 || f.fY != 0)) {
c.fX -= SkScalar.Epsilon; // !!! enough?
fPath.moveTo(c);
fPath.lineTo(f);
fPath.close();
}
#endif
c = f;
op = '\0';
break;
case '~': {
SkPoint args[2];
data = find_points(data, args, 2, false, NULL);
fPath.moveTo(args[0].fX, args[0].fY);
fPath.lineTo(args[1].fX, args[1].fY);
}
break;
default:
SkASSERT(0);
return;
}
if (previousOp == 0)
f = c;
previousOp = op;
} while (data[0] > 0);
}
bool SkParsePath::FromSVGString(const char data[], SkPath* result) {
SkPath path;
SkPoint f = {0, 0};
SkPoint c = {0, 0};
SkPoint lastc = {0, 0};
SkPoint points[3];
char op = '\0';
char previousOp = '\0';
bool relative = false;
for (;;) {
data = skip_ws(data);
if (data[0] == '\0') {
break;
}
char ch = data[0];
if (is_digit(ch) || ch == '-' || ch == '+') {
if (op == '\0') {
return false;
}
} else {
op = ch;
relative = false;
if (is_lower(op)) {
op = (char) to_upper(op);
relative = true;
}
data++;
data = skip_sep(data);
}
switch (op) {
case 'M':
data = find_points(data, points, 1, relative, &c);
path.moveTo(points[0]);
op = 'L';
c = points[0];
break;
case 'L':
data = find_points(data, points, 1, relative, &c);
path.lineTo(points[0]);
c = points[0];
break;
case 'H': {
SkScalar x;
data = find_scalar(data, &x, relative, c.fX);
path.lineTo(x, c.fY);
c.fX = x;
} break;
case 'V': {
SkScalar y;
data = find_scalar(data, &y, relative, c.fY);
path.lineTo(c.fX, y);
c.fY = y;
} break;
case 'C':
data = find_points(data, points, 3, relative, &c);
goto cubicCommon;
case 'S':
data = find_points(data, &points[1], 2, relative, &c);
points[0] = c;
if (previousOp == 'C' || previousOp == 'S') {
points[0].fX -= lastc.fX - c.fX;
points[0].fY -= lastc.fY - c.fY;
}
cubicCommon:
path.cubicTo(points[0], points[1], points[2]);
lastc = points[1];
c = points[2];
break;
case 'Q': // Quadratic Bezier Curve
data = find_points(data, points, 2, relative, &c);
goto quadraticCommon;
case 'T':
data = find_points(data, &points[1], 1, relative, &c);
points[0] = points[1];
if (previousOp == 'Q' || previousOp == 'T') {
points[0].fX = c.fX * 2 - lastc.fX;
points[0].fY = c.fY * 2 - lastc.fY;
}
quadraticCommon:
path.quadTo(points[0], points[1]);
lastc = points[0];
c = points[1];
break;
case 'Z':
path.close();
#if 0 // !!! still a bug?
if (fPath.isEmpty() && (f.fX != 0 || f.fY != 0)) {
c.fX -= SkScalar.Epsilon; // !!! enough?
fPath.moveTo(c);
fPath.lineTo(f);
fPath.close();
}
#endif
c = f;
op = '\0';
break;
case '~': {
SkPoint args[2];
data = find_points(data, args, 2, false, NULL);
path.moveTo(args[0].fX, args[0].fY);
path.lineTo(args[1].fX, args[1].fY);
} break;
default:
return false;
}
if (previousOp == 0) {
f = c;
}
previousOp = op;
}
// we're good, go ahead and swap in the result
result->swap(path);
return true;
}
// takes an array of six coordinates alternating x y z
void draw_triangle(KZ_Point a, KZ_Point b, KZ_Point c) {
int ab_count = point_count(a, b);
KZ_Point *ab_points = malloc((ab_count + 1) * sizeof(KZ_Point));
if (ab_points == NULL)
printf("ab broken\n");
ab_count = find_points(a, b, ab_points);
int bc_count = point_count(b, c);
KZ_Point *bc_points = malloc((bc_count + 1) * sizeof(KZ_Point));
if (bc_points == NULL)
printf("bc broken\n");
bc_count = find_points(b, c, bc_points);
int ca_count = point_count(c, a);
KZ_Point *ca_points = malloc((ca_count + 1) * sizeof(KZ_Point));
if (ca_points == NULL)
printf("ca broken\n");
ca_count = find_points(c, a, ca_points);
#if DRAW_FILL
// order arrays by y descending
KZ_Point *abs;
int abinc;
if (ab_points[ab_count - 1].y > ab_points[0].y) {
abs = ab_points + ab_count - 1;
abinc = -1;
} else {
abs = ab_points;
abinc = 1;
}
KZ_Point *bcs;
int bcinc;
if (bc_points[bc_count - 1].y > bc_points[0].y) {
bcs = bc_points + bc_count - 1;
bcinc = -1;
} else {
bcs = bc_points;
bcinc = 1;
}
KZ_Point *cas;
int cainc;
if (ca_points[ca_count - 1].y > ca_points[0].y) {
cas = ca_points + ca_count - 1;
cainc = -1;
} else {
cas = ca_points;
cainc = 1;
}
// find the mid y value
int max_y = a.y > b.y ? a.y:b.y;
max_y = c.y > max_y ? c.y:max_y;
int mid_y;
if (max_y == a.y) {
mid_y = b.y > c.y ? b.y : c.y;
} else if (max_y == b.y) {
mid_y = a.y > c.y ? a.y : c.y;
} else if (max_y == c.y) {
mid_y = b.y > a.y ? b.y : a.y;
} else {
printf("mid_y can't be found\n");
return;
}
//categorize lines by upper, lower, long
KZ_Point *upper_segment = NULL;
int upper_inc = 0;
int upper_count = 0;
KZ_Point *lower_segment = NULL;
int lower_inc = 0;
int lower_count = 0;
KZ_Point *long_segment = NULL;
int long_inc = 0;
int long_count = 0;
if (mid_y == a.y) {
long_inc = bcinc;
long_segment = bcs;
long_count = bc_count;
if (max_y == b.y) {
upper_inc = abinc;
upper_segment = abs;
upper_count = ab_count;
lower_inc = cainc;
lower_segment = cas;
lower_count = ca_count;
} else if (max_y == c.y) {
upper_inc = cainc;
upper_segment = cas;
upper_count = ca_count;
lower_inc = abinc;
lower_segment = abs;
lower_count = ab_count;
}
} else if (mid_y == b.y) {
long_inc = cainc;
long_segment = cas;
long_count = ca_count;
if (max_y == a.y) {
upper_inc = abinc;
upper_segment = abs;
upper_count = ab_count;
lower_inc = bcinc;
lower_segment = bcs;
lower_count = bc_count;
} else if (max_y == c.y) {
upper_inc = bcinc;
upper_segment = bcs;
upper_count = bc_count;
lower_inc = abinc;
lower_segment = abs;
lower_count = ab_count;
}
} else if (mid_y == c.y) {
long_inc = abinc;
long_segment = abs;
long_count = ab_count;
if (max_y == a.y) {
upper_inc = cainc;
upper_segment = cas;
upper_count = ca_count;
lower_inc = bcinc;
lower_segment = bcs;
lower_count = bc_count;
} else if (max_y == b.y) {
upper_inc = bcinc;
upper_segment = bcs;
upper_count = bc_count;
lower_inc = cainc;
lower_segment = cas;
lower_count = ca_count;
}
}
int longi = 0, shorti = 0;
while (shorti * upper_inc < upper_count
&& upper_segment[shorti + upper_inc].y == upper_segment[shorti].y) {
shorti += upper_inc;
}
while (longi * long_inc < long_count
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| long_segment[longi].y != upper_segment[shorti].y)) {
longi += long_inc;
}
while (shorti * upper_inc < upper_count && longi * long_inc < long_count) {
draw_horizontal(upper_segment[shorti], long_segment[longi]);
do
shorti += upper_inc;
while (shorti * upper_inc < upper_count - 1
&& upper_segment[shorti + upper_inc].y == upper_segment[shorti].y);
do
longi += long_inc;
while (longi * long_inc < long_count - 1
&& long_segment[longi + long_inc].y >= mid_y
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| (shorti * upper_inc < upper_count
&& long_segment[longi].y != upper_segment[shorti].y)
|| (shorti * upper_inc >= upper_count
&& long_segment[longi].y > mid_y)));
}
shorti = 0;
int stopper = long_segment[longi].y;
while (shorti * lower_inc < lower_count - 1
&& (lower_segment[shorti].y > stopper
|| lower_segment[shorti + lower_inc].y == lower_segment[shorti].y)) {
shorti += lower_inc;
}
while (shorti * lower_inc < lower_count && longi * long_inc < long_count) {
draw_horizontal(lower_segment[shorti], long_segment[longi]);
do
shorti += lower_inc;
while (shorti * lower_inc < lower_count
&& lower_segment[shorti + lower_inc].y == lower_segment[shorti].y);
do
longi += long_inc;
while (longi * long_inc < long_count
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| (shorti * lower_inc < lower_count
&& long_segment[longi].y != lower_segment[shorti].y)
|| (shorti * lower_inc >= lower_count
&& long_segment[longi].y > mid_y)));
}
#endif
#if DRAW_EDGES
int p;
for (p = 0; p < ab_count; p++) {
ab_points[p].r = -1;
ab_points[p].kr = EDGE_RED;
ab_points[p].kg = EDGE_GREEN;
ab_points[p].kb = EDGE_BLUE;
consider_KZ_Point(ab_points[p]);
}
for (p = 0; p < bc_count; p++) {
bc_points[p].r = -1;
bc_points[p].kr = EDGE_RED;
bc_points[p].kg = EDGE_GREEN;
bc_points[p].kb = EDGE_BLUE;
consider_KZ_Point(bc_points[p]);
}
for (p = 0; p < ca_count; p++) {
ca_points[p].r = -1;
ca_points[p].kr = EDGE_RED;
ca_points[p].kg = EDGE_GREEN;
ca_points[p].kb = EDGE_BLUE;
consider_KZ_Point(ca_points[p]);
}
#endif
#if DRAW_VERTICES
a.r = b.r = c.r = -1;
a.kr = b.kr = c.kr = VERTICES_RED;
a.kg = b.kg = c.kg = VERTICES_BLUE;
a.kb = b.kb = c.kb = VERTICES_GREEN;
if (pix_in_screen(a.x, a.y))
consider_KZ_Point(a);
if (pix_in_screen(b.x, b.y))
consider_KZ_Point(b);
if (pix_in_screen(c.x, c.y))
consider_KZ_Point(c);
#endif
free(ab_points);
free(bc_points);
free(ca_points);
}
void
hatch_create(void)
{
Eina_List *list, *l;
XY *a, *b;
int i, num, res, res1;
int flag = 0;
Evas *e;
char buf[4096];
char *s, *st;
Eina_List *poly;
double dx, dy, g, g2;
e = shell->evas;
hs_set(shell->context.hatch_style);
poly = hatch_get_poly();
serv_set_hint(DUP(_("enter hatch angle: ")));
s = serv_get_string();
g = atof(s);
FREE(s);
num = hs_get_num();
for (i = 0; i < num; i++)
{
int flag = 0, cnt = 0;
double y, yr, ystep, scale;
g2 = g + hs_get_angle(i);
scale = hs_get_scale(i);
ystep = hs_get_ystep(i);
dy = hs_get_yoffset(i);
dx = hs_get_xoffset(i);
st = hs_get_line_style(i);
trans_rotate(poly, g2 / 180 * M_PI);
for (y = drawing->h * (-2); y < drawing->h * 3; y += ystep)
{
cnt++;
yr = y + dy;
list = find_points(poly, -210.0, yr, 420.0, yr);
if (!list)
continue;
if (!list->next)
continue;
trans_rotate(list, -g2 / 180 * M_PI);
flag = 0;
for (l = list; l->next; l = l->next)
{
flag = !flag;
a = (XY *) l->data;
b = (XY *) l->next->data;
if (flag)
{
_hatch_create_line(a->x, a->y,
b->x, b->y,
DUP(st),
scale,
shell->
context.thickness, dx * cnt);
}
}
}
trans_rotate(poly, -g2 / 180 * M_PI);
FREE(st);
}
}
