Statistics
| Revision:

root / branches / simulator / projects / simulator / simulator / gui / gtk_environment_view.c @ 1086

History | View | Annotate | Download (12.9 KB)

1
#include <math.h>
2
#include <stdlib.h>
3
#include <stdio.h>
4
#include <sys/time.h>
5
#include <string.h>
6

    
7
#include <gtk/gtkmain.h>
8
#include <gtk/gtksignal.h>
9

    
10
#include "gtk_environment_view.h"
11

    
12
#include "robot.h"
13
#include "world.h"
14

    
15
#define ENVIRONMENT_VIEW_MIN_WIDTH                100
16
#define ENVIRONMENT_VIEW_MIN_HEIGHT                50
17

    
18
#define ROBOT_DIAMETER                                        30
19

    
20
static GtkWidgetClass* parent_class = NULL;
21

    
22
static void gtk_environment_view_class_init(GtkEnvironmentViewClass* environmentClass);
23
static void gtk_environment_view_init(GtkEnvironmentView* view);
24
static void gtk_environment_view_destroy(GtkObject* object);
25

    
26
static void gtk_environment_view_size_request(GtkWidget* widget,
27
                GtkRequisition* req);
28
static void gtk_environment_view_realize(GtkWidget* widget);
29
static void gtk_environment_view_size_allocate(GtkWidget* widget, GtkAllocation* a);
30
static gboolean gtk_environment_view_expose(GtkWidget* widget,
31
                GdkEventExpose* event);
32

    
33
static gboolean gtk_environment_view_mouse_down(GtkWidget* widget,
34
                                GdkEventButton* event);
35
static gboolean gtk_environment_view_mouse_release(GtkWidget* widget,
36
                                GdkEventButton* event);
37
static gboolean gtk_environment_view_mouse_move(GtkWidget* widget,
38
                                GdkEventMotion* event);
39

    
40

    
41
static void draw_world(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, world_t* world);
42
static void draw_robot(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, 
43
                        float x, float y, float angle);
44

    
45
static void to_world_coordinates(GtkEnvironmentView* view, int x, int y, float* world_x, float* world_y);
46
static void to_window_coordinates(GtkEnvironmentView* view, float x, float y, int* win_x, int* win_y);
47
static void to_window_scale(GtkEnvironmentView* view, float v, int* win_v);
48

    
49

    
50
GtkType gtk_environment_view_get_type(void)
51
{
52
        static GtkType environment_view_type = 0;
53
        if (!environment_view_type)
54
        {
55
                static const GtkTypeInfo environment_view_info =
56
                {
57
                        "GtkEnvironmentView",
58
                        sizeof(GtkEnvironmentView),
59
                        sizeof(GtkEnvironmentViewClass),
60
                        (GtkClassInitFunc)gtk_environment_view_class_init,
61
                        (GtkObjectInitFunc)gtk_environment_view_init,
62
                        NULL, NULL, (GtkClassInitFunc)NULL
63
                };
64

    
65
                environment_view_type = gtk_type_unique(GTK_TYPE_WIDGET,
66
                        &environment_view_info);
67
        }
68

    
69
        return environment_view_type;
70
}
71

    
72
static void gtk_environment_view_class_init(GtkEnvironmentViewClass* environmentClass)
73
{
74
        GtkObjectClass* object_class;
75
        GtkWidgetClass* widget_class;
76

    
77
        object_class = (GtkObjectClass*)environmentClass;
78
        widget_class = (GtkWidgetClass*)environmentClass;
79

    
80
        parent_class = gtk_type_class(gtk_widget_get_type());
81

    
82
        object_class->destroy = gtk_environment_view_destroy;
83

    
84
        widget_class->realize = gtk_environment_view_realize;
85
        widget_class->expose_event = gtk_environment_view_expose;
86
        widget_class->size_request = gtk_environment_view_size_request;
87
        widget_class->size_allocate = gtk_environment_view_size_allocate;
88
        widget_class->button_press_event = gtk_environment_view_mouse_down;
89
        widget_class->button_release_event = gtk_environment_view_mouse_release;
90
        widget_class->motion_notify_event = gtk_environment_view_mouse_move;
91
}
92

    
93
GtkWidget* gtk_environment_view_new(void)
94
{
95
        GtkEnvironmentView* view;
96
        view = gtk_type_new(gtk_environment_view_get_type());
97

    
98
        return GTK_WIDGET(view);
99
}
100

    
101
static void gtk_environment_view_init(GtkEnvironmentView* view)
102
{
103
        view->topLeftX = -2.0;
104
        view->topLeftY = -1.3;
105
        view->scale = 2.0 / 800.0;
106
        view->width = -1;
107
        view->height = -1;
108

    
109
        GTK_WIDGET_SET_FLAGS(&(view->widget), GTK_DOUBLE_BUFFERED);
110

    
111
        view->mouseDown = 0;
112
}
113

    
114
static void gtk_environment_view_destroy(GtkObject* object)
115
{
116
        GtkEnvironmentView* view;
117
        g_return_if_fail(object != NULL);
118
        g_return_if_fail(GTK_IS_ENVIRONMENT_VIEW(object));
119

    
120
        view = GTK_ENVIRONMENT_VIEW(object);
121

    
122
        if (GTK_OBJECT_CLASS(parent_class)->destroy)
123
                (* GTK_OBJECT_CLASS(parent_class)->destroy) (object);
124
}
125

    
126
static void gtk_environment_view_realize(GtkWidget* widget)
127
{
128
        GtkEnvironmentView* view;
129
        GdkWindowAttr attributes;
130
        gint attributes_mask;
131

    
132
        g_return_if_fail( widget != NULL);
133
        g_return_if_fail( GTK_IS_ENVIRONMENT_VIEW(widget));
134

    
135
        GTK_WIDGET_SET_FLAGS(widget, GTK_REALIZED);
136
        view = GTK_ENVIRONMENT_VIEW(widget);
137

    
138
        attributes.x = widget->allocation.x;
139
        attributes.y = widget->allocation.y;
140
        attributes.width = widget->allocation.width;
141
        attributes.height = widget->allocation.height;
142
        attributes.wclass = GDK_INPUT_OUTPUT;
143
        attributes.window_type = GDK_WINDOW_CHILD;
144
        attributes.event_mask = gtk_widget_get_events(widget) |
145
                GDK_EXPOSURE_MASK | GDK_BUTTON_PRESS_MASK |
146
                GDK_BUTTON_RELEASE_MASK | GDK_POINTER_MOTION_MASK |
147
                GDK_POINTER_MOTION_HINT_MASK;
148
        attributes.visual = gtk_widget_get_visual(widget);
149
        attributes.colormap = gtk_widget_get_colormap(widget);
150

    
151
        attributes_mask = GDK_WA_X | GDK_WA_Y | GDK_WA_VISUAL | GDK_WA_COLORMAP;
152
        widget->window = gdk_window_new(widget->parent->window, &attributes,
153
                        attributes_mask);
154
        gdk_window_set_user_data(widget->window, widget);
155
        widget->style = gtk_style_attach(widget->style, widget->window);
156
        gtk_style_set_background(widget->style, widget->window,
157
                        GTK_STATE_NORMAL);
158
}
159

    
160
static void gtk_environment_view_size_request(GtkWidget* widget, 
161
                GtkRequisition* req)
