Colony

/**

 * @file world.c

 * @author Colony Project

 * @brief Simulator world code

 *

 * This is the world.

 **/

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <stdarg.h>

#include "world.h"

#include "robot.h"

#include "robot_shared.h"

world_t world;

int init_world(int max_objs, bbox_t b)

{

    double pts[8] = {b.p1.x, b.p1.y, b.p1.x, b.p2.y, b.p2.x,b.p2.y,b.p2.x,b.p1.y};

    world.max_objs = max_objs;

    world.cur_objs = 0;

    world.objs = calloc(max_objs,  sizeof(object_t));

    world.win = b;

    /* Add a polygon surrounding the world */

    create(ID_POLY,4,POLY_CONNECTED,pts);

}

int destroy_world()

{

    int i;

    for (i = 0; i < world.cur_objs; i++) {

        destroy(&world.objs[i]);

    }

    free(world.objs);

}

double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj) =

    {

    collide_poly

    };

int (*create_func[NUM_SHAPES])(object_t *obj, va_list ap) =

{

    create_poly

};

int (*destroy_func[NUM_SHAPES])(object_t *obj) = 

    {

    destroy_poly

    };

double collide_circle(ray_t *ray, object_t *obj)

{

    return -1;

}

double collide_seg(ray_t *ray, point_t p3, point_t p4)

{   

    double denom, nume_a, nume_b, ua, ub, xint, yint, dist;

    point_t p1 = ray->p;

    point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)};

    //printf("(%g,%g) --> (%g,%g) with (%g,%g) --> (%g,%g)\n",p1.x,p1.y,p2.x,p2.y,p3.x,p3.y,p4.x,p4.y);

    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);

    if (denom == 0) {

        return RAY_MISS;

    }

    nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x);

    nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x);

    ua = nume_a/denom;

    if (ua < 0 || ua > 1) {

        return RAY_MISS;

    }

    ub = nume_b/denom;

    if (ub < 0) {

        return RAY_MISS;

    }

    xint = p3.x+ua*(p4.x-p3.x);

    yint = p3.y+ua*(p4.y-p3.y);

    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));

    return dist;

}

double collide_poly(ray_t *ray, object_t *obj)

{

    int i;

    double min = RAY_MISS;

    double x;

    poly_t *p = (poly_t *) obj->props;

    if (obj->id != ID_POLY){

        return -1;

    }

    for (i = 0; i < p->num_pts - 1; i++) {

        if ((x = collide_seg(ray, p->pts[i], p->pts[i+1])) < min){

            min = x;

        }

    }

    if (p->type == POLY_CONNECTED) {

        if ((x = collide_seg(ray,p->pts[i],p->pts[0])) < min)

            min = x;

    }

    return min;

}

double collide(ray_t *ray, object_t *obj) 

{

    return collide_func[obj->id+ID_OFFSET](ray, obj);

}

double collide_world(ray_t *ray)

{

    double min = RAY_MISS;

    int i, x;

    for (i = 0; i < world.cur_objs; i++) {        

        x = collide(ray, &world.objs[i]);

        min = (x < min) ? x : min;

    }

    return min;

}

object_t *create(int id, ...)

{

    object_t *obj = &(world.objs[world.cur_objs++]);

    va_list args;

    va_start(args, id);

    if (create_func[id+ID_OFFSET](obj, args) < 0) {

        /* Cleanup ? */

    }

    va_end(args);

    return obj;

}

int destroy(object_t *obj) 

{

    if (obj->id == ID_NULL)

        return 0;

    destroy_func[obj->id+ID_OFFSET](obj);

    obj->id = ID_NULL;

}

/**

 *

 **/

int create_poly(object_t *obj, va_list ap) 

{

    int i;

    poly_t *p;

    int argc =  va_arg(ap, int);

    int poly_type = va_arg(ap, int);

    p = malloc(sizeof(poly_t));

    p->num_pts = argc;

    p->pts = malloc((argc) * sizeof(point_t));

    p->type = poly_type;

                double *pts = va_arg(ap, double*);

    for(i=0;i < argc; i++) {

                        p->pts[i].x = pts[2*i];

                        p->pts[i].y = pts[2*i+1];

    }

    obj->id = ID_POLY;

    obj->bbox = NULL;

    obj->props = p;

    return 1;

}

int create_rect(object_t *obj, va_list ap)

{

    int i;

    poly_t *p;

    int argc = va_arg(ap, int);

}

int destroy_poly (object_t *obj)

{

    poly_t *p;

    p =  (poly_t *)obj->props;

    if (p != NULL) free(p->pts);

    free(p);

    return 1;

}

void print_world(void)

{

    int i;

    printf("WORLD: %d object(s) in (%g,%g)->(%g,%g)\n", world.cur_objs, world.win.p1.x,world.win.p1.y,world.win.p2.x,world.win.p2.y);

    for (i = 0; i < world.cur_objs; i++) {

        print_object(&world.objs[i]);

    }

}

void print_object(object_t *obj)

{

    int i;

    poly_t *p;

    switch (obj->id) {

        case ID_NULL:

            printf("\tNULL\n");

        case ID_POLY:

            p = (poly_t *) obj->props;

            printf("\tPOLYGON (%d points, %s) { ", p->num_pts, p->type?"connected" : "disconnected");

            for (i = 0; i < p->num_pts; i++){

                printf("(%g, %g) ", p->pts[i].x, p->pts[i].y);

            }

            printf("}\n");

            break;

        default:

            break;

    }

}

/*

 * XXX: This shouldn't be here. Where should it be?

 */

/* These need to be measured... */

double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};

void update_rangefinders(Robot *bot)

{

    RangeFinder r = bot->shared->ranges;

    ray_t rf = {bot->pose.x, bot->pose.y, 0};

    /* Motion code has +y going down, I don't.

     * So to compensate, just reflect theta up to first quadrant.*/

    double theta = bot->pose.theta;

    theta = 2*M_PI - theta; 

    //ray_t rf = {0,0,0};

    int ir;

    double x;

    for (ir = 0; ir < 5; ir++)

    {

        rf.d = rf_thetas[ir];

        x = collide_world(&rf);

        printf("[%d] = %g\n", ir, x);

        bot->shared->ranges.d[ir] = x;

    }

}

int load_object (const char* line) {

  char buf[BUF_SIZE];

  int i = 0;

  int j;

  int id;

  if (strstr(line,"POLYGON") == line){

    int num;

    int type;

    double *pts;

    id = ID_POLY;

    i = strlen("POLYGON") + 1;

    sscanf(line+i,"%s",buf);

    num = strtol(buf,NULL,0);

    i += strlen(buf) + 1;

    if (strstr(line+i,"CONNECTED") == line+i){

      i += strlen("CONNECTED") + 1;

      type = POLY_CONNECTED;

    }else if (strstr(line+i,"DISCONNECTED") == line+i){

      i += strlen("DISCONNECTED") + 1;

      type = POLY_DISCONNECTED;

    }else if (strstr(line+i,"RECT") == line+i){

      i += strlen("RECT") + 1;

      type = POLY_RECT;

    }else{

      // invalid input

                        fprintf(stderr,"Invalid input for POLYGON\n");

      return -1;

    }

    char *c1,*c2;

    c1 = (char*)(line+i);

    c2 = (char*)(line+i);

    pts = (double*) malloc(sizeof(double)*num*2);

    for (j = 0;j < num;j++){

      pts[2*j] = strtod(c1,&c2);

      pts[2*j+1] = strtod(c2,&c1);

#if DEBUG

      printf("%lf %lf\n",pts[2*j],pts[2*j+1]);

#endif

    }

    create(id,num,type,pts);

    free(pts);

  }else if (strstr(line,"CIRCLE") == line){

                id = ID_CIRCLE;

                i = strlen("CIRCLE") + 1;

                //TODO

        }else if (strstr(line,"RECTANGLE") == line){

                id = ID_RECTANGLE;

                i = strlen("RECTANGLE") + 1;

                //TODO

        }else{

                //Invalid input

#if DEBUG

                fprintf(stderr,"Invalid object: %s",line);

#endif

        }

  return 0;

}

int load_world (const char* filename,int max_objs) {

  FILE* fin;

  char buf[BUF_SIZE];

  bbox_t bbox = {0,0,0,0};

  if ((fin = fopen(filename,"r"))==NULL){

    // open file failed

                perror("Fail to open file");

    return -1;

  }

  if (fgets(buf,512,fin) != NULL){

    sscanf(buf,"WORLD %lf %lf %lf %lf",&bbox.p1.x,&bbox.p1.y,&bbox.p2.x,&bbox.p2.y);

    init_world(max_objs, bbox);

    while (fgets(buf,512,fin) != NULL){

      load_object(buf);

    }

  }

  fclose(fin);

  return 0;

}