Subversion Repositories shark

#include <stdio.h>
#include <math.h>
#include "percorso.h"

int nelem;
PATH_ELEM * head;

/* void print_elem(PATH_ELEM *elem) */
/* { */
/*   if (elem->type ==  PATH_ELEM_ARC) { */
/*     PATH_DATA_ARC *arc= (PATH_DATA_ARC *) elem->data; */
/*     printf("Center:  x =%3.3lf   y =%3.3lf\n", arc->c.x, arc->c.y); */
/*     printf("angles:  a1=%3.3lf   a2=%3.3lf\n", arc->alpha1, arc->alpha2); */
/*     printf("Radius:  r =%3.3lf\n", arc->r); */
/*   } */
/*   else if (elem->type == PATH_ELEM_LINE) { */
/*     PATH_DATA_LINE *line = (PATH_DATA_LINE *) elem->data; */
/*     printf("x1 = %3.3lf   y1 = %3.3lf     x2 = %3.3lf     y2 = %3.3lf\n", */
/*         line->p1.x, line->p1.y, line->p2.x, line->p2.y); */
/*   } */
/* } */

/* void print_point(POINT *p) */
/* { */
/*   printf("p.x = %3.3lf    p.y = %3.3lf\n", p->x, p->y); */
/* } */


/*-----------------------------------------------------------------*/

inline double point_dist(POINT *p1, POINT *p2)
{
  return sqrt((p1->x - p2->x)*(p1->x - p2->x) +
              (p1->y - p2->y)*(p1->y - p2->y));
}

double get_angle_from_arc(POINT *p1, PATH_DATA_ARC *arc)
{
  return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c)));
}

int inside_arc(PATH_DATA_ARC *arc, POINT *p)
{
  double alpha = get_angle_from_arc(p, arc);
  if (p->y < arc->c.y) alpha = 2*M_PI - alpha;

  if (alpha <= arc->alpha2  && alpha >= arc->alpha1) return 1;
  else return 0;
}

double get_distance_from_arc(POINT *p1, PATH_DATA_ARC *arc)
{
  return point_dist (p1,&(arc->c)) - arc->r;
}


double get_distance_from_line(POINT *p, PATH_DATA_LINE *l)
{
  return ((l->p1.y - l->p2.y) * p->x +
          (l->p2.x - l->p1.x) * p->y +
          (l->p1.x * l->p2.y - l->p2.x * l->p1.y)) /
    point_dist(&(l->p1), &(l->p2));
}


inline double get_t(POINT *p, PATH_DATA_LINE *line)
{
  double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) +
               (line->p2.y - line->p1.y) * (p->y - line->p1.y)) /
    (point_dist(&(line->p1), &(line->p2)) *
    point_dist(&(line->p1), &(line->p2)));
  return t;
}


double get_angle_from_line(POINT *p, PATH_DATA_LINE *l)
{
  return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x);
}

int includes_elem(POINT *p, PATH_ELEM *e, double radius)
{
  if (e->type == PATH_ELEM_ARC) {
    PATH_DATA_ARC *arc = (PATH_DATA_ARC *) e->data;
    if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p))
      return 1;
    else return 0;
  }
  else if (e->type==PATH_ELEM_LINE) {
    PATH_DATA_LINE *line = (PATH_DATA_LINE *) e->data;
    double t = get_t(p, line);
    double d = get_distance_from_line(p, line);
    if (t >= 0 && t <= 1 && d < radius) return 1;
    else return 0;
  }
  return 0;
}

void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r,
              double a1, double a2, int dir)
{
  arc->c.x = cx;
  arc->c.y = cy;
  arc->r = r;
  arc->alpha1 = a1;
  arc->alpha2 = a2;
  arc->dir = dir;
}

void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2,
              double y2, int dir)
{
  //printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2);
  l->p1.x = x1;
  l->p2.x = x2;
  l->p1.y = y1;
  l->p2.y = y2;
  l->dir = dir;
  //printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y);
}

/*---------------------------------------------------------------*/

PATH_ELEM *find_closest_elem(POINT *p, double radius)
{
  PATH_ELEM *cur = head;
  int i = 0;
 
  for (i=0; i<nelem; i++) {
    //printf("Searching element %d \n", i);
    //print_elem(cur);
    if (includes_elem(p, cur, radius)) return cur;
    else cur = cur->next;
  }
  return 0;
}

