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. */
10 #include "spatialcomputer.h"
11 #include "visualizer.h"
13 /*****************************************************************************
15 *****************************************************************************/
17 Layer::Layer(SpatialComputer
* p
) { parent
=p
; can_dump
=p
->is_dump_default
; }
19 /*****************************************************************************
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
;
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
);
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
);
61 for(int i
=0;i
<num_layers
;i
++)
62 { DeviceLayer
* d
= (DeviceLayer
*)layers
[i
]; if(d
) delete d
; }
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
72 fprintf(out
,"%d %.2f %.2f",uid
,vm
->ticks
,vm
->time
);
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
);
78 fprintf(out
,"Selected = %s, Debug = %s\n",bool2str(is_selected
),
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
);
89 if(parent
->is_dump_value
) {
92 post_stripped_data_to(buf
, &vm
->res
);
93 fprintf(out
," %s",buf
);
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
) {
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
];
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
),
116 fprintf(out
,"[INVALID]");
118 for(int j
=0;j
<vm
->n_hood_vals
;j
++) {
119 post_data_to(buf
,&nbr
->imports
[j
]); fprintf(out
,"%s ",buf
);
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() {
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
);
145 void Device::internal_event(SECONDS time
, DeviceEvent type
) {
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(); }
158 if(script_export_needed() || (((int)vm
->ticks
) % 10)==0) {
159 export_script(); // send script every 10 rounds, or as needed
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
;
173 void Device::visualize() {
177 const flo
* p
= body
->position(); glTranslatef(p
[0],p
[1],p
[2]);
178 // draw the body & other dynamics layers
180 for(int i
=0;i
<num_layers
;i
++)
181 { DeviceLayer
* d
= (DeviceLayer
*)layers
[i
]; if(d
) d
->visualize(); }
184 palette
->use_color(DEVICE_SELECTED
);
185 draw_circle(4*body
->display_radius());
188 palette
->use_color(DEVICE_DEBUG
);
190 glTranslatef(0,0,-0.1);
191 draw_disk(2*body
->display_radius());
195 if (vis_context
->is_show_vec
) {
196 DATA
*dst
= &vm
->res
;
199 glTranslatef(0, 0, 0);
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
);
211 glVertex3f(0.8*x
, 0.8*y
, 0.8*z
);
213 palette
->use_color(VECTOR_TIP
);
214 glBegin(GL_LINE_STRIP
);
215 glVertex3f(0.8*x
, 0.8*y
, 0.8*z
);
225 text_scale(); // prepare to draw text
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
;
236 //glTranslatef(0, 0, 0);
237 post_data_to(buf
, dst
);
238 palette
->use_color(DEVICE_VALUE
);
239 draw_text(1, 1, buf
);
243 if(vis_context
->is_show_version
) {
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
);
255 // Special render for OpenGL selection mode
256 void Device::render_selection() {
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
267 /*****************************************************************************
269 *****************************************************************************/
270 class SimulatedDevice
: public Device
{
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 /*****************************************************************************
284 *****************************************************************************/
285 // get the spatial layout of the computer, including node distribution
286 void SpatialComputer::get_distribution(Args
* args
, int n
) {
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;
299 volume
=new Rect(-width
/2, width
/2, -height
/2, height
/2);
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
) {
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
336 for(int i
=0;i
<n
;i
++) {
337 if(distribution
->next_location(loc
)) {
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
);
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
));
382 hardware
.set_vm_context(d
);
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
) {
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
;
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
;
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
;
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
);
433 BOOL
SpatialComputer::handle_mouse(MouseEvent
* mouse
) {
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() {
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
);
456 // special render for OpenGL selecting mode
457 void SpatialComputer::render_selection() {
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
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
);
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
;
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
;
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
);
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
);
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
);
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
,
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
;
562 /*****************************************************************************
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\"");
587 void SpatialComputer::dump_frame(SECONDS time
, BOOL time_in_name
) {
589 // ensure that the directory exists
590 snprintf(buf
, 1000, "mkdir -p %s", dump_dir
); system(buf
);
593 sprintf(buf
,"%s/%s%.2f-%.2f.log",dump_dir
,dump_stem
,get_real_secs(),time
);
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