Revision 1026

View differences:

branches/simulator/projects/simulator/simulator/core/world.c
7 7
 **/
8 8

  
9 9
#include <stdio.h>
10
#include <stdlib.h>
11
#include <math.h>
12
#include <stdarg.h>
10 13
#include "world.h"
11 14

  
12 15

  
13
double collide_circle(ray_t *ray, object_t *obj){
16

  
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
{
14 27
    return -1;
15 28
}
16
double collide_rect(ray_t *ray, object_t *obj){
17
    return -2;
29

  
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

  
18 59
}
60
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;
19 66

  
20
double collide(ray_t *ray, object_t *obj) {
67
    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
{
21 114
    if (ray == NULL || obj == NULL)
115
    {
22 116
	return -1;
117
    }
23 118
    return collide_func[obj->id](ray, obj);
24 119
}
120

  
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

  
218

  
branches/simulator/projects/simulator/simulator/core/world.h
11 11
#ifndef __WORLD_H__
12 12
#define __WORLD_H__
13 13

  
14
#define ID_RECTANGLE 0
14
#include <stdarg.h>
15

  
16
#define ID_RECTANGLE 3
15 17
#define ID_CIRCLE 1
16
#define NUM_SHAPES 2
17 18

  
19
#define ID_POLY 0
20
#define POLY_DISCONNECTED 0
21
#define POLY_CONNECTED 1
22
#define POLY_RECT 2
23

  
24
#define NUM_SHAPES 3
25
#define RAY_MISS 1E30
26
#define CREATE(id, ...) (create_func[id](__VA_ARGS__))
18 27
typedef struct {
19 28
    double x;
20 29
    double y;
......
23 32
typedef struct {
24 33
    point_t p1; /* Lower left point */
25 34
    point_t p2; /* Upper right point */
26
} bbox_t;
35
} bbox_t, rect_t, seg_t;
27 36

  
28 37
typedef struct {
29 38
    point_t p; /* origin */
......
31 40
} ray_t;
32 41

  
33 42
typedef struct {
43
    int type;
44
    int num_pts;
45
    point_t *pts;
46
} poly_t;
47

  
48
typedef struct {
34 49
    int id;
35 50
    bbox_t *bbox;
36 51
    void *props; /* shape-specific properties */
......
43 58
} world_t;
44 59

  
45 60
/* Specific collision functions */
61
double collide(ray_t *ray, object_t *obj);
46 62
double collide_circle(ray_t *ray, object_t *obj);
47 63
double collide_rect(ray_t *ray, object_t *obj);
48

  
64
double collide_poly(ray_t *ray, object_t *obj);
49 65
/*  Array of function pointers to the specific collide functions.
50 66
 *  Must be listed in the same order as IDs */
51
double  (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj) = 
52
    {
53
	collide_rect,
54
	collide_circle 
55
    };
67
extern double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj);
68
extern int (*create_func[NUM_SHAPES])(object_t *obj, va_list ap);
56 69

  
70

  
71
int create_poly(object_t *obj, va_list ap);
72
object_t *create(int id, ...);
73

  
74
void destroy_poly (object_t *obj);
57 75
#endif
58 76

  

Also available in: Unified diff