00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "trueClock.h"
00017 #include "config_express.h"
00018 #include "numeric_types.h"
00019
00020 #include <math.h>
00021
00022 TrueClock *TrueClock::_global_ptr = NULL;
00023
00024 #if defined(WIN32_VC) || defined(WIN64_VC)
00025
00026
00027
00028
00029
00030
00031
00032 #include <sys/timeb.h>
00033 #define WIN32_LEAN_AND_MEAN
00034 #include <windows.h>
00035
00036 static const double _0001 = 1.0 / 1000.0;
00037 static const double _00000001 = 1.0 / 10000000.0;
00038
00039
00040
00041
00042
00043
00044
00045 static const double paranoid_clock_interval = 3.0;
00046
00047
00048
00049
00050 static const double paranoid_clock_jump_error = 2.0;
00051
00052
00053
00054
00055 static const double paranoid_clock_jump_error_max_delta = 1.0;
00056
00057
00058
00059
00060 static const double paranoid_clock_report_scale_factor = 0.1;
00061
00062
00063
00064
00065
00066 static const double paranoid_clock_chase_threshold = 0.5;
00067
00068
00069
00070
00071 static const double paranoid_clock_chase_factor = 0.1;
00072
00073
00074
00075
00076
00077
00078 double TrueClock::
00079 get_long_time() {
00080 int tc = GetTickCount();
00081 return (double)(tc - _init_tc) * _0001;
00082 }
00083
00084
00085
00086
00087
00088
00089 double TrueClock::
00090 get_short_raw_time() {
00091 double time;
00092
00093 if (_has_high_res) {
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 PN_int64 count;
00104 QueryPerformanceCounter((LARGE_INTEGER *)&count);
00105
00106 time = (double)(count - _init_count) * _recip_frequency;
00107
00108 } else {
00109
00110
00111
00112
00113
00114 int tc = GetTickCount();
00115 time = (double)(tc - _init_tc) * _0001;
00116 }
00117
00118 return time;
00119 }
00120
00121
00122
00123
00124
00125
00126 typedef BOOL (WINAPI * PFNSETPROCESSAFFINITYMASK)(HANDLE, DWORD_PTR);
00127 typedef BOOL (WINAPI * PFNGETPROCESSAFFINITYMASK)(HANDLE, PDWORD_PTR, PDWORD_PTR);
00128
00129 bool TrueClock::
00130 set_cpu_affinity(PN_uint32 mask) const {
00131 HMODULE hker = GetModuleHandle("kernel32");
00132 if (hker != 0) {
00133 PFNGETPROCESSAFFINITYMASK gp = (PFNGETPROCESSAFFINITYMASK)
00134 GetProcAddress(hker, "GetProcessAffinityMask");
00135 PFNSETPROCESSAFFINITYMASK sp = (PFNSETPROCESSAFFINITYMASK)
00136 GetProcAddress(hker, "SetProcessAffinityMask");
00137 if (gp != 0 && sp != 0) {
00138 DWORD proc_mask;
00139 DWORD sys_mask;
00140 if (gp(GetCurrentProcess(), (PDWORD_PTR)&proc_mask, (PDWORD_PTR)&sys_mask)) {
00141
00142 proc_mask = mask & sys_mask;
00143 if (proc_mask) {
00144 return sp(GetCurrentProcess(), proc_mask) != 0;
00145 }
00146 }
00147 }
00148 }
00149 return false;
00150 }
00151
00152
00153
00154
00155
00156
00157 TrueClock::
00158 TrueClock() {
00159 _error_count = 0;
00160 _has_high_res = false;
00161
00162 _time_scale = 1.0;
00163 _time_offset = 0.0;
00164 _tod_offset = 0.0;
00165 _time_scale_changed = false;
00166 _last_reported_time_scale = 1.0;
00167 _report_time_scale_time = 0.0;
00168
00169 ConfigVariableBool lock_to_one_cpu
00170 ("lock-to-one-cpu", false,
00171 PRC_DESC("Set this to true if you want the entire process to use one "
00172 "CPU, even on multi-core and multi-CPU workstations. This is "
00173 "mainly a hack to solve a bug in which QueryPerformanceCounter "
00174 "returns inconsistent results on multi-core machines. "));
00175
00176 if (lock_to_one_cpu) {
00177 set_cpu_affinity(0x01);
00178 }
00179
00180 if (get_use_high_res_clock()) {
00181 PN_int64 int_frequency;
00182 _has_high_res =
00183 (QueryPerformanceFrequency((LARGE_INTEGER *)&int_frequency) != 0);
00184 if (_has_high_res) {
00185 if (int_frequency <= 0) {
00186 clock_cat.error()
00187 << "TrueClock::get_real_time() - frequency is negative!" << endl;
00188 _has_high_res = false;
00189
00190 } else {
00191 _frequency = (double)int_frequency;
00192 _recip_frequency = 1.0 / _frequency;
00193
00194 QueryPerformanceCounter((LARGE_INTEGER *)&_init_count);
00195 }
00196 }
00197 }
00198
00199
00200
00201
00202 _init_tc = GetTickCount();
00203
00204
00205
00206 GetSystemTimeAsFileTime((FILETIME *)&_init_tod);
00207
00208 _chase_clock = CC_keep_even;
00209
00210
00211
00212
00213 _timestamps.push_back(Timestamp(0.0, 0.0));
00214
00215 if (!_has_high_res) {
00216 clock_cat.warning()
00217 << "No high resolution clock available." << endl;
00218 }
00219 }
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247 double TrueClock::
00248 correct_time(double time) {
00249
00250 PN_uint64 int_tod;
00251 GetSystemTimeAsFileTime((FILETIME *)&int_tod);
00252 double tod = (double)(int_tod - _init_tod) * _00000001;
00253
00254 nassertr(!_timestamps.empty(), time);
00255
00256
00257
00258 double time_delta = (time - _timestamps.back()._time) * _time_scale;
00259 double tod_delta = (tod - _timestamps.back()._tod);
00260
00261 if (time_delta < -0.0001 ||
00262 fabs(time_delta - tod_delta) > paranoid_clock_jump_error) {
00263
00264
00265
00266
00267
00268 clock_cat.debug()
00269 << "Clock error detected; elapsed time " << time_delta
00270 << "s on high-resolution counter, and " << tod_delta
00271 << "s on time-of-day clock.\n";
00272 ++_error_count;
00273
00274
00275
00276
00277 double time_adjust = 0.0;
00278 double tod_adjust = 0.0;
00279
00280 if (time_delta < 0.0 && tod < 0.0) {
00281
00282 time_adjust = -time_delta;
00283 tod_adjust = -tod_delta;
00284
00285 } else if (time_delta < 0.0 || (tod_delta >= 0.0 && tod_delta < time_delta)) {
00286
00287 double new_tod_delta = min(tod_delta, paranoid_clock_jump_error);
00288 time_adjust = new_tod_delta - time_delta;
00289 tod_adjust = new_tod_delta - tod_delta;
00290
00291 } else {
00292
00293 double new_time_delta = min(time_delta, paranoid_clock_jump_error);
00294 time_adjust = new_time_delta - time_delta;
00295 tod_adjust = new_time_delta - tod_delta;
00296 }
00297
00298 _time_offset += time_adjust;
00299 time_delta += time_adjust;
00300 _tod_offset += tod_adjust;
00301 tod_delta += tod_adjust;
00302
00303
00304
00305
00306
00307 Timestamps::iterator ti;
00308 for (ti = _timestamps.begin(); ti != _timestamps.end(); ++ti) {
00309 (*ti)._time -= time_adjust / _time_scale;
00310 (*ti)._tod -= tod_adjust;
00311 }
00312
00313
00314
00315 _timestamps.push_back(Timestamp(time, tod));
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327 double corrected_time = time * _time_scale + _time_offset;
00328 double corrected_tod = tod + _tod_offset;
00329 if (corrected_time - corrected_tod > paranoid_clock_jump_error_max_delta &&
00330 _time_scale > 0.00001) {
00331 clock_cat.info()
00332 << "Force-adjusting time_scale to catch up to errors.\n";
00333 set_time_scale(time, _time_scale * 0.5);
00334 }
00335
00336 } else if (tod_delta < 0.0) {
00337
00338
00339
00340
00341 } else {
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352 Timestamp oldest = _timestamps.front();
00353 double time_age = (time - oldest._time);
00354 double tod_age = (tod - oldest._tod);
00355
00356 double keep_interval = paranoid_clock_interval;
00357
00358 if (tod_age > keep_interval / 2.0 && time_age > 0.0) {
00359
00360
00361
00362 double new_time_scale = tod_age / time_age;
00363
00364
00365
00366
00367 set_time_scale(time, new_time_scale);
00368
00369
00370
00371 double ratio = _time_scale / _last_reported_time_scale;
00372 if (fabs(ratio - 1.0) > paranoid_clock_report_scale_factor) {
00373 _time_scale_changed = true;
00374 _last_reported_time_scale = _time_scale;
00375
00376
00377 _report_time_scale_time = tod + _tod_offset + keep_interval;
00378 if (clock_cat.is_debug()) {
00379 clock_cat.debug()
00380 << "Will report time scale, now " << 100.0 / _time_scale
00381 << "%, tod_age = " << tod_age << ", time_age = " << time_age
00382 << ", ratio = " << ratio << "\n";
00383 }
00384 }
00385 }
00386
00387
00388 if (tod_age > keep_interval) {
00389 while (!_timestamps.empty() &&
00390 tod - _timestamps.front()._tod > keep_interval) {
00391 _timestamps.pop_front();
00392 }
00393 }
00394
00395
00396 _timestamps.push_back(Timestamp(time, tod));
00397 }
00398
00399 double corrected_time = time * _time_scale + _time_offset;
00400 double corrected_tod = tod + _tod_offset;
00401
00402 if (_time_scale_changed && corrected_tod >= _report_time_scale_time) {
00403 double percent = 100.0 / _time_scale;
00404
00405 percent = floor(percent / 20.0 + 0.5) * 20.0;
00406 clock_cat.info()
00407 << "Clock appears to be running at " << percent << "% real time.\n";
00408 _last_reported_time_scale = _time_scale;
00409 _time_scale_changed = false;
00410 }
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427 switch (_chase_clock) {
00428 case CC_slow_down:
00429 if (corrected_time < corrected_tod) {
00430
00431 _chase_clock = CC_keep_even;
00432 if (clock_cat.is_debug()) {
00433 clock_cat.debug()
00434 << "Clock back down to real time.\n";
00435
00436
00437 ++_error_count;
00438 }
00439
00440 } else {
00441
00442 double fixup = 1.0 - (1.0 / (corrected_time - corrected_tod));
00443 double correction = time_delta * max(fixup, paranoid_clock_chase_factor);
00444 _time_offset -= correction;
00445 corrected_time -= correction;
00446 }
00447 break;
00448
00449 case CC_keep_even:
00450 if ((corrected_tod - corrected_time) > paranoid_clock_chase_threshold) {
00451
00452 _chase_clock = CC_speed_up;
00453
00454 if (clock_cat.is_debug()) {
00455 clock_cat.debug()
00456 << "Clock is behind by " << (corrected_tod - corrected_time)
00457 << "s; speeding up to correct.\n";
00458 }
00459 } else if ((corrected_time - corrected_tod) > paranoid_clock_chase_threshold) {
00460
00461 _chase_clock = CC_slow_down;
00462
00463 if (clock_cat.is_debug()) {
00464 clock_cat.debug()
00465 << "Clock is ahead by " << (corrected_time - corrected_tod)
00466 << "s; slowing down to correct.\n";
00467 }
00468 }
00469 break;
00470
00471 case CC_speed_up:
00472 if (corrected_time > corrected_tod) {
00473
00474 _chase_clock = CC_keep_even;
00475 if (clock_cat.is_debug()) {
00476 clock_cat.debug()
00477 << "Clock back up to real time.\n";
00478
00479
00480 ++_error_count;
00481 }
00482
00483 } else {
00484
00485 double fixup = 1.0 - (1.0 / (corrected_tod - corrected_time));
00486 double correction = time_delta * max(fixup, paranoid_clock_chase_factor);
00487 _time_offset += correction;
00488 corrected_time += correction;
00489 }
00490 break;
00491 }
00492
00493 if (clock_cat.is_spam()) {
00494 clock_cat.spam()
00495 << "time " << time << " tod " << corrected_tod
00496 << " corrected time " << corrected_time << "\n";
00497 }
00498
00499 return corrected_time;
00500 }
00501
00502
00503
00504
00505
00506
00507
00508
00509 void TrueClock::
00510 set_time_scale(double time, double new_time_scale) {
00511 nassertv(new_time_scale > 0.0);
00512 _time_offset = time * _time_scale + _time_offset - (time * new_time_scale);
00513 _time_scale = new_time_scale;
00514 }
00515
00516 #else // !WIN32_VC
00517
00518
00519
00520
00521
00522
00523
00524 #include <sys/time.h>
00525 #include <stdio.h>
00526
00527 static long _init_sec;
00528
00529
00530
00531
00532
00533
00534 double TrueClock::
00535 get_long_time() {
00536 struct timeval tv;
00537
00538 int result;
00539
00540 #ifdef GETTIMEOFDAY_ONE_PARAM
00541 result = gettimeofday(&tv);
00542 #else
00543 result = gettimeofday(&tv, (struct timezone *)NULL);
00544 #endif
00545
00546 if (result < 0) {
00547
00548 return 0.0;
00549 }
00550
00551
00552
00553
00554
00555 return (double)(tv.tv_sec - _init_sec) + (double)tv.tv_usec / 1000000.0;
00556 }
00557
00558
00559
00560
00561
00562
00563 double TrueClock::
00564 get_short_raw_time() {
00565 struct timeval tv;
00566
00567 int result;
00568
00569 #ifdef GETTIMEOFDAY_ONE_PARAM
00570 result = gettimeofday(&tv);
00571 #else
00572 result = gettimeofday(&tv, (struct timezone *)NULL);
00573 #endif
00574
00575 if (result < 0) {
00576
00577 return 0.0;
00578 }
00579
00580
00581
00582
00583
00584 return (double)(tv.tv_sec - _init_sec) + (double)tv.tv_usec / 1000000.0;
00585 }
00586
00587
00588
00589
00590
00591
00592 bool TrueClock::
00593 set_cpu_affinity(PN_uint32 mask) const {
00594 return false;
00595 }
00596
00597
00598
00599
00600
00601
00602 TrueClock::
00603 TrueClock() {
00604 _error_count = 0;
00605 struct timeval tv;
00606
00607 int result;
00608 #ifdef GETTIMEOFDAY_ONE_PARAM
00609 result = gettimeofday(&tv);
00610 #else
00611 result = gettimeofday(&tv, (struct timezone *)NULL);
00612 #endif
00613
00614 if (result < 0) {
00615 perror("gettimeofday");
00616 _init_sec = 0;
00617 } else {
00618 _init_sec = tv.tv_sec;
00619 }
00620 }
00621
00622 #endif