162
{
163
        if (req->width < ENVIRONMENT_VIEW_MIN_WIDTH)
164
                req->width = ENVIRONMENT_VIEW_MIN_WIDTH;
165
        if (req->height < ENVIRONMENT_VIEW_MIN_HEIGHT)
166
                req->height = ENVIRONMENT_VIEW_MIN_HEIGHT;
167
}
168

    
169
static void gtk_environment_view_size_allocate(GtkWidget* widget, GtkAllocation* a)
170
{
171
        GtkEnvironmentView* view;
172

    
173
        g_return_if_fail(widget != NULL);
174
        g_return_if_fail(GTK_IS_ENVIRONMENT_VIEW(widget));
175
        g_return_if_fail(a != NULL);
176

    
177
        view = GTK_ENVIRONMENT_VIEW(widget);
178
        widget->allocation = *a;
179

    
180
        view->width = a->width;
181
        view->height = a->height;
182

    
183
        if (GTK_WIDGET_REALIZED(widget))
184
        {
185
                gdk_window_move_resize(widget->window, a->x, a->y, a->width, a->height);
186
        }
187
}
188

    
189
static gboolean gtk_environment_view_expose(GtkWidget* widget,
190
                GdkEventExpose* event)
191
{
192
        GtkEnvironmentView* view;
193

    
194
        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)
195
                return FALSE;
196
        if (event->count > 0)
197
                return FALSE;
198
        
199
        view = GTK_ENVIRONMENT_VIEW(widget);
200

    
201
        draw_world(view, widget->window, widget->style->fg_gc[GTK_WIDGET_STATE(widget)], &world);
202
        
203
        robot_iterator_reset();
204
        Robot* r;
205
        while ((r = robot_iterator_next()) != NULL) {
206
                draw_robot(view, widget->window, 
207
                        widget->style->fg_gc[GTK_WIDGET_STATE(widget)], 
208
            r->pose.x, r->pose.y, r->pose.theta);
209
    }
210

    
211
        return FALSE;
212
}
213

    
214
static gboolean gtk_environment_view_mouse_down(GtkWidget* widget,
215
                                GdkEventButton* event)
