| 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++;
|