Statistics
| Revision:

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