216
{
217
        GtkEnvironmentView* view;
218
        Robot* r;
219
        float closestdist;
220
        int closest;
221

    
222
        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)
223
                return FALSE;
224

    
225
        view = GTK_ENVIRONMENT_VIEW(widget);
226

    
227
        if (event->button != 1)
228
                return FALSE;
229

    
230
        view->mouseDownX = event->x;
231
        view->mouseDownY = event->y;
232
        view->mouseX = event->x;
233
        view->mouseY = event->y;
234
        view->mouseDown = 1;
235
        
236
        /*closestdist = 10e8;
237
        robot_iterator_reset();
238
        while ((r = robot_iterator_next()) != NULL) {
239
        {
240
                float dist = r->pose.x - 
241
        }*/
242

    
243
        return FALSE;
244
}
245

    
246
static gboolean gtk_environment_view_mouse_release(GtkWidget* widget,
247
                                GdkEventButton* event)
248
{
249
        GtkEnvironmentView* view;
250
        double x1, x2, y1, y2;
251
        int w, h;
252

    
253
        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)
254
                return FALSE;
255

    
256
        view = GTK_ENVIRONMENT_VIEW(widget);
257

    
258
        if (event->button != 1 && event->button != 3)
259
                return FALSE;
260
        
261
        if (event->button == 1)
262
                view->mouseDown = 0;
263
        view->mouseX = event->x;
264
        view->mouseY = event->y;
265
        w = abs(view->mouseX - view->mouseDownX);
266
        h = abs(view->mouseY - view->mouseDownY);
267
        
268
        // if the box is big, zoom to the box
269
        if (event->button == 1 && (w >= 10 || h >= 10))
270
        {
271
                // we need to make sure the box is to scale
272
                // scale to height
273
                if (fabs((double)w / h) <= fabs((double)view->width / view->height))
274
                {
275
                        w = (int)((double)view->width / view->height * h);
276
                        if (view->mouseX < view->mouseDownX)
277
                                view->mouseX = view->mouseDownX - w;
278
                        else
279
                                view->mouseX = view->mouseDownX + w;
280
                }
281
                // scale to width
282
                else
283
                {
284
                        h = (int)((double)view->height / view->width * w);
285
                        if (view->mouseY < view->mouseDownY)
286
                                view->mouseY = view->mouseDownY - h;
287
                        else
288
                                view->mouseY = view->mouseDownY + h;
289
                }
290
        
291
                x1 = view->mouseX * view->scale + view->topLeftX;
292
                y1 = view->mouseY * view->scale + view->topLeftY;
293
                x2 = view->mouseDownX * view->scale + view->topLeftX;
294
                y2 = view->mouseDownY * view->scale + view->topLeftY;
295
                if (x2 < x1)
296
                        x1 = x2;
297
                if (y2 < y1)
298
                        y1 = y2;
299

    
300
                view->scale = (w * view->scale) / view->width;
301
                view->topLeftX = x1;
302
                view->topLeftY = y1;
303
        }
304
        else
305
        {
306
                x1 = view->mouseX * view->scale + view->topLeftX;
307
                y1 = view->mouseY * view->scale + view->topLeftY;
308

    
309
                // zoom in on left click, out on right click
310
                if (event->button == 1)
311
                        view->scale /= 2.0;
312
                else
313
                        view->scale *= 2.0;
314
                view->topLeftX = x1 - view->width / 2 * view->scale;
315
                view->topLeftY = y1 - view->height / 2 * view->scale;
316
        }
317

    
318
        return FALSE;
319
}
320

    
321
static gboolean gtk_environment_view_mouse_move(GtkWidget* widget,
322
                                GdkEventMotion* event)
323
{
324
        GtkEnvironmentView* view;
325

    
326
        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)
327
                return FALSE;
328

    
329
        view = GTK_ENVIRONMENT_VIEW(widget);
330

    
331
        view->mouseX = event->x;
332
        view->mouseY = event->y;
333

    
334
        if (!view->mouseDown)
335
                return FALSE;
336

    
337
        // we need to make sure the new view is to scale
338
        /*int w = abs(view->mouseX - view->mouseDownX);
339
        int h = abs(view->mouseY - view->mouseDownY);
340
        // scale to height
341
        if (fabs((double)w / h) <= fabs((double)view->width / view->height))
342
        {
343
                w = (int)((double)view->width / view->height * h);
344
                if (view->mouseX < view->mouseDownX)
345
                        view->mouseX = view->mouseDownX - w;
346
                else
347
                        view->mouseX = view->mouseDownX + w;
348
        }
349
        // scale to width
350
        else
351
        {
352
                h = (int)((double)view->height / view->width * w);
353
                if (view->mouseY < view->mouseDownY)
354
                        view->mouseY = view->mouseDownY - h;
355
                else
356
                        view->mouseY = view->mouseDownY + h;
357
        }*/
