Preliminary, but functional, autotoolsification
[proto.git] / src / sim / spatialcomputer.cpp
blob43cff8f1184b478a5d8ee0168340f1ee92d6bea3
1 /* Top-level spatial computer classes
2 Copyright (C) 2005-2008, Jonathan Bachrach, Jacob Beal, and contributors
3 listed in the AUTHORS file in the MIT Proto distribution's top directory.
5 This file is part of MIT Proto, and is distributed under the terms of
6 the GNU General Public License, with a linking exception, as described
7 in the file LICENSE in the MIT Proto distribution's top directory. */
9 #include "config.h"
10 #include "spatialcomputer.h"
11 #include "visualizer.h"
13 /*****************************************************************************
14 * MISC DEFS *
15 *****************************************************************************/
17 Layer::Layer(SpatialComputer* p) { parent=p; can_dump=p->is_dump_default; }
19 /*****************************************************************************
20 * DEVICE *
21 *****************************************************************************/
22 int Device::top_uid=0; // originally, the device UIDs start at zero
24 Device::Device(SpatialComputer* parent, METERS *loc, DeviceTimer *timer) {
25 uid=top_uid++; this->timer = timer; this->parent = parent;
26 run_time=0; // should be reset at script-load
27 body = parent->physics->new_body(this,loc[0],loc[1],loc[2]);
28 // integrate w. layers, which may add devicelayers to the device
29 num_layers = parent->dynamics.max_id();
30 layers = (DeviceLayer**)calloc(num_layers,sizeof(DeviceLayer*));
31 for(int i=0;i<num_layers;i++)
32 { Layer* l = (Layer*)parent->dynamics.get(i); if(l) l->add_device(this); }
33 vm = allocate_machine(); // unusable until script is loaded
34 is_selected=FALSE; is_debug=FALSE;
37 // copy all state
38 Device* Device::clone_device(METERS *loc) {
39 Device* new_d = new Device(parent,loc,timer->clone_device());
41 //void Device::load_script(uint8_t* script, int len) {
42 // new_machine(vm, uid, 0, 0, 0, 1, script, len);
43 uint8_t *copy_src = vm->membuf, *copy_dst = new_d->vm->membuf;
44 for(int i=0;i<vm->memlen;i++) { copy_dst[i]=copy_src[i]; }
45 new_d->load_script(vm->script,vm->scripts[vm->cur_script].len);
46 // copy over state
48 new_d->vm->time = vm->time; new_d->vm->last_time = vm->last_time;
49 new_d->run_time=run_time; new_d->is_selected=is_selected;
50 new_d->is_debug=is_debug;
51 for(int i=0;i<num_layers;i++) {
52 DeviceLayer *d = layers[i], *nd = new_d->layers[i];
53 if(d && nd) nd->copy_state(d);
55 return new_d;
58 Device::~Device() {
59 delete timer;
60 delete body;
61 for(int i=0;i<num_layers;i++)
62 { DeviceLayer* d = (DeviceLayer*)layers[i]; if(d) delete d; }
63 free(layers);
64 deallocate_machine(&vm);
67 // dump function should produce matlab-readable data at verbosity 0
68 // and at verbosity 1+ should make human-readable
69 void Device::dump_state(FILE* out, int verbosity) {
70 // dump heading information
71 if(verbosity==0) {
72 fprintf(out,"%d %.2f %.2f",uid,vm->ticks,vm->time);
73 } else {
74 fprintf(out,"Device %d ",uid);
75 if(verbosity>=2) fprintf(out,"[in slot %d]",backptr);
76 fprintf(out,"(Internal time: %.2f %.2f)\n",vm->ticks,vm->time);
77 if(verbosity>=2)
78 fprintf(out,"Selected = %s, Debug = %s\n",bool2str(is_selected),
79 bool2str(is_debug));
81 // dump layers
82 if(parent->physics->can_dump) body->dump_state(out,verbosity);
83 for(int i=0;i<num_layers;i++) {
84 DeviceLayer* d = (DeviceLayer*)layers[i];
85 Layer* l = (Layer*)parent->dynamics.get(i);
86 if(d && l->can_dump) d->dump_state(out,verbosity);
88 // dump output
89 if(parent->is_dump_value) {
90 char buf[1000];
91 if(verbosity==0) {
92 post_stripped_data_to(buf, &vm->res);
93 fprintf(out," %s",buf);
94 } else {
95 post_data_to(buf, &vm->res);
96 fprintf(out,"Output %s\n",buf);
100 // dump neighborhood data
101 if(verbosity>=1 && parent->is_dump_hood) {
102 char buf[1000];
103 fprintf(out,"Export Values: "); // first export values
104 for(int i=0;i<vm->n_hood_vals;i++) {
105 post_data_to(buf,&vm->hood_exports[i]); fprintf(out,"%s ",buf);
107 fprintf(out,"\nNeighbor Values:\n"); // then neighbor data
108 for(int i=0;i<vm->n_hood;i++) {
109 NBR* nbr = vm->hood[i];
110 fprintf(out,
111 "Neighbor %4d [X=%.2f, Y=%.2f, Z=%.2f, Range=%.2f, Time=%.2f]: ",
112 nbr->id, nbr->x, nbr->y, nbr->z,
113 sqrt(nbr->x*nbr->x + nbr->y*nbr->y + nbr->z*nbr->z),
114 nbr->stamp);
115 if(nbr->stamp==-1) {
116 fprintf(out,"[INVALID]");
117 } else {
118 for(int j=0;j<vm->n_hood_vals;j++) {
119 post_data_to(buf,&nbr->imports[j]); fprintf(out,"%s ",buf);
122 fprintf(out,"\n");
125 if(verbosity==0) fprintf(out,"\n"); // terminate line
128 void Device::load_script(uint8_t* script, int len) {
129 new_machine(vm, uid, 0, 0, 0, 1, script, len);
132 // a convenient combined function
133 BOOL Device::debug() { return is_debug && parent->is_debug; }
135 // scale the display to draw text labels for a device
136 #define TEXT_SCALE 3.75 // arbitrary constant for text sizing
137 void Device::text_scale() {
138 #ifdef WANT_GLUT
139 flo d = body->display_radius(); glScalef(d,d,d); // scale text to body
140 d = vis_context->display_mag; glScalef(d,d,d); // then magnify as specified
141 glScalef(TEXT_SCALE,TEXT_SCALE,TEXT_SCALE);
142 #endif // WANT_GLUT
145 void Device::internal_event(SECONDS time, DeviceEvent type) {
146 switch(type) {
147 case COMPUTE:
148 body->preupdate(); // run the pre-compute update
149 for(int i=0;i<num_layers;i++)
150 { DeviceLayer* d = (DeviceLayer*)layers[i]; if(d) d->preupdate(); }
151 exec_machine(vm->ticks+1, time); // do the actual computation
152 body->update(); // run the post-compute update
153 for(int i=0;i<num_layers;i++)
154 { DeviceLayer* d = (DeviceLayer*)layers[i]; if(d) d->update(); }
155 break;
156 case BROADCAST:
157 export_machine();
158 if(script_export_needed() || (((int)vm->ticks) % 10)==0) {
159 export_script(); // send script every 10 rounds, or as needed
161 break;
165 BOOL Device::handle_key(KeyEvent* key) {
166 for(int i=0;i<num_layers;i++) {
167 DeviceLayer* d = (DeviceLayer*)layers[i];
168 if(d && d->handle_key(key)) return TRUE;
170 return FALSE;
173 void Device::visualize() {
174 #ifdef WANT_GLUT
175 glPushMatrix();
176 // center on device
177 const flo* p = body->position(); glTranslatef(p[0],p[1],p[2]);
178 // draw the body & other dynamics layers
179 body->visualize();
180 for(int i=0;i<num_layers;i++)
181 { DeviceLayer* d = (DeviceLayer*)layers[i]; if(d) d->visualize(); }
183 if(is_selected) {
184 palette->use_color(DEVICE_SELECTED);
185 draw_circle(4*body->display_radius());
187 if(debug()) {
188 palette->use_color(DEVICE_DEBUG);
189 glPushMatrix();
190 glTranslatef(0,0,-0.1);
191 draw_disk(2*body->display_radius());
192 glPopMatrix();
195 if (vis_context->is_show_vec) {
196 DATA *dst = &vm->res;
197 glPushMatrix();
198 glLineWidth(4);
199 glTranslatef(0, 0, 0);
200 switch (dst->tag) {
201 case VEC_TAG: {
202 VEC_VAL *v;
203 v = VEC_GET(dst);
204 if (v->n >= 2) {
205 flo x = NUM_GET(&v->elts[0]);
206 flo y = NUM_GET(&v->elts[1]);
207 flo z = v->n >= 3 ? NUM_GET(&v->elts[2]) : 0;
208 palette->use_color(VECTOR_BODY);
209 glBegin(GL_LINE_STRIP);
210 glVertex3f(0, 0, 0);
211 glVertex3f(0.8*x, 0.8*y, 0.8*z);
212 glEnd();
213 palette->use_color(VECTOR_TIP);
214 glBegin(GL_LINE_STRIP);
215 glVertex3f(0.8*x, 0.8*y, 0.8*z);
216 glVertex3f(x, y, z);
217 glEnd();
219 break; }
221 glLineWidth(1);
222 glPopMatrix();
225 text_scale(); // prepare to draw text
226 char buf[1024];
227 if (vis_context->is_show_id) {
228 palette->use_color(DEVICE_ID);
229 sprintf(buf, "%2d", uid);
230 draw_text(1, 1, buf);
233 if(vis_context->is_show_val) {
234 DATA *dst = &vm->res;
235 glPushMatrix();
236 //glTranslatef(0, 0, 0);
237 post_data_to(buf, dst);
238 palette->use_color(DEVICE_VALUE);
239 draw_text(1, 1, buf);
240 glPopMatrix();
243 if(vis_context->is_show_version) {
244 glPushMatrix();
245 palette->use_color(DEVICE_ID);
246 sprintf(buf, "%2d:%s", vm->scripts[vm->cur_script].version,
247 (vm->scripts[vm->cur_script].is_complete)?"OK":"wait");
248 draw_text(4, 4, buf);
249 glPopMatrix();
251 glPopMatrix();
252 #endif // WANT_GLUT
255 // Special render for OpenGL selection mode
256 void Device::render_selection() {
257 #ifdef WANT_GLUT
258 glPushMatrix();
259 // center on device
260 const flo* p = body->position(); glTranslatef(p[0],p[1],p[2]);
261 glLoadName(backptr); // set the name
262 body->render_selection(); // draw the body
263 glPopMatrix();
264 #endif // WANT_GLUT
267 /*****************************************************************************
268 * SIMULATED DEVICE *
269 *****************************************************************************/
270 class SimulatedDevice : public Device {
271 public:
272 SimulatedDevice(SpatialComputer* parent, METERS* loc, DeviceTimer *timer) :
273 Device(parent,loc,timer) {}
274 void dump_state(FILE* out, int verbosity) {
275 if(verbosity>=1) fprintf(out,"(Simulated) ");
276 Device::dump_state(out,verbosity);
282 /*****************************************************************************
283 * SPATIAL COMPUTER *
284 *****************************************************************************/
285 // get the spatial layout of the computer, including node distribution
286 void SpatialComputer::get_distribution(Args* args, int n) {
287 // spatial layout
288 int dimensions=2; // default to 2D world
289 METERS width=132, height=100, depth=0; // sides of the network distribution
290 if(args->extract_switch("-3d")) { depth=40; dimensions=3; }
291 if(args->extract_switch("-dim")) { // dim X [Y [Z]]
292 width=args->pop_number();
293 if(str_is_number(args->peek_next())) height=args->pop_number();
294 if(str_is_number(args->peek_next())) {
295 depth=args->pop_number(); dimensions=3;
298 if(dimensions==2)
299 volume=new Rect(-width/2, width/2, -height/2, height/2);
300 else
301 volume=new Rect3(-width/2, width/2, -height/2, height/2, -depth/2, depth);
302 // customizable distribution selection
303 choose_distribution(args,n);
306 // add the layer to dynamics and set its ID, for use in hardware callbacks
307 int SpatialComputer::addLayer(Layer* layer) {
308 return (layer->id = dynamics.add(layer));
311 SpatialComputer::SpatialComputer(Args* args) {
312 sim_time=0;
313 int n=(args->extract_switch("-n"))?(int)args->pop_number():100; // # devices
314 // load dumping variables
315 is_dump_default=TRUE;
316 args->undefault(&is_dump_default,"-Dall","-NDall");
317 is_dump_hood=is_dump_default;
318 args->undefault(&is_dump_hood,"-Dhood","-NDhood");
319 is_dump_value=is_dump_default;
320 args->undefault(&is_dump_value,"-Dvalue","-NDvalue");
321 is_dump = args->extract_switch("-D");
322 is_probe_filter = args->extract_switch("-probe-dump-filter");
323 is_show_snaps = !args->extract_switch("-no-dump-snaps");
324 dump_start = args->extract_switch("-dump-after") ? args->pop_number() : 0;
325 dump_period = args->extract_switch("-dump-period") ? args->pop_number() : 1;
326 dump_dir = args->extract_switch("-dump-dir") ? args->pop_next() : "dumps";
327 dump_stem = args->extract_switch("-dump-stem") ? args->pop_next() : "dump";
328 just_dumped=FALSE; next_dump = dump_start; snap_vis_time=0;
329 // setup customization
330 get_distribution(args,n); // how to get device positions
331 choose_time_model(args,n); // how device computation runs
332 choose_layers(args,n); // what types of physics apply
333 scheduler = new Scheduler(n, time_model->cycle_time());
334 // create the actual devices
335 METERS loc[3];
336 for(int i=0;i<n;i++) {
337 if(distribution->next_location(loc)) {
338 SECONDS start;
339 Device* d = new SimulatedDevice(this,loc,time_model->next_timer(&start));
340 d->backptr = devices.add(d);
341 scheduler->schedule_event((void*)d->backptr,start,0,COMPUTE,d->uid);
345 // load display variables
346 display_mag = (args->extract_switch("-mag"))?args->pop_number():1;
347 is_show_val = args->extract_switch("-v");
348 is_show_vec = args->extract_switch("-sv");
349 is_show_id = args->extract_switch("-i");
350 is_show_version = args->extract_switch("-show-script-version");
351 is_debug = args->extract_switch("-g");
352 hardware.is_kernel_trace = args->extract_switch("-t");
353 hardware.is_kernel_debug = args->extract_switch("-debug-kernel");
354 hardware.is_kernel_debug_script = args->extract_switch("-debug-script");
357 SpatialComputer::~SpatialComputer() {
358 // delete devices first, because their "death" needs dynamics to still exist
359 for(int i=0;i<devices.max_id();i++)
360 { Device* d = (Device*)devices.get(i); if(d) delete d; }
361 // delete everything else in arbitrary order
362 delete scheduler; delete volume; delete time_model; delete distribution;
363 for(int i=0;i<dynamics.max_id();i++)
364 { Layer* ec = (Layer*)dynamics.get(i); if(ec) delete ec; }
367 // for the initial loading only
368 void SpatialComputer::load_script(uint8_t* script, int len) {
369 for(int i=0;i<devices.max_id();i++) {
370 Device* d = (Device*)devices.get(i);
371 if(d) {
372 hardware.set_vm_context(d);
373 d->load_script(script,len);
377 // install a script by injecting it as packets w. the next version
378 void SpatialComputer::load_script_at_selection(uint8_t* script, int len) {
379 for(int i=0;i<selection.max_id();i++) {
380 Device* d = (Device*)devices.get((long)selection.get(i));
381 if(d) {
382 hardware.set_vm_context(d);
383 version++;
384 for (i=0; i < (int)floor(len / (float)MAX_SCRIPT_PKT); i++) {
385 radio_receive_script_pkt(version, len, i, &script[i*MAX_SCRIPT_PKT]);
387 if (len % MAX_SCRIPT_PKT) {
388 radio_receive_script_pkt(version, len, i, &script[i*MAX_SCRIPT_PKT]);
394 BOOL SpatialComputer::handle_key(KeyEvent* key) {
395 // is this a key recognized internally?
396 if(key->normal && !key->ctrl) {
397 switch(key->key) {
398 case 'i': is_show_id = !is_show_id; return TRUE;
399 case 'v': is_show_vec = !is_show_vec; return TRUE;
400 case 'n': is_show_val = !is_show_val; return TRUE;
401 case 'j': is_show_version = !is_show_version; return TRUE;
402 case 'U': selection.clear(); update_selection(); return TRUE;
403 case 'a': hardware.is_kernel_trace = !hardware.is_kernel_trace; return TRUE;
404 case 'd': is_debug = !is_debug; return TRUE;
405 case 'D':
406 for(int i=0;i<selection.max_id();i++) {
407 Device* d = (Device*)devices.get((long)selection.get(i));
408 if(d) d->is_debug = !d->is_debug;
410 return TRUE;
411 case '8': is_probe_filter=TRUE; is_dump = TRUE; return TRUE;
412 case '9': is_probe_filter=FALSE; is_dump = TRUE; return TRUE;
413 case '0': is_dump = FALSE; return TRUE;
414 case 'Z': dump_frame(sim_time,TRUE); return TRUE;
415 break;
418 // is this key recognized by the selected nodes?
419 // try it in the various sensor/actuator layers
420 if(physics->handle_key(key)) return TRUE;
421 for(int i=0;i<dynamics.max_id();i++) {
422 Layer* d = (Layer*)dynamics.get(i);
423 if(d && d->handle_key(key)) return TRUE;
425 BOOL in_selection = FALSE;
426 for(int i=0;i<selection.max_id();i++) {
427 Device* d = (Device*)devices.get((long)selection.get(i));
428 if(d) in_selection |= d->handle_key(key);
430 return FALSE;
433 BOOL SpatialComputer::handle_mouse(MouseEvent* mouse) {
434 return FALSE;
437 SpatialComputer* vis_context;
438 extern double get_real_secs ();
439 #define FLASH_TIME 0.1 // time that a snap flashes the background
440 void SpatialComputer::visualize() {
441 #ifdef WANT_GLUT
442 vis_context=this;
443 physics->visualize();
444 for(int i=0;i<dynamics.max_id();i++)
445 { Layer* d = (Layer*)dynamics.get(i); if(d) d->visualize(); }
446 for(int i=0;i<devices.max_id();i++)
447 { Device* d = (Device*)devices.get(i); if(d) d->visualize(); }
448 // show "photo flashes" when dumps have occured
449 SECONDS time = get_real_secs();
450 if(just_dumped) { just_dumped=FALSE; snap_vis_time = time; }
451 BOOL flash = is_show_snaps && (time-snap_vis_time) < FLASH_TIME;
452 palette->set_background(flash ? PHOTO_FLASH : BACKGROUND);
453 #endif // WANT_GLUT
456 // special render for OpenGL selecting mode
457 void SpatialComputer::render_selection() {
458 vis_context=this;
459 for(int i=0;i<devices.max_id();i++)
460 { Device* d = (Device*)devices.get(i); if(d) d->render_selection(); }
462 void SpatialComputer::update_selection() {
463 for(int i=0;i<devices.max_id();i++) // clear old selection bits
464 { Device* d = (Device*)devices.get(i); if(d) d->is_selected=FALSE; }
465 for(int i=0;i<selection.max_id();i++) { // set new selection bits
466 int n = (long)selection.get(i);
467 Device* d = (Device*)devices.get(n);
468 if(d) d->is_selected=TRUE;
471 void SpatialComputer::drag_selection(flo* delta) {
472 if(volume->dimensions()==2) { // project back onto the surface
473 delta[2]=0;
475 if(delta[0]==0 && delta[1]==0 && delta[2]==0) return;
476 for(int i=0;i<selection.max_id();i++) { // move each selected device
477 int n = (long)selection.get(i);
478 Device* d = (Device*)devices.get(n);
479 if(d) {
480 const flo* p = d->body->position(); // calc new position
481 d->body->set_position(p[0]+delta[0],p[1]+delta[1],p[2]+delta[2]);
482 for(int j=0;j<dynamics.max_id();j++) // move the device
483 { Layer* dyn = (Layer*)dynamics.get(j); if(dyn) dyn->device_moved(d); }
488 BOOL SpatialComputer::evolve(SECONDS limit) {
489 SECONDS dt = limit-sim_time;
490 // evolve world
491 physics->evolve(dt);
492 for(int i=0;i<devices.max_id();i++) { // tell layers about moving devices
493 Device* d = (Device*)devices.get(i);
494 if(d && d->body->moved) {
495 for(int j=0;j<dynamics.max_id();j++)
496 { Layer* dyn = (Layer*)dynamics.get(j); if(dyn) dyn->device_moved(d); }
497 d->body->moved=FALSE;
500 // evolve devices
501 Event e; scheduler->set_bound(limit);
502 while(scheduler->pop_next_event(&e)) {
503 int id = (long)e.target;
504 Device* d = (Device*)devices.get(id);
505 if(d && d->uid==e.uid) {
506 sim_time=e.true_time; // set time to new value
507 hardware.set_vm_context(d); // align kernel/sim patch for this device
508 d->internal_event(e.internal_time,(DeviceEvent)e.type);
509 if(e.type==COMPUTE) {
510 d->run_time = e.internal_time;
511 SECONDS tt, it; // true and internal time
512 d->timer->next_compute(&tt,&it); tt+=sim_time; it+=d->run_time;
513 scheduler->schedule_event((void*)id,tt,it,COMPUTE,d->uid);
514 d->timer->next_transmit(&tt,&it); tt+=sim_time; it+=d->run_time;
515 scheduler->schedule_event((void*)id,tt,it,BROADCAST,d->uid);
519 sim_time=limit;
521 // clone or kill devices (at end of update period)
522 while(!death_q.empty()) {
523 int id = death_q.front(); death_q.pop(); // get next to kill
524 Device* d = (Device*)devices.get(id);
525 if(d) {
526 // scheduled events for dead devices are ignored; need not be deleted
527 if(d->is_selected) { // fix selection (if needed)
528 for(int i=0;i<selection.max_id();i++) {
529 int n = (long)selection.get(i);
530 if(n==id) selection.remove(i);
533 delete d;
534 devices.remove(id);
537 while(!clone_q.empty()) {
538 CloneReq* cr = clone_q.front(); clone_q.pop(); // get next to clone
539 Device* d = (Device*)devices.get(cr->id);
540 if(d && d==cr->parent) { // check device: might have been deleted
541 Device* new_d = d->clone_device(cr->child_pos);
542 new_d->backptr = devices.add(new_d);
543 if(new_d->is_selected) { selection.add((void*)new_d->backptr); }
544 // schedule next event
545 SECONDS tt, it; // true and internal time
546 new_d->timer->next_compute(&tt,&it); tt+=sim_time; it+=new_d->run_time;
547 scheduler->schedule_event((void*)new_d->backptr,tt,it,COMPUTE,
548 new_d->uid);
550 delete cr;
553 // dump if needed
554 if(is_dump && sim_time >= dump_start && sim_time >= next_dump) {
555 dump_frame(next_dump,FALSE);
556 while(next_dump <= sim_time) next_dump+=dump_period;
559 return TRUE;
562 /*****************************************************************************
563 * DUMPING FACILITY *
564 *****************************************************************************/
566 void SpatialComputer::dump_selection(FILE* out, int verbosity) {
567 for(int i=0;i<selection.max_id();i++) {
568 int n = (long)selection.get(i);
569 Device* d = (Device*)devices.get(n); if(d) d->dump_state(out,verbosity);
573 void SpatialComputer::dump_state(FILE* out) {
574 for(int i=0;i<devices.max_id();i++)
575 { Device* d = (Device*)devices.get(i); if(d) d->dump_state(out,0); }
578 void SpatialComputer::dump_header(FILE* out) {
579 fprintf(out,"% \"UID\" \"TICKS\" \"TIME\""); // device fields
580 physics->dump_header(out);
581 for(int i=0;i<dynamics.max_id();i++)
582 { Layer* d = (Layer*)dynamics.get(i); if(d) d->dump_header(out); }
583 if(is_dump_value) fprintf(out," \"OUT\"");
584 fprintf(out,"\n");
587 void SpatialComputer::dump_frame(SECONDS time, BOOL time_in_name) {
588 char buf[1000];
589 // ensure that the directory exists
590 snprintf(buf, 1000, "mkdir -p %s", dump_dir); system(buf);
591 // open the file
592 if(time_in_name)
593 sprintf(buf,"%s/%s%.2f-%.2f.log",dump_dir,dump_stem,get_real_secs(),time);
594 else
595 sprintf(buf,"%s/%s%.2f.log",dump_dir,dump_stem,time);
596 FILE* out = fopen(buf,"w");
597 if(out==NULL) { post("Unable to open dump file '%s'\n",buf); return; }
598 // output all the state
599 dump_header(out); dump_state(out);
600 fclose(out); // close the file
601 just_dumped = TRUE; // prime drawing to flash