-
Notifications
You must be signed in to change notification settings - Fork 175
/
world.cc
1072 lines (845 loc) · 28 KB
/
world.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/**
$Id$
**/
/** @defgroup world World
Stage simulates a 'world' composed of `models', defined in a `world
file'.
API: Stg::World
<h2>Worldfile properties</h2>
@par Summary and default values
@verbatim
name <worldfile name>
interval_sim 100
quit_time 0
resolution 0.02
show_clock 0
show_clock_interval 100
threads 0
@endverbatim
@par Details
- name <string>\n
An identifying name for the world, used e.g. in the title bar of
the GUI.
- interval_sim <float>\n
The amount of simulation time run for each call of
World::Update(). Each model has its own configurable update
interval, which can be greater or less than this, but intervals
shorter than this are not visible in the GUI or in World update
callbacks. You are not likely to need to change the default of 100
msec: this is used internally by clients such as Player and WebSim.
- quit_time <float>\n
Stop the simulation after this many simulated seconds have
elapsed. In libstage, World::Update() returns true. In Stage with
a GUI, the simulation is paused.wo In Stage without a GUI, Stage
quits.
- resolution <float>\n
The resolution (in meters) of the underlying bitmap model. Larger
values speed up raytracing at the expense of fidelity in collision
detection and sensing. The default is often a reasonable choice.
- show_clock <int>\n
If non-zero, print the simulation time on stdout every
$show_clock_interval updates. Useful to watch the progress of
non-GUI simulations.
- show_clock_interval <int>\n
Sets the number of updates between printing the time on stdoutm,
if $show_clock is enabled. The default is once every 10 simulated
seconds. Smaller values slow the simulation down a little.
- threads <int>\n
The number of worker threads to spawn. Some
models can be updated in parallel (e.g. laser, ranger), and
running 2 or more threads here may make the simulation run faster,
depending on the number of CPU cores available and the
worldfile. As a guideline, use one thread per core if you have
parallel-enabled high-resolution models, e.g. a laser with
hundreds or thousands of samples, or lots of models.
@par More examples
The Stage source distribution contains several example world files in
<tt>(stage src)/worlds</tt> along with the worldfile properties
described on the manual page for each model type.
*/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
//#define DEBUG
#include <stdlib.h>
#include <assert.h>
#include <string.h> // for strdup(3)
#include <locale.h>
#include <limits.h>
#include <libgen.h> // for dirname(3)
#include "stage.hh"
#include "file_manager.hh"
#include "worldfile.hh"
#include "region.hh"
#include "option.hh"
using namespace Stg;
// // function objects for comparing model positions
bool World::ltx::operator()(const Model* a, const Model* b) const
{
const meters_t ax = a->GetGlobalPose().x;
const meters_t bx = b->GetGlobalPose().x;
if( ax == bx )
return a < b; // tie breaker
return ax < bx;
}
bool World::lty::operator()(const Model* a, const Model* b) const
{
const meters_t ay = a->GetGlobalPose().y;
const meters_t by = b->GetGlobalPose().y;
if( ay == by )
return a < b; // tie breaker
return ay < by;
}
// static data members
unsigned int World::next_id = 0;
bool World::quit_all = false;
std::set<World*> World::world_set;
std::string World::ctrlargs;
std::vector<std::string> World::args;
World::World( const std::string& name,
double ppm )
:
// private
destroy( false ),
dirty( true ),
models(),
models_by_name(),
models_with_fiducials(),
models_with_fiducials_byx(),
models_with_fiducials_byy(),
ppm( ppm ), // raytrace resolution
quit( false ),
show_clock( false ),
show_clock_interval( 100 ), // 10 simulated seconds using defaults
thread_mutex(),
threads_working( 0 ),
threads_start_cond(),
threads_done_cond(),
total_subs( 0 ),
worker_threads( 0 ),
// protected
cb_list(NULL),
extent(),
graphics( false ),
option_table(),
powerpack_list(),
quit_time( 0 ),
ray_list(),
sim_time( 0 ),
superregions(),
sr_cached(NULL),
updates( 0 ),
wf( NULL ),
paused( false ),
event_queues(1), // use 1 thread by default
sim_interval( 1e5 ) // 100 msec has proved a good default
{
if( ! Stg::InitDone() )
{
PRINT_WARN( "Stg::Init() must be called before a World is created." );
exit(-1);
}
pthread_mutex_init( &thread_mutex, NULL );
pthread_cond_init( &threads_start_cond, NULL );
pthread_cond_init( &threads_done_cond, NULL );
World::world_set.insert( this );
ground = new Model(this, NULL, "model");
assert(ground);
ground->SetToken( "_ground_model" ); // allow users to identify this unique model
AddModelName( ground, ground->Token() ); // add this name to the world's table
ground->ClearBlocks();
ground->SetGuiMove(false);
}
World::~World( void )
{
PRINT_DEBUG2( "destroying world %d %s", id, token.c_str() );
if( ground ) delete ground;
if( wf ) delete wf;
World::world_set.erase( this );
}
SuperRegion* World::CreateSuperRegion( point_int_t origin )
{
SuperRegion* sr = new SuperRegion( this, origin );
superregions[ origin ] = sr;
dirty = true; // force redraw
return sr;
}
void World::DestroySuperRegion( SuperRegion* sr )
{
superregions.erase( sr->GetOrigin() );
delete sr;
}
bool World::UpdateAll()
{
bool quit = true;
FOR_EACH( world_it, World::world_set )
{
if( (*world_it)->Update() == false )
quit = false;
}
return quit;
}
void* World::update_thread_entry( std::pair<World*,int> *thread_info )
{
World* world = thread_info->first;
const int thread_instance = thread_info->second;
//printf( "thread ID %d waiting for mutex\n", thread_instance );
pthread_mutex_lock( &world->thread_mutex );
while( 1 )
{
//printf( "thread ID %d waiting for start\n", thread_instance );
// wait until the main thread signals us
//puts( "worker waiting for start signal" );
pthread_cond_wait( &world->threads_start_cond, &world->thread_mutex );
pthread_mutex_unlock( &world->thread_mutex );
//puts( "worker thread awakes" );
// consume events on the queue up to the current sim time
world->ConsumeQueue( thread_instance );
//printf( "thread %d done\n", thread_instance );
// done working, so increment the counter. If this was the last
// thread to finish working, signal the main thread, which is
// blocked waiting for this to happen
pthread_mutex_lock( &world->thread_mutex );
if( --world->threads_working == 0 )
{
//puts( "last worker signalling main thread" );
pthread_cond_signal( &world->threads_done_cond );
}
// keep lock going round the loop
}
return NULL;
}
void World::AddModel( Model* mod )
{
models.insert( mod );
models_by_name[mod->token] = mod;
}
void World::AddModelName( Model* mod, const std::string& name )
{
models_by_name[name] = mod;
}
void World::RemoveModel( Model* mod )
{
models.erase( mod );
models_by_name.erase( mod->token );
}
void World::LoadBlock( Worldfile* wf, int entity )
{
// lookup the group in which this was defined
Model* mod = models_by_wfentity[ wf->GetEntityParent( entity )];
if( ! mod )
PRINT_ERR( "block has no model for a parent" );
mod->LoadBlock( wf, entity );
}
void World::LoadSensor( Worldfile* wf, int entity )
{
// lookup the group in which this was defined
ModelRanger* rgr = dynamic_cast<ModelRanger*>(models_by_wfentity[ wf->GetEntityParent( entity )]);
//todo verify that the parent is indeed a ranger
if( ! rgr )
PRINT_ERR( "block has no ranger model for a parent" );
rgr->LoadSensor( wf, entity );
}
Model* World::CreateModel( Model* parent, const std::string& typestr )
{
Model* mod = NULL; // new model to return
// find the creator function pointer in the map. use the
// vanilla model if the type is NULL.
creator_t creator = NULL;
// printf( "creating model of type %s\n", typestr );
std::map< std::string, creator_t>::iterator it =
Model::name_map.find( typestr );
if( it == Model::name_map.end() )
{
puts("");
PRINT_ERR1( "Model type %s not found in model typetable", typestr.c_str() );
}
else
creator = it->second;
// if we found a creator function, call it
if( creator )
{
//printf( "creator fn: %p\n", creator );
mod = (*creator)( this, parent, typestr );
}
else
{
PRINT_ERR1( "Unknown model type %s in world file.",
typestr.c_str() );
exit( 1 );
}
//printf( "created model %s\n", mod->Token() );
return mod;
}
void World::LoadModel( Worldfile* wf, int entity )
{
const int parent_entity = wf->GetEntityParent( entity );
PRINT_DEBUG2( "wf entity %d parent entity %d\n",
entity, parent_entity );
Model* parent = models_by_wfentity[ parent_entity ];
const char *typestr = (char*)wf->GetEntityType(entity);
assert(typestr);
Model* mod = CreateModel( parent, typestr );
// configure the model with properties from the world file
mod->Load(wf, entity );
// record the model we created for this worldfile entry
models_by_wfentity[entity] = mod;
}
void World::Load( const std::string& worldfile_path )
{
// note: must call Unload() before calling Load() if a world already
// exists TODO: unload doesn't clean up enough right now
printf( " [Loading %s]", worldfile_path.c_str() );
fflush(stdout);
this->wf = new Worldfile();
wf->Load( worldfile_path );
PRINT_DEBUG1( "wf has %d entitys", wf->GetEntityCount() );
// end the output line of worldfile components
//puts("");
const int entity = 0;
this->token =
wf->ReadString( entity, "name", this->token );
this->quit_time =
(usec_t)( million * wf->ReadFloat( entity, "quit_time", this->quit_time ) );
this->ppm =
1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm );
this->show_clock =
wf->ReadInt( entity, "show_clock", this->show_clock );
this->show_clock_interval =
wf->ReadInt( entity, "show_clock_interval", this->show_clock_interval );
// read msec instead of usec: easier for user
this->sim_interval =
1e3 * wf->ReadFloat( entity, "interval_sim", this->sim_interval / 1e3 );
this->worker_threads = wf->ReadInt( entity, "threads", this->worker_threads );
if( worker_threads > 0 )
{
PRINT_WARN( "\nmulti-thread support is experimental and may not work properly, if at all." );
event_queues.resize( worker_threads + 1 );
//printf( "worker threads %d\n", worker_threads );
// kick off the threads
for( unsigned int t=0; t<worker_threads; ++t )
{
// a little configuration for each thread can't be a local
// stack var, since it's accssed in the threads
std::pair<World*,int>* infop = new std::pair<World*,int>( this, t+1 );
//printf( "starting thread %d with ID %d \n", (int)t, info[t].second );
//normal posix pthread C function pointer
typedef void* (*func_ptr) (void*);
pthread_t pt;
pthread_create( &pt,
NULL,
(func_ptr)World::update_thread_entry,
infop );
}
printf( "[threads %u]", worker_threads );
}
// Iterate through entitys and create objects of the appropriate type
for( int entity = 1; entity < wf->GetEntityCount(); ++entity )
{
const char *typestr = (char*)wf->GetEntityType(entity);
// don't load window entries here
if( strcmp( typestr, "window" ) == 0 )
{
/* do nothing here */
}
else if( strcmp( typestr, "block" ) == 0 )
LoadBlock( wf, entity );
else if( strcmp( typestr, "sensor" ) == 0 )
LoadSensor( wf, entity );
else
LoadModel( wf, entity );
}
// call all controller init functions
FOR_EACH( it, models )
{
// all this is a hack and shouldn't be necessary
(*it)->blockgroup.CalcSize();
(*it)->UnMap();
(*it)->Map();
// to here
(*it)->InitControllers();
}
putchar( '\n' );
}
void World::UnLoad()
{
if( wf ) delete wf;
FOR_EACH( it, children )
delete (*it);
children.clear();
models_by_name.clear();
models_by_wfentity.clear();
ray_list.clear();
// todo - clean up regions & superregions?
token = "[unloaded]";
}
bool World::PastQuitTime()
{
return( (quit_time > 0) && (sim_time >= quit_time) );
}
std::string World::ClockString() const
{
const uint32_t usec_per_hour = 3600000000U;
const uint32_t usec_per_minute = 60000000U;
const uint32_t usec_per_second = 1000000U;
const uint32_t usec_per_msec = 1000U;
const uint32_t hours = sim_time / usec_per_hour;
const uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute;
const uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second;
const uint32_t msec = (sim_time % usec_per_second) / usec_per_msec;
std::string str;
char buf[256];
if( hours > 0 )
{
snprintf( buf, 255, "%uh", hours );
str += buf;
}
snprintf( buf, 255, " %um %02us %03umsec", minutes, seconds, msec);
str += buf;
return str;
}
void World::AddUpdateCallback( world_callback_t cb,
void* user )
{
// add the callback & argument to the list
cb_list.push_back( std::pair<world_callback_t,void*>(cb, user) );
}
int World::RemoveUpdateCallback( world_callback_t cb,
void* user )
{
std::pair<world_callback_t,void*> p( cb, user );
FOR_EACH( it, cb_list )
{
if( (*it) == p )
{
cb_list.erase( it );
break;
}
}
// return the number of callbacks now in the list. Useful for
// detecting when the list is empty.
return cb_list.size();
}
void World::CallUpdateCallbacks()
{
FOR_EACH( it, cb_list )
{
if( ((*it).first )( this, (*it).second ) )
it = cb_list.erase( it );
}
}
void World::ConsumeQueue( unsigned int queue_num )
{
std::priority_queue<Event>& queue = event_queues[queue_num];
if( queue.empty() )
return;
//printf( "event queue len %d\n", (int)queue.size() );
// update everything on the event queue that happens at this time or earlier
do
{
Event ev( queue.top() );
if( ev.time > sim_time ) break;
queue.pop();
//printf( "@ %llu next event ptr %p\n", sim_time, ev.mod );
//std::string modelType = ev.mod->GetModelType();
//printf( "@ %llu next event <%s %llu %s>\n", sim_time, modelType.c_str(), ev.time, ev.mod->Token() );
if( ev.mod->subs > 0 ) // no subscriptions means the event is discarded
ev.mod->Update(); // update the model
}
while( !queue.empty() );
}
bool World::Update()
{
if( show_clock && ((this->updates % show_clock_interval) == 0) )
{
printf( "\r[Stage: %s]", ClockString().c_str() );
fflush( stdout );
}
//puts( "World::Update()" );
// if we've run long enough, exit
if( PastQuitTime() )
return true;
sim_time += sim_interval;
FOR_EACH( it, active_velocity )
(*it)->UpdatePose();
// rebuild the sets sorted by position on x,y axis
#if( 1 )
models_with_fiducials_byx.clear();
models_with_fiducials_byy.clear();
FOR_EACH( it, models_with_fiducials )
{
models_with_fiducials_byx.insert( *it );
models_with_fiducials_byy.insert( *it );
}
#endif
//printf( "x %lu y %lu\n", models_with_fiducials_byy.size(),
// models_with_fiducials_byx.size() );
// handle the zeroth queue synchronously in the main thread
ConsumeQueue( 0 );
// handle all the remaining queues asynchronously in worker threads
if( worker_threads > 0 )
{
pthread_mutex_lock( &thread_mutex );
threads_working = worker_threads;
// unblock the workers - they are waiting on this condition var
//puts( "main thread signalling workers" );
pthread_cond_broadcast( &threads_start_cond );
// todo - take the 1th thread work here?
// wait for all the last update job to complete - it will
// signal the worker_threads_done condition var
while( threads_working > 0 )
{
//puts( "main thread waiting for workers to finish" );
pthread_cond_wait( &threads_done_cond, &thread_mutex );
}
pthread_mutex_unlock( &thread_mutex );
//puts( "main thread awakes" );
// TODO: allow threadsafe callbacks to be called in worker
// threads
}
dirty = true; // need redraw
// world callbacks
CallUpdateCallbacks();
FOR_EACH( it, active_energy )
(*it)->UpdateCharge();
++updates;
return false;
}
unsigned int World::GetEventQueue( Model* mod ) const
{
if( worker_threads < 1 )
return 0;
return( (random() % worker_threads) + 1);
}
Model* World::GetModel( const std::string& name ) const
{
PRINT_DEBUG1( "looking up model name %s in models_by_name", name.c_str() );
std::map<std::string,Model*>::const_iterator it =
models_by_name.find( name );
if( it == models_by_name.end() )
{
PRINT_WARN1( "lookup of model name %s: no matching name", name.c_str() );
return NULL;
}
else
return it->second; // the Model*
}
void World::RecordRay( double x1, double y1, double x2, double y2 )
{
float* drawpts = new float[4];
drawpts[0] = x1;
drawpts[1] = y1;
drawpts[2] = x2;
drawpts[3] = y2;
ray_list.push_back( drawpts );
}
void World::ClearRays()
{
FOR_EACH( it, ray_list )
{
float* pts = *it;
delete [] pts;
}
ray_list.clear();
}
void World::Raytrace( const Pose &gpose, // global pose
const meters_t range,
const radians_t fov,
const ray_test_func_t func,
const Model* model,
const void* arg,
RaytraceResult* samples, // preallocated storage for samples
const uint32_t sample_count, // number of samples
const bool ztest )
{
// find the direction of the first ray
Pose raypose = gpose;
const double starta = fov/2.0 - raypose.a;
for( uint32_t s=0; s < sample_count; ++s )
{
raypose.a = (s * fov / (double)(sample_count-1)) - starta;
samples[s] = Raytrace( raypose, range, func, model, arg, ztest );
}
}
// Stage spends 50-99% of its time in this method.
RaytraceResult World::Raytrace( const Pose &gpose,
const meters_t range,
const ray_test_func_t func,
const Model* mod,
const void* arg,
const bool ztest )
{
return Raytrace( Ray( mod, gpose, range, func, arg, ztest ));
}
RaytraceResult World::Raytrace( const Ray& r )
{
//rt_cells.clear();
//rt_candidate_cells.clear();
// initialize the sample
RaytraceResult sample( r.origin, r.range );
// our global position in (floating point) cell coordinates
double globx( r.origin.x * ppm );
double globy( r.origin.y * ppm );
// record our starting position
const double startx( globx );
const double starty( globy );
// eliminate a potential divide by zero
const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a );
const double cosa(cos(angle));
const double sina(sin(angle));
const double tana(sina/cosa); // = tan(angle)
// the x and y components of the ray (these need to be doubles, or a
// very weird and rare bug is produced)
const double dx( ppm * r.range * cosa);
const double dy( ppm * r.range * sina);
// fast integer line 3d algorithm adapted from Cohen's code from
// Graphics Gems IV
const int32_t sx(sgn(dx));
const int32_t sy(sgn(dy));
const int32_t ax(abs(dx));
const int32_t ay(abs(dy));
const int32_t bx(2*ax);
const int32_t by(2*ay);
int32_t exy(ay-ax); // difference between x and y distances
int32_t n(ax+ay); // the manhattan distance to the goal cell
// the distances between region crossings in X and Y
const double xjumpx( sx * REGIONWIDTH );
const double xjumpy( sx * REGIONWIDTH * tana );
const double yjumpx( sy * REGIONWIDTH / tana );
const double yjumpy( sy * REGIONWIDTH );
// manhattan distance between region crossings in X and Y
const double xjumpdist( fabs(xjumpx)+fabs(xjumpy) );
const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) );
// these are updated as we go along the ray
double xcrossx(0), xcrossy(0);
double ycrossx(0), ycrossy(0);
double distX(0), distY(0);
bool calculatecrossings( true );
// Stage spends up to 95% of its time in this loop! It would be
// neater with more function calls encapsulating things, but even
// inline calls have a noticeable (2-3%) effect on performance.
while( n > 0 ) // while we are still not at the ray end
{
Region* reg( GetSuperRegion( GETSREG(globx), GETSREG(globy) )
->GetRegion( GETREG(globx), GETREG(globy) ));
if( reg->count ) // if the region contains any objects
{
//assert( reg->cells.size() );
// invalidate the region crossing points used to jump over
// empty regions
calculatecrossings = true;
// convert from global cell to local cell coords
int32_t cx( GETCELL(globx) );
int32_t cy( GETCELL(globy) );
//Cell* c = reg->GetCell(cx,cy);
Cell* c( ®->cells[ cx + cy * REGIONWIDTH ] );
assert(c); // should be good: we know the region contains objects
// while within the bounds of this region and while some ray remains
// we'll tweak the cell pointer directly to move around quickly
while( (cx>=0) && (cx<REGIONWIDTH) &&
(cy>=0) && (cy<REGIONWIDTH) &&
n > 0 )
{
FOR_EACH( it, c->blocks )
{
Block* block( *it );
assert( block );
// skip if not in the right z range
if( r.ztest &&
( r.origin.z < block->global_z.min ||
r.origin.z > block->global_z.max ) )
continue;
// test the predicate we were passed
if( (*r.func)( block->mod, (Model*)r.mod, r.arg ))
{
// a hit!
sample.color = block->GetColor();
sample.mod = block->mod;
if( ax > ay ) // faster than the equivalent hypot() call
sample.range = fabs((globx-startx) / cosa) / ppm;
else
sample.range = fabs((globy-starty) / sina) / ppm;
return sample;
}
}
// increment our cell in the correct direction
if( exy < 0 ) // we're iterating along X
{
globx += sx; // global coordinate
exy += by;
c += sx; // move the cell left or right
cx += sx; // cell coordinate for bounds checking
}
else // we're iterating along Y
{
globy += sy; // global coordinate
exy -= bx;
c += sy * REGIONWIDTH; // move the cell up or down
cy += sy; // cell coordinate for bounds checking
}
--n; // decrement the manhattan distance remaining
//rt_cells.push_back( point_int_t( globx, globy ));
}
//printf( "leaving populated region\n" );
}
else // jump over the empty region
{
// on the first run, and when we've been iterating over
// cells, we need to calculate the next crossing of a region
// boundary along each axis
if( calculatecrossings )
{
calculatecrossings = false;
// find the coordinate in cells of the bottom left corner of
// the current region
const int32_t ix( globx );
const int32_t iy( globy );
double regionx( ix/REGIONWIDTH*REGIONWIDTH );
double regiony( iy/REGIONWIDTH*REGIONWIDTH );
if( (globx < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH;
if( (globy < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH;
// calculate the distance to the edge of the current region
const double xdx( sx < 0 ?
regionx - globx - 1.0 : // going left
regionx + REGIONWIDTH - globx ); // going right
const double xdy( xdx*tana );
const double ydy( sy < 0 ?
regiony - globy - 1.0 : // going down
regiony + REGIONWIDTH - globy ); // going up
const double ydx( ydy/tana );
// these stored hit points are updated as we go along
xcrossx = globx+xdx;
xcrossy = globy+xdy;
ycrossx = globx+ydx;
ycrossy = globy+ydy;
// find the distances to the region crossing points
// manhattan distance is faster than using hypot()
distX = fabs(xdx)+fabs(xdy);
distY = fabs(ydx)+fabs(ydy);
}
if( distX < distY ) // crossing a region boundary left or right
{
// move to the X crossing
globx = xcrossx;
globy = xcrossy;
n -= distX; // decrement remaining manhattan distance
// calculate the next region crossing
xcrossx += xjumpx;
xcrossy += xjumpy;
distY -= distX;
distX = xjumpdist;
//rt_candidate_cells.push_back( point_int_t( xcrossx, xcrossy ));
}
else // crossing a region boundary up or down
{
// move to the X crossing
globx = ycrossx;
globy = ycrossy;
n -= distY; // decrement remaining manhattan distance
// calculate the next region crossing
ycrossx += yjumpx;
ycrossy += yjumpy;
distX -= distY;
distY = yjumpdist;
//rt_candidate_cells.push_back( point_int_t( ycrossx, ycrossy ));
}
}
//rt_cells.push_back( point_int_t( globx, globy ));
}
// hit nothing
sample.mod = NULL;
return sample;
}
static int _save_cb( Model* mod, void* dummy )
{
mod->Save();
return 0;
}
bool World::Save( const char *filename )
{
ForEachDescendant( _save_cb, NULL );
return this->wf->Save( filename );
}
static int _reload_cb( Model* mod, void* dummy )
{
mod->Load();
return 0;
}
// reload the current worldfile
void World::Reload( void )
{
ForEachDescendant( _reload_cb, NULL );
}
SuperRegion* World::AddSuperRegion( const point_int_t& sup )
{
//printf( "Creating super region [ %d %d ]\n", sup.x, sup.y );
SuperRegion* sr = CreateSuperRegion( sup );
// the bounds of the world have changed
//printf( "lower left (%.2f,%.2f,%.2f)\n", pt.x, pt.y, pt.z );
// set the lower left corner of the new superregion
Extend( point3_t( (sup.x << SRBITS) / ppm,
(sup.y << SRBITS) / ppm,
0 ));
// top right corner of the new superregion
Extend( point3_t( ((sup.x+1) << SRBITS) / ppm,
((sup.y+1) << SRBITS) / ppm,
0 ));
//printf( "top right (%.2f,%.2f,%.2f)\n", pt.x, pt.y, pt.z );
// // map all jit models
// FOR_EACH( it, jit_render )
// (*it)->Map();
return sr;
}