358

    
359
        gtk_widget_queue_draw_area(widget, 0, 0, view->width, view->height);
360

    
361
        return FALSE;
362
}
363

    
364
void gtk_environment_view_refresh(GtkWidget* view)
365
{
366
        gdk_threads_enter();
367
        gtk_widget_queue_draw(view);
368
        gdk_threads_leave();
369
}
370

    
371
void to_world_coordinates(GtkEnvironmentView* view, int x, int y, float* world_x, float* world_y)
372
{
373
        double leftx = world.win.p1.x;
374
        double upy = world.win.p1.x;
375
        double xwidth = world.win.p2.x - leftx;
376
        double yheight = world.win.p2.y - upy;
377
        int width = view->width, height = view->height;
378
        if (xwidth / width < yheight / height)
379
                width = (int)(height / yheight * xwidth);
380
        else
381
                height = (int)(width / xwidth * yheight);
382

    
383
        *world_x = (float)x / width * xwidth + leftx;
384
        *world_y = (float)y / height * yheight + upy;
385
}
386

    
387
void to_window_coordinates(GtkEnvironmentView* view, float x, float y, int* win_x, int* win_y)
388
{
389
        double leftx = world.win.p1.x;
390
        double upy = world.win.p1.x;
391
        double xwidth = world.win.p2.x - leftx;
392
        double yheight = world.win.p2.y - upy;
393
        int width = view->width, height = view->height;
394
        if (xwidth / width < yheight / height)
395
                width = (int)(height / yheight * xwidth);
396
        else
397
                height = (int)(width / xwidth * yheight);
398

    
399
        *win_x = (int)((x - leftx) / xwidth * width);
400
        *win_y = (int)((y - upy) / yheight * height);
401
}
402

    
403
void to_window_scale(GtkEnvironmentView* view, float v, int* win_v)
404
{
405
        double leftx = world.win.p1.x;
406
        double upy = world.win.p1.x;
407
        double xwidth = world.win.p2.x - leftx;
408
        double yheight = world.win.p2.y - upy;
409
        int width = view->width, height = view->height;
410
        if (xwidth / width < yheight / height)
411
                width = (int)(height / yheight * xwidth);
412
        else
413
                height = (int)(width / xwidth * yheight);
414
        
415
        *win_v = (int)(v / xwidth * width);
416
}
417

    
418
static void draw_world(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, world_t* world)
419
{
420
        int i, j;
421
        GdkPoint* points;
422
        poly_t* p;
423

    
424
        if (!drawable || !gc || !world)
425
                return;
426

    
427
        for (i = 0 ; i < world->max_objs; i++)
428
        {
429
                object_t* o = &(world->objs[i]);
430
                switch (o->id)
431
                {
432
                        case ID_POLY:
433
                                p = (poly_t*)o->props;
434
                                points = (GdkPoint*)malloc(sizeof(GdkPoint) * p->num_pts);
435
                                for (j = 0; j < p->num_pts; j++)
436
                                        to_window_coordinates(view, p->pts[j].x, p->pts[j].y, &(points[j].x), &(points[j].y));
437
                                switch(p->type)
438
                                {
439
                                        case POLY_CONNECTED:
440
                                                gdk_draw_polygon(drawable, gc, 1, points, p->num_pts);
441
                                                break;
442
                                        default:
443
                                                fprintf(stderr, "Detected unexpected type of polygon.\n");
444
                                                break;
445
                                }
446
                                free(points);
447
                                break;
448
                        // no object
449
                        case ID_NULL:
450
                                break;
451
                        default:
452
                                fprintf(stderr, "Unexpected object type %d in world.\n", o->id);
453
                                break;
454
                }
455
        }
456
}
457

    
458
static void draw_robot(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, 
459
                        float x, float y, float angle)
460
{
461
        int rx, ry;
462
        int rox, roy;
463
        if (!drawable || !gc)
464
                return;
465
        
466
        to_window_scale(view, ROBOT_DIAMETER, &rx);
467
        to_window_scale(view, ROBOT_DIAMETER, &ry);
468
        to_window_coordinates(view, x, y, &rox, &roy);
469

    
470
        gdk_draw_arc(drawable, gc, FALSE, (int)(rox - (float)rx / 2), (int)(roy - (float)ry / 2), 
471
                rx, ry, 0, 360*64);
472

    
473
        gdk_draw_line(drawable, gc, rox, roy, 
474
                rox + (rx / 1.8) * cos(-angle),
475
                roy - (ry / 1.8) * sin(-angle));
476
}
477