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