root / branches / simulator / projects / simulator / simulator / core / world.c @ 1040
History | View | Annotate | Download (4.01 KB)
1 |
/**
|
---|---|
2 |
* @file world.c
|
3 |
* @author Colony Project
|
4 |
* @brief Simulator world code
|
5 |
*
|
6 |
* This is the world.
|
7 |
**/
|
8 |
|
9 |
#include <stdio.h> |
10 |
#include <stdlib.h> |
11 |
#include <math.h> |
12 |
#include <stdarg.h> |
13 |
#include "world.h" |
14 |
|
15 |
world_t world; |
16 |
|
17 |
int init_world(int max_objs, bbox_t b) |
18 |
{ |
19 |
world.max_objs = max_objs; |
20 |
world.cur_objs = 0;
|
21 |
world.objs = calloc(max_objs, sizeof(object_t));
|
22 |
world.win = b; |
23 |
//PBBOX(b);
|
24 |
} |
25 |
int destroy_world()
|
26 |
{ |
27 |
int i;
|
28 |
for (i = 0; i < world.cur_objs; i++) { |
29 |
destroy(&world.objs[i]); |
30 |
} |
31 |
free(world.objs); |
32 |
} |
33 |
|
34 |
double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj) =
|
35 |
{ |
36 |
collide_poly |
37 |
}; |
38 |
int (*create_func[NUM_SHAPES])(object_t *obj, va_list ap) =
|
39 |
{ |
40 |
create_poly |
41 |
}; |
42 |
int (*destroy_func[NUM_SHAPES])(object_t *obj) =
|
43 |
{ |
44 |
destroy_poly |
45 |
}; |
46 |
|
47 |
|
48 |
|
49 |
double collide_circle(ray_t *ray, object_t *obj)
|
50 |
{ |
51 |
return -1; |
52 |
} |
53 |
|
54 |
double collide_seg(ray_t *ray, point_t p3, point_t p4)
|
55 |
{ |
56 |
double denom, nume_a, nume_b, ua, ub, xint, yint, dist;
|
57 |
point_t p1 = ray->p; |
58 |
point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)}; |
59 |
|
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);
|
61 |
denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y); |
62 |
if (denom == 0) { |
63 |
return RAY_MISS;
|
64 |
} |
65 |
nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x); |
66 |
nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x); |
67 |
|
68 |
ua = nume_a/denom; |
69 |
if (ua < 0 || ua > 1) { |
70 |
return RAY_MISS;
|
71 |
} |
72 |
|
73 |
ub = nume_b/denom; |
74 |
if (ub < 0) { |
75 |
return RAY_MISS;
|
76 |
} |
77 |
|
78 |
xint = p3.x+ua*(p4.x-p3.x); |
79 |
yint = p3.y+ua*(p4.y-p3.y); |
80 |
dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y)); |
81 |
return dist;
|
82 |
|
83 |
} |
84 |
|
85 |
double collide_poly(ray_t *ray, object_t *obj)
|
86 |
{ |
87 |
int i;
|
88 |
double min = RAY_MISS;
|
89 |
double x;
|
90 |
poly_t *p = (poly_t *) obj->props; |
91 |
|
92 |
if (obj->id != ID_POLY){
|
93 |
return -1; |
94 |
} |
95 |
|
96 |
for (i = 0; i < p->num_pts - 1; i++) { |
97 |
if ((x = collide_seg(ray, p->pts[i], p->pts[i+1])) < min){ |
98 |
min = x; |
99 |
} |
100 |
} |
101 |
if (p->type == POLY_CONNECTED) {
|
102 |
if ((x = collide_seg(ray,p->pts[i],p->pts[0])) < min) |
103 |
min = x; |
104 |
} |
105 |
return min;
|
106 |
} |
107 |
|
108 |
double collide(ray_t *ray, object_t *obj)
|
109 |
{ |
110 |
if (ray == NULL || obj == NULL) |
111 |
{ |
112 |
return -1; |
113 |
} |
114 |
return collide_func[obj->id+ID_OFFSET](ray, obj);
|
115 |
} |
116 |
|
117 |
|
118 |
|
119 |
object_t *create(int id, ...)
|
120 |
{ |
121 |
object_t *obj = &(world.objs[world.cur_objs++]); |
122 |
va_list args; |
123 |
|
124 |
va_start(args, id); |
125 |
if (create_func[id+ID_OFFSET](obj, args) < 0) { |
126 |
/* Cleanup ? */
|
127 |
} |
128 |
va_end(args); |
129 |
return obj;
|
130 |
} |
131 |
|
132 |
int destroy(object_t *obj)
|
133 |
{ |
134 |
if (obj->id == ID_NULL)
|
135 |
return 0; |
136 |
destroy_func[obj->id+ID_OFFSET](obj); |
137 |
obj->id = ID_NULL; |
138 |
} |
139 |
|
140 |
/**
|
141 |
*
|
142 |
**/
|
143 |
int create_poly(object_t *obj, va_list ap)
|
144 |
{ |
145 |
int i;
|
146 |
poly_t *p; |
147 |
|
148 |
int argc = va_arg(ap, int); |
149 |
int poly_type = va_arg(ap, int); |
150 |
|
151 |
p = malloc(sizeof(poly_t));
|
152 |
|
153 |
|
154 |
p->num_pts = argc; |
155 |
p->pts = malloc((argc) * sizeof(point_t));
|
156 |
p->type = poly_type; |
157 |
|
158 |
for(i=0;i < argc; i++) { |
159 |
p->pts[i].x = va_arg(ap, double);
|
160 |
p->pts[i].y = va_arg(ap, double);
|
161 |
} |
162 |
obj->id = ID_POLY; |
163 |
obj->bbox = NULL;
|
164 |
obj->props = p; |
165 |
return 1; |
166 |
} |
167 |
|
168 |
int create_rect(object_t *obj, va_list ap)
|
169 |
{ |
170 |
int i;
|
171 |
poly_t *p; |
172 |
int argc = va_arg(ap, int); |
173 |
} |
174 |
|
175 |
int destroy_poly (object_t *obj)
|
176 |
{ |
177 |
poly_t *p; |
178 |
p = (poly_t *)obj->props; |
179 |
if (p != NULL) free(p->pts); |
180 |
free(p); |
181 |
return 1; |
182 |
} |
183 |
|
184 |
void print_world(void) |
185 |
{ |
186 |
int i;
|
187 |
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);
|
188 |
for (i = 0; i < world.cur_objs; i++) { |
189 |
print_object(&world.objs[i]); |
190 |
} |
191 |
} |
192 |
void print_object(object_t *obj)
|
193 |
{ |
194 |
int i;
|
195 |
poly_t *p; |
196 |
switch (obj->id) {
|
197 |
case ID_NULL:
|
198 |
printf("\tNULL\n");
|
199 |
case ID_POLY:
|
200 |
p = (poly_t *) obj->props; |
201 |
printf("\tPOLYGON (%d points, %s) { ", p->num_pts, p->type?"connected" : "disconnected"); |
202 |
for (i = 0; i < p->num_pts; i++){ |
203 |
printf("(%g, %g) ", p->pts[i].x, p->pts[i].y);
|
204 |
} |
205 |
printf("}\n");
|
206 |
break;
|
207 |
default:
|
208 |
break;
|
209 |
|
210 |
} |
211 |
} |
212 |
|
213 |
|