Subversion Repositories shark

Rev

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
/* } */