double get_distance_from_elem(POINT *p, PATH_ELEM *e)
{
  double d = 0;
  if (e->type == PATH_ELEM_ARC) {
    PATH_DATA_ARC *arc = e->data;
    d = get_distance_from_arc(p, arc);
  }
  else if (e->type == PATH_ELEM_LINE) {
      PATH_DATA_LINE *line = e->data;      
      d = get_distance_from_line(p, line);
  }
  return d;
}

double get_angle_from_elem(POINT *p, PATH_ELEM *e)
{
  double alpha = 0;
  if (e->type == PATH_ELEM_ARC) {
    PATH_DATA_ARC *arc = e->data;
    alpha = get_angle_from_arc(p, arc) - M_PI/2;
  }
  else if (e->type == PATH_ELEM_LINE) {
    PATH_DATA_LINE *line = e->data;      
    alpha = get_angle_from_line(p, line);
  }
  return alpha;
}


/* ritorna:
   1  : curva a sinistra
   -1 : curva a destra  
   0  : dritto */
int is_curve(PATH_ELEM *e)
{
  if (e->type == PATH_ELEM_ARC) {
    PATH_DATA_ARC *arc = e->data;
    return arc->dir;
  }
  else return 0;
}

/*---------------------------------------------------------------*/

PATH_ELEM elems[11];
PATH_DATA_ARC arcs[6];
PATH_DATA_LINE lines[5];

void Init_All_Path()
{
  init_line(&lines[0], 0,0,3,0, -1);
  elems[0].type = PATH_ELEM_LINE;
  elems[0].data = &lines[0];
  elems[0].next = &elems[1];

  init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1);
  elems[1].type = PATH_ELEM_ARC;
  elems[1].data = &arcs[0];
  elems[1].next = &elems[2];

  init_line(&lines[1], 4,1,4,7, -1);
  elems[2].type = PATH_ELEM_LINE;
  elems[2].data = &lines[1];
  elems[2].next = &elems[3];

  init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1);
  elems[3].type = PATH_ELEM_ARC;
  elems[3].data = &arcs[1];
  elems[3].next = &elems[4];

  init_line(&lines[2], 2,9,-1,9, 1);
  elems[4].type = PATH_ELEM_LINE;
  elems[4].data = &lines[2];
  elems[4].next = &elems[5];

  init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1);
  elems[5].type = PATH_ELEM_ARC;
  elems[5].data = &arcs[2];
  elems[5].next = &elems[6];

  init_line(&lines[3], -3,8,-3,5, 1);
  elems[6].type = PATH_ELEM_LINE;
  elems[6].data = &lines[3];
  elems[6].next = &elems[7];

  init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1);
  elems[7].type = PATH_ELEM_ARC;
  elems[7].data = &arcs[3];
  elems[7].next = &elems[8];

  init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1);
  elems[8].type = PATH_ELEM_ARC;
  elems[8].data = &arcs[4];
  elems[8].next = &elems[9];

  init_line(&lines[4], -1,3,-1,1, 1);
  elems[9].type = PATH_ELEM_LINE;
  elems[9].data = &lines[4];
  elems[9].next = &elems[10];

  init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1);  
  elems[10].type = PATH_ELEM_ARC;
  elems[10].data = &arcs[5];
  elems[10].next = &elems[0];

  head = &elems[0];
  nelem = 11;
}


/* int main(void) */
/* { */
/*   double d; */
/*   double alpha; */
/*   POINT p; */

/*   init_all(); */

/*   p.x = -1.75; */
/*   p.y = 4.25; */

/*   PATH_ELEM *e = find_closest_elem(&p, head, 1); */

/*   if (e != 0) { */
/*     d = get_distance_from_elem(&p, e); */
/*     alpha = get_angle_from_elem(&p, e); */
/*     print_elem(e); */
/*     printf("distance : %3.3lf   alpha %3.3lf\n", d, alpha); */
/*     printf("direzione %d\n", is_curve(e)); */
/*   } */
/*   else */
/*     printf("Not found!\n"); */


/*   p.x = 4.75; */
/*   p.y = 7.1; */
 
/*   e = find_closest_elem(&p, head, 1); */

/*   if (e != 0) { */
/*     d = get_distance_from_elem(&p, e); */
/*     alpha = get_angle_from_elem(&p, e); */
/*     print_elem(e); */
/*     printf("distance : %3.3lf   alpha %3.3lf\n", d, alpha); */
/*     printf("direzione %d\n", is_curve(e)); */
/*   } */
/*   else */
/*     printf("Not found!\n"); */

 
/* } */