Revision 1042
started working on integrating with rangefinders
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