Project

General

Profile

Revision 1042

Added by Ben Poole about 15 years ago

started working on integrating with rangefinders

View differences:

branches/simulator/projects/simulator/simulator/core/world.c
11 11
#include <math.h>
12 12
#include <stdarg.h>
13 13
#include "world.h"
14
#include "robot.h"
15
#include "robot_shared.h"
14 16

  
15 17
world_t world;
16 18

  
......
57 59
    point_t p1 = ray->p;
58 60
    point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)};
59 61

  
60
   // 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);
62
    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);
61 63
    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);
62 64
    if (denom == 0) {
63 65
	return RAY_MISS;
......
78 80
    xint = p3.x+ua*(p4.x-p3.x);
79 81
    yint = p3.y+ua*(p4.y-p3.y);
80 82
    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
83
    printf("Got: %g\n", dist);
81 84
    return dist;
82 85

  
83 86
}
84 87

  
85 88
double collide_poly(ray_t *ray, object_t *obj)
86 89
{
90

  
87 91
    int i;
88 92
    double min = RAY_MISS;
89 93
    double x;
......
107 111

  
108 112
double collide(ray_t *ray, object_t *obj) 
109 113
{
110
    if (ray == NULL || obj == NULL)
111
    {
112
	return -1;
113
    }
114 114
    return collide_func[obj->id+ID_OFFSET](ray, obj);
115 115
}
116 116

  
117
double collide_world(ray_t *ray)
118
{
119
    double min = RAY_MISS;
120
    int i, x;
121
    for (i = 0; i < world.cur_objs; i++) {	
122
	x = collide(ray, &world.objs[i]);
123
	min = (x < min) ? x : min;
124
    }
125
    return min;
126
}
117 127

  
118 128

  
129

  
119 130
object_t *create(int id, ...)
120 131
{
121 132
    object_t *obj = &(world.objs[world.cur_objs++]);
......
155 166
    p->pts = malloc((argc) * sizeof(point_t));
156 167
    p->type = poly_type;
157 168

  
158
    double* pts = (double*) malloc((argc) * sizeof(double) * 2);
159
    pts = va_arg(ap,double*);
160

  
161 169
    for(i=0;i < argc; i++) {
162
	p->pts[i].x = pts[2*i];
163
	p->pts[i].y = pts[2*i+1];
170
	p->pts[i].x = va_arg(ap, double);
171
	p->pts[i].y = va_arg(ap, double);
164 172
    }
165 173
    obj->id = ID_POLY;
166 174
    obj->bbox = NULL;
......
213 221
    }
214 222
}
215 223

  
224

  
225
/*
226
 * XXX: This shouldn't be here. Where should it be?
227
 */
228
/* These need to be measured... */
229
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
230
void update_rangefinders(Robot *bot)
231
{
232
    //RangeFinder r = bot->shared->ranges;
233
    //ray_t rf = {bot->pose.x, bot->pose.y};
234
    ray_t rf = {0,0,0};
235
    int ir;
236
    double x;
237
    for (ir = 0; ir < 5; ir++)
238
    {
239
	rf.d = rf_thetas[ir];
240
	x = collide_poly(&rf, world.objs);
241
	printf("[%d] = %g\n", ir, x);
242
	//bot->shared->ranges.d[ir] = collide_world(&rf);
243
    }
244
}
245

  
246

  
216 247
int starts_with (const char* line, const char* word) {
217 248
  do {
218 249
    line++;
branches/simulator/projects/simulator/simulator/core/world.h
30 30
#define CREATE(id, ...) (create_func[id](__VA_ARGS__))
31 31
#define PBBOX(b) {printf("(%g,%g) --> (%g, %g)\n",(b).p1.x,(b).p1.y,(b).p2.x,\
32 32
						  (b).p2.y);}
33
#define PRAY(v) {printf("(%g,%g), %g\n", (v).p.x, (v).p.y,(v).d);}
33 34

  
34 35
#define MAX_OBJS 100
35 36
#define BUF_SIZE 512
......
73 74
double collide_circle(ray_t *ray, object_t *obj);
74 75
double collide_rect(ray_t *ray, object_t *obj);
75 76
double collide_poly(ray_t *ray, object_t *obj);
77
double collide_world(ray_t *ray);
78

  
76 79
/*  Array of function pointers to the specific collide functions.
77 80
 *  Must be listed in the same order as IDs */
78 81
extern double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj);

Also available in: Unified diff