Rev 1624 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1624 | giacomo | 1 | #include <stdio.h> |
| 2 | #include <math.h> |
||
| 3 | #include "percorso.h" |
||
| 4 | |||
| 5 | int nelem; |
||
| 6 | PATH_ELEM * head; |
||
| 7 | |||
| 8 | /* void print_elem(PATH_ELEM *elem) */ |
||
| 9 | /* { */ |
||
| 10 | /* if (elem->type == PATH_ELEM_ARC) { */ |
||
| 11 | /* PATH_DATA_ARC *arc= (PATH_DATA_ARC *) elem->data; */ |
||
| 12 | /* printf("Center: x =%3.3lf y =%3.3lf\n", arc->c.x, arc->c.y); */ |
||
| 13 | /* printf("angles: a1=%3.3lf a2=%3.3lf\n", arc->alpha1, arc->alpha2); */ |
||
| 14 | /* printf("Radius: r =%3.3lf\n", arc->r); */ |
||
| 15 | /* } */ |
||
| 16 | /* else if (elem->type == PATH_ELEM_LINE) { */ |
||
| 17 | /* PATH_DATA_LINE *line = (PATH_DATA_LINE *) elem->data; */ |
||
| 18 | /* printf("x1 = %3.3lf y1 = %3.3lf x2 = %3.3lf y2 = %3.3lf\n", */ |
||
| 19 | /* line->p1.x, line->p1.y, line->p2.x, line->p2.y); */ |
||
| 20 | /* } */ |
||
| 21 | /* } */ |
||
| 22 | |||
| 23 | /* void print_point(POINT *p) */ |
||
| 24 | /* { */ |
||
| 25 | /* printf("p.x = %3.3lf p.y = %3.3lf\n", p->x, p->y); */ |
||
| 26 | /* } */ |
||
| 27 | |||
| 28 | |||
| 29 | /*-----------------------------------------------------------------*/ |
||
| 30 | |||
| 31 | inline double point_dist(POINT *p1, POINT *p2) |
||
| 32 | { |
||
| 33 | return sqrt((p1->x - p2->x)*(p1->x - p2->x) + |
||
| 34 | (p1->y - p2->y)*(p1->y - p2->y)); |
||
| 35 | } |
||
| 36 | |||
| 37 | double get_angle_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
||
| 38 | { |
||
| 39 | return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c))); |
||
| 40 | } |
||
| 41 | |||
| 42 | int inside_arc(PATH_DATA_ARC *arc, POINT *p) |
||
| 43 | { |
||
| 44 | double alpha = get_angle_from_arc(p, arc); |
||
| 45 | if (p->y < arc->c.y) alpha = 2*M_PI - alpha; |
||
| 46 | |||
| 47 | if (alpha <= arc->alpha2 && alpha >= arc->alpha1) return 1; |
||
| 48 | else return 0; |
||
| 49 | } |
||
| 50 | |||
| 51 | double get_distance_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
||
| 52 | { |
||
| 53 | return point_dist (p1,&(arc->c)) - arc->r; |
||
| 54 | } |
||
| 55 | |||
| 56 | |||
| 57 | double get_distance_from_line(POINT *p, PATH_DATA_LINE *l) |
||
| 58 | { |
||
| 59 | return ((l->p1.y - l->p2.y) * p->x + |
||
| 60 | (l->p2.x - l->p1.x) * p->y + |
||
| 61 | (l->p1.x * l->p2.y - l->p2.x * l->p1.y)) / |
||
| 62 | point_dist(&(l->p1), &(l->p2)); |
||
| 63 | } |
||
| 64 | |||
| 65 | |||
| 66 | inline double get_t(POINT *p, PATH_DATA_LINE *line) |
||
| 67 | { |
||
| 68 | double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) + |
||
| 69 | (line->p2.y - line->p1.y) * (p->y - line->p1.y)) / |
||
| 70 | (point_dist(&(line->p1), &(line->p2)) * |
||
| 71 | point_dist(&(line->p1), &(line->p2))); |
||
| 72 | return t; |
||
| 73 | } |
||
| 74 | |||
| 75 | |||
| 76 | double get_angle_from_line(POINT *p, PATH_DATA_LINE *l) |
||
| 77 | { |
||
| 78 | return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x); |
||
| 79 | } |
||
| 80 | |||
| 81 | int includes_elem(POINT *p, PATH_ELEM *e, double radius) |
||
| 82 | { |
||
| 83 | if (e->type == PATH_ELEM_ARC) { |
||
| 84 | PATH_DATA_ARC *arc = (PATH_DATA_ARC *) e->data; |
||
| 85 | if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p)) |
||
| 86 | return 1; |
||
| 87 | else return 0; |
||
| 88 | } |
||
| 89 | else if (e->type==PATH_ELEM_LINE) { |
||
| 90 | PATH_DATA_LINE *line = (PATH_DATA_LINE *) e->data; |
||
| 91 | double t = get_t(p, line); |
||
| 92 | double d = get_distance_from_line(p, line); |
||
| 93 | if (t >= 0 && t <= 1 && d < radius) return 1; |
||
| 94 | else return 0; |
||
| 95 | } |
||
| 96 | return 0; |
||
| 97 | } |
||
| 98 | |||
| 99 | void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r, |
||
| 100 | double a1, double a2, int dir) |
||
| 101 | { |
||
| 102 | arc->c.x = cx; |
||
| 103 | arc->c.y = cy; |
||
| 104 | arc->r = r; |
||
| 105 | arc->alpha1 = a1; |
||
| 106 | arc->alpha2 = a2; |
||
| 107 | arc->dir = dir; |
||
| 108 | } |
||
| 109 | |||
| 110 | void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2, |
||
| 111 | double y2, int dir) |
||
| 112 | { |
||
| 113 | //printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2); |
||
| 114 | l->p1.x = x1; |
||
| 115 | l->p2.x = x2; |
||
| 116 | l->p1.y = y1; |
||
| 117 | l->p2.y = y2; |
||
| 118 | l->dir = dir; |
||
| 119 | //printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y); |
||
| 120 | } |
||
| 121 | |||
| 122 | /*---------------------------------------------------------------*/ |
||
| 123 | |||
| 124 | PATH_ELEM *find_closest_elem(POINT *p, double radius) |
||
| 125 | { |
||
| 126 | PATH_ELEM *cur = head; |
||
| 127 | int i = 0; |
||
| 128 | |||
| 129 | for (i=0; i<nelem; i++) { |
||
| 130 | //printf("Searching element %d \n", i); |
||
| 131 | //print_elem(cur); |
||
| 132 | if (includes_elem(p, cur, radius)) return cur; |
||
| 133 | else cur = cur->next; |
||
| 134 | } |
||
| 135 | return 0; |
||
| 136 | } |
||
| 137 | |||
| 138 | double get_distance_from_elem(POINT *p, PATH_ELEM *e) |
||
| 139 | { |
||
| 140 | double d = 0; |
||
| 141 | if (e->type == PATH_ELEM_ARC) { |
||
| 142 | PATH_DATA_ARC *arc = e->data; |
||
| 143 | d = get_distance_from_arc(p, arc); |
||
| 144 | } |
||
| 145 | else if (e->type == PATH_ELEM_LINE) { |
||
| 146 | PATH_DATA_LINE *line = e->data; |
||
| 147 | d = get_distance_from_line(p, line); |
||
| 148 | } |
||
| 149 | return d; |
||
| 150 | } |
||
| 151 | |||
| 152 | double get_angle_from_elem(POINT *p, PATH_ELEM *e) |
||
| 153 | { |
||
| 154 | double alpha = 0; |
||
| 155 | if (e->type == PATH_ELEM_ARC) { |
||
| 156 | PATH_DATA_ARC *arc = e->data; |
||
| 157 | alpha = get_angle_from_arc(p, arc) - M_PI/2; |
||
| 158 | } |
||
| 159 | else if (e->type == PATH_ELEM_LINE) { |
||
| 160 | PATH_DATA_LINE *line = e->data; |
||
| 161 | alpha = get_angle_from_line(p, line); |
||
| 162 | } |
||
| 163 | return alpha; |
||
| 164 | } |
||
| 165 | |||
| 166 | |||
| 167 | /* ritorna: |
||
| 168 | 1 : curva a sinistra |
||
| 169 | -1 : curva a destra |
||
| 170 | |||
| 171 | int is_curve(PATH_ELEM *e) |
||
| 172 | { |
||
| 173 | if (e->type == PATH_ELEM_ARC) { |
||
| 174 | PATH_DATA_ARC *arc = e->data; |
||
| 175 | return arc->dir; |
||
| 176 | } |
||
| 177 | else return 0; |
||
| 178 | } |
||
| 179 | |||
| 180 | /*---------------------------------------------------------------*/ |
||
| 181 | |||
| 182 | PATH_ELEM elems[11]; |
||
| 183 | PATH_DATA_ARC arcs[6]; |
||
| 184 | PATH_DATA_LINE lines[5]; |
||
| 185 | |||
| 186 | void Init_All_Path() |
||
| 187 | { |
||
| 188 | init_line(&lines[0], 0,0,3,0, -1); |
||
| 189 | elems[0].type = PATH_ELEM_LINE; |
||
| 190 | elems[0].data = &lines[0]; |
||
| 191 | elems[0].next = &elems[1]; |
||
| 192 | |||
| 193 | init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1); |
||
| 194 | elems[1].type = PATH_ELEM_ARC; |
||
| 195 | elems[1].data = &arcs[0]; |
||
| 196 | elems[1].next = &elems[2]; |
||
| 197 | |||
| 198 | init_line(&lines[1], 4,1,4,7, -1); |
||
| 199 | elems[2].type = PATH_ELEM_LINE; |
||
| 200 | elems[2].data = &lines[1]; |
||
| 201 | elems[2].next = &elems[3]; |
||
| 202 | |||
| 203 | init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1); |
||
| 204 | elems[3].type = PATH_ELEM_ARC; |
||
| 205 | elems[3].data = &arcs[1]; |
||
| 206 | elems[3].next = &elems[4]; |
||
| 207 | |||
| 208 | init_line(&lines[2], 2,9,-1,9, 1); |
||
| 209 | elems[4].type = PATH_ELEM_LINE; |
||
| 210 | elems[4].data = &lines[2]; |
||
| 211 | elems[4].next = &elems[5]; |
||
| 212 | |||
| 213 | init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1); |
||
| 214 | elems[5].type = PATH_ELEM_ARC; |
||
| 215 | elems[5].data = &arcs[2]; |
||
| 216 | elems[5].next = &elems[6]; |
||
| 217 | |||
| 218 | init_line(&lines[3], -3,8,-3,5, 1); |
||
| 219 | elems[6].type = PATH_ELEM_LINE; |
||
| 220 | elems[6].data = &lines[3]; |
||
| 221 | elems[6].next = &elems[7]; |
||
| 222 | |||
| 223 | init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1); |
||
| 224 | elems[7].type = PATH_ELEM_ARC; |
||
| 225 | elems[7].data = &arcs[3]; |
||
| 226 | elems[7].next = &elems[8]; |
||
| 227 | |||
| 228 | init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1); |
||
| 229 | elems[8].type = PATH_ELEM_ARC; |
||
| 230 | elems[8].data = &arcs[4]; |
||
| 231 | elems[8].next = &elems[9]; |
||
| 232 | |||
| 233 | init_line(&lines[4], -1,3,-1,1, 1); |
||
| 234 | elems[9].type = PATH_ELEM_LINE; |
||
| 235 | elems[9].data = &lines[4]; |
||
| 236 | elems[9].next = &elems[10]; |
||
| 237 | |||
| 238 | init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1); |
||
| 239 | elems[10].type = PATH_ELEM_ARC; |
||
| 240 | elems[10].data = &arcs[5]; |
||
| 241 | elems[10].next = &elems[0]; |
||
| 242 | |||
| 243 | head = &elems[0]; |
||
| 244 | nelem = 11; |
||
| 245 | } |
||
| 246 | |||
| 247 | |||
| 248 | /* int main(void) */ |
||
| 249 | /* { */ |
||
| 250 | /* double d; */ |
||
| 251 | /* double alpha; */ |
||
| 252 | /* POINT p; */ |
||
| 253 | |||
| 254 | /* init_all(); */ |
||
| 255 | |||
| 256 | /* p.x = -1.75; */ |
||
| 257 | /* p.y = 4.25; */ |
||
| 258 | |||
| 259 | /* PATH_ELEM *e = find_closest_elem(&p, head, 1); */ |
||
| 260 | |||
| 261 | /* if (e != 0) { */ |
||
| 262 | /* d = get_distance_from_elem(&p, e); */ |
||
| 263 | /* alpha = get_angle_from_elem(&p, e); */ |
||
| 264 | /* print_elem(e); */ |
||
| 265 | /* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
||
| 266 | /* printf("direzione %d\n", is_curve(e)); */ |
||
| 267 | /* } */ |
||
| 268 | /* else */ |
||
| 269 | /* printf("Not found!\n"); */ |
||
| 270 | |||
| 271 | |||
| 272 | /* p.x = 4.75; */ |
||
| 273 | /* p.y = 7.1; */ |
||
| 274 | |||
| 275 | /* e = find_closest_elem(&p, head, 1); */ |
||
| 276 | |||
| 277 | /* if (e != 0) { */ |
||
| 278 | /* d = get_distance_from_elem(&p, e); */ |
||
| 279 | /* alpha = get_angle_from_elem(&p, e); */ |
||
| 280 | /* print_elem(e); */ |
||
| 281 | /* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
||
| 282 | /* printf("direzione %d\n", is_curve(e)); */ |
||
| 283 | /* } */ |
||
| 284 | /* else */ |
||
| 285 | /* printf("Not found!\n"); */ |
||
| 286 | |||
| 287 | |||
| 288 | /* } */ |