/* * local_clock - the NTP logical clock loop filter. * * Return codes: * -1 update ignored: exceeds panic threshold * 0 update ignored: popcorn or exceeds step threshold * 1 clock was slewed * 2 clock was stepped * * LOCKCLOCK: The only thing this routine does is set the * sys_rootdisp variable equal to the peer dispersion. */ int local_clock( struct peer *peer, /* synch source peer structure */ double fp_offset /* clock offset (s) */ ) { int rval; /* return code */ int osys_poll; /* old system poll */ int ntp_adj_ret; /* returned by ntp_adjtime */ double mu; /* interval since last update */ double clock_frequency; /* clock frequency */ double dtemp, etemp; /* double temps */ char tbuf[80]; /* report buffer */ /* * If the loop is opened or the NIST LOCKCLOCK is in use, * monitor and record the offsets anyway in order to determine * the open-loop response and then go home. */ #ifdef LOCKCLOCK { #else if (!ntp_enable) { #endif /* LOCKCLOCK */ record_loop_stats(fp_offset, drift_comp, clock_jitter, clock_stability, sys_poll); return (0); } #ifndef LOCKCLOCK /* * If the clock is way off, panic is declared. The clock_panic * defaults to 1000 s; if set to zero, the panic will never * occur. The allow_panic defaults to FALSE, so the first panic * will exit. It can be set TRUE by a command line option, in * which case the clock will be set anyway and time marches on. * But, allow_panic will be set FALSE when the update is less * than the step threshold; so, subsequent panics will exit. */ if (fabs(fp_offset) > clock_panic && clock_panic > 0 && !allow_panic) { snprintf(tbuf, sizeof(tbuf), "%+.0f s; set clock manually within %.0f s.", fp_offset, clock_panic); report_event(EVNT_SYSFAULT, NULL, tbuf); return (-1); } /* * This section simulates ntpdate. If the offset exceeds the * step threshold (128 ms), step the clock to that time and * exit. Otherwise, slew the clock to that time and exit. Note * that the slew will persist and eventually complete beyond the * life of this program. Note that while ntpdate is active, the * terminal does not detach, so the termination message prints * directly to the terminal. */ if (mode_ntpdate) { if ( ( fp_offset > clock_max_fwd && clock_max_fwd > 0) || (-fp_offset > clock_max_back && clock_max_back > 0)) { step_systime(fp_offset); msyslog(LOG_NOTICE, "ntpd: time set %+.6f s", fp_offset); printf("ntpd: time set %+.6fs\n", fp_offset); } else { adj_systime(fp_offset); msyslog(LOG_NOTICE, "ntpd: time slew %+.6f s", fp_offset); printf("ntpd: time slew %+.6fs\n", fp_offset); } record_loop_stats(fp_offset, drift_comp, clock_jitter, clock_stability, sys_poll); exit (0); } /* * The huff-n'-puff filter finds the lowest delay in the recent * interval. This is used to correct the offset by one-half the * difference between the sample delay and minimum delay. This * is most effective if the delays are highly assymetric and * clockhopping is avoided and the clock frequency wander is * relatively small. */ if (sys_huffpuff != NULL) { if (peer->delay < sys_huffpuff[sys_huffptr]) sys_huffpuff[sys_huffptr] = peer->delay; if (peer->delay < sys_mindly) sys_mindly = peer->delay; if (fp_offset > 0) dtemp = -(peer->delay - sys_mindly) / 2; else dtemp = (peer->delay - sys_mindly) / 2; fp_offset += dtemp; #ifdef DEBUG if (debug) printf( "local_clock: size %d mindly %.6f huffpuff %.6f\n", sys_hufflen, sys_mindly, dtemp); #endif } /* * Clock state machine transition function which defines how the * system reacts to large phase and frequency excursion. There * are two main regimes: when the offset exceeds the step * threshold (128 ms) and when it does not. Under certain * conditions updates are suspended until the stepout theshold * (900 s) is exceeded. See the documentation on how these * thresholds interact with commands and command line options. * * Note the kernel is disabled if step is disabled or greater * than 0.5 s or in ntpdate mode. */ osys_poll = sys_poll; if (sys_poll < peer->minpoll) sys_poll = peer->minpoll; if (sys_poll > peer->maxpoll) sys_poll = peer->maxpoll; mu = current_time - clock_epoch; clock_frequency = drift_comp; rval = 1; if ( ( fp_offset > clock_max_fwd && clock_max_fwd > 0) || (-fp_offset > clock_max_back && clock_max_back > 0) || force_step_once ) { if (force_step_once) { force_step_once = FALSE; /* we want this only once after startup */ msyslog(LOG_NOTICE, "Doing intital time step" ); } switch (state) { /* * In SYNC state we ignore the first outlier and switch * to SPIK state. */ case EVNT_SYNC: snprintf(tbuf, sizeof(tbuf), "%+.6f s", fp_offset); report_event(EVNT_SPIK, NULL, tbuf); state = EVNT_SPIK; return (0); /* * In FREQ state we ignore outliers and inlyers. At the * first outlier after the stepout threshold, compute * the apparent frequency correction and step the phase. */ case EVNT_FREQ: if (mu < clock_minstep) return (0); clock_frequency = direct_freq(fp_offset); /* fall through to EVNT_SPIK */ /* * In SPIK state we ignore succeeding outliers until * either an inlyer is found or the stepout threshold is * exceeded. */ case EVNT_SPIK: if (mu < clock_minstep) return (0); /* fall through to default */ /* * We get here by default in NSET and FSET states and * from above in FREQ or SPIK states. * * In NSET state an initial frequency correction is not * available, usually because the frequency file has not * yet been written. Since the time is outside the step * threshold, the clock is stepped. The frequency will * be set directly following the stepout interval. * * In FSET state the initial frequency has been set from * the frequency file. Since the time is outside the * step threshold, the clock is stepped immediately, * rather than after the stepout interval. Guys get * nervous if it takes 15 minutes to set the clock for * the first time. * * In FREQ and SPIK states the stepout threshold has * expired and the phase is still above the step * threshold. Note that a single spike greater than the * step threshold is always suppressed, even with a * long time constant. */ default: snprintf(tbuf, sizeof(tbuf), "%+.6f s", fp_offset); report_event(EVNT_CLOCKRESET, NULL, tbuf); step_systime(fp_offset); reinit_timer(); tc_counter = 0; clock_jitter = LOGTOD(sys_precision); rval = 2; if (state == EVNT_NSET) { rstclock(EVNT_FREQ, 0); return (rval); } break; } rstclock(EVNT_SYNC, 0); } else { /* * The offset is less than the step threshold. Calculate * the jitter as the exponentially weighted offset * differences. */ etemp = SQUARE(clock_jitter); dtemp = SQUARE(max(fabs(fp_offset - last_offset), LOGTOD(sys_precision))); clock_jitter = SQRT(etemp + (dtemp - etemp) / CLOCK_AVG); switch (state) { /* * In NSET state this is the first update received and * the frequency has not been initialized. Adjust the * phase, but do not adjust the frequency until after * the stepout threshold. */ case EVNT_NSET: adj_systime(fp_offset); rstclock(EVNT_FREQ, fp_offset); break; /* * In FREQ state ignore updates until the stepout * threshold. After that, compute the new frequency, but * do not adjust the frequency until the holdoff counter * decrements to zero. */ case EVNT_FREQ: if (mu < clock_minstep) return (0); clock_frequency = direct_freq(fp_offset); /* fall through */ /* * We get here by default in FSET, SPIK and SYNC states. * Here compute the frequency update due to PLL and FLL * contributions. Note, we avoid frequency discipline at * startup until the initial transient has subsided. */ default: allow_panic = FALSE; if (freq_cnt == 0) { /* * The FLL and PLL frequency gain constants * depend on the time constant and Allan * intercept. The PLL is always used, but * becomes ineffective above the Allan intercept * where the FLL becomes effective. */ if (sys_poll >= allan_xpt) clock_frequency += (fp_offset - clock_offset) / max(ULOGTOD(sys_poll), mu) * CLOCK_FLL; /* * The PLL frequency gain (numerator) depends on * the minimum of the update interval and Allan * intercept. This reduces the PLL gain when the * FLL becomes effective. */ etemp = min(ULOGTOD(allan_xpt), mu); dtemp = 4 * CLOCK_PLL * ULOGTOD(sys_poll); clock_frequency += fp_offset * etemp / (dtemp * dtemp); } rstclock(EVNT_SYNC, fp_offset); if (fabs(fp_offset) < CLOCK_FLOOR) freq_cnt = 0; break; } } #ifdef KERNEL_PLL /* * This code segment works when clock adjustments are made using * precision time kernel support and the ntp_adjtime() system * call. This support is available in Solaris 2.6 and later, * Digital Unix 4.0 and later, FreeBSD, Linux and specially * modified kernels for HP-UX 9 and Ultrix 4. In the case of the * DECstation 5000/240 and Alpha AXP, additional kernel * modifications provide a true microsecond clock and nanosecond * clock, respectively. * * Important note: The kernel discipline is used only if the * step threshold is less than 0.5 s, as anything higher can * lead to overflow problems. This might occur if some misguided * lad set the step threshold to something ridiculous. */ if (pll_control && kern_enable && freq_cnt == 0) { /* * We initialize the structure for the ntp_adjtime() * system call. We have to convert everything to * microseconds or nanoseconds first. Do not update the * system variables if the ext_enable flag is set. In * this case, the external clock driver will update the * variables, which will be read later by the local * clock driver. Afterwards, remember the time and * frequency offsets for jitter and stability values and * to update the frequency file. */ ZERO(ntv); if (ext_enable) { ntv.modes = MOD_STATUS; } else { #ifdef STA_NANO ntv.modes = MOD_BITS | MOD_NANO; #else /* STA_NANO */ ntv.modes = MOD_BITS; #endif /* STA_NANO */ if (clock_offset < 0) dtemp = -.5; else dtemp = .5; #ifdef STA_NANO ntv.offset = (int32)(clock_offset * 1e9 + dtemp); ntv.constant = sys_poll; #else /* STA_NANO */ ntv.offset = (int32)(clock_offset * 1e6 + dtemp); ntv.constant = sys_poll - 4; #endif /* STA_NANO */ if (ntv.constant < 0) ntv.constant = 0; ntv.esterror = (u_int32)(clock_jitter * 1e6); ntv.maxerror = (u_int32)((sys_rootdelay / 2 + sys_rootdisp) * 1e6); ntv.status = STA_PLL; /* * Enable/disable the PPS if requested. */ if (hardpps_enable) { ntv.status |= (STA_PPSTIME | STA_PPSFREQ); if (!(pll_status & STA_PPSTIME)) sync_status("PPS enabled", pll_status, ntv.status); } else { ntv.status &= ~(STA_PPSTIME | STA_PPSFREQ); if (pll_status & STA_PPSTIME) sync_status("PPS disabled", pll_status, ntv.status); } if (sys_leap == LEAP_ADDSECOND) ntv.status |= STA_INS; else if (sys_leap == LEAP_DELSECOND) ntv.status |= STA_DEL; } /* * Pass the stuff to the kernel. If it squeals, turn off * the pps. In any case, fetch the kernel offset, * frequency and jitter. */ ntp_adj_ret = ntp_adjtime(&ntv); /* * A squeal is a return status < 0, or a state change. */ if ((0 > ntp_adj_ret) || (ntp_adj_ret != kernel_status)) { kernel_status = ntp_adj_ret; ntp_adjtime_error_handler(__func__, &ntv, ntp_adj_ret, errno, hardpps_enable, 0, __LINE__ - 1); } pll_status = ntv.status; #ifdef STA_NANO clock_offset = ntv.offset / 1e9; #else /* STA_NANO */ clock_offset = ntv.offset / 1e6; #endif /* STA_NANO */ clock_frequency = FREQTOD(ntv.freq); /* * If the kernel PPS is lit, monitor its performance. */ if (ntv.status & STA_PPSTIME) { #ifdef STA_NANO clock_jitter = ntv.jitter / 1e9; #else /* STA_NANO */ clock_jitter = ntv.jitter / 1e6; #endif /* STA_NANO */ } #if defined(STA_NANO) && NTP_API == 4 /* * If the TAI changes, update the kernel TAI. */ if (loop_tai != sys_tai) { loop_tai = sys_tai; ntv.modes = MOD_TAI; ntv.constant = sys_tai; if ((ntp_adj_ret = ntp_adjtime(&ntv)) != 0) { ntp_adjtime_error_handler(__func__, &ntv, ntp_adj_ret, errno, 0, 1, __LINE__ - 1); } } #endif /* STA_NANO */ } #endif /* KERNEL_PLL */ /* * Clamp the frequency within the tolerance range and calculate * the frequency difference since the last update. */ if (fabs(clock_frequency) > NTP_MAXFREQ) msyslog(LOG_NOTICE, "frequency error %.0f PPM exceeds tolerance %.0f PPM", clock_frequency * 1e6, NTP_MAXFREQ * 1e6); dtemp = SQUARE(clock_frequency - drift_comp); if (clock_frequency > NTP_MAXFREQ) drift_comp = NTP_MAXFREQ; else if (clock_frequency < -NTP_MAXFREQ) drift_comp = -NTP_MAXFREQ; else drift_comp = clock_frequency; /* * Calculate the wander as the exponentially weighted RMS * frequency differences. Record the change for the frequency * file update. */ etemp = SQUARE(clock_stability); clock_stability = SQRT(etemp + (dtemp - etemp) / CLOCK_AVG); /* * Here we adjust the time constant by comparing the current * offset with the clock jitter. If the offset is less than the * clock jitter times a constant, then the averaging interval is * increased, otherwise it is decreased. A bit of hysteresis * helps calm the dance. Works best using burst mode. Don't * fiddle with the poll during the startup clamp period. */ if (freq_cnt > 0) { tc_counter = 0; } else if (fabs(clock_offset) < CLOCK_PGATE * clock_jitter) { tc_counter += sys_poll; if (tc_counter > CLOCK_LIMIT) { tc_counter = CLOCK_LIMIT; if (sys_poll < peer->maxpoll) { tc_counter = 0; sys_poll++; } } } else { tc_counter -= sys_poll << 1; if (tc_counter < -CLOCK_LIMIT) { tc_counter = -CLOCK_LIMIT; if (sys_poll > peer->minpoll) { tc_counter = 0; sys_poll--; } } } /* * If the time constant has changed, update the poll variables. */ if (osys_poll != sys_poll) poll_update(peer, sys_poll); /* * Yibbidy, yibbbidy, yibbidy; that'h all folks. */ record_loop_stats(clock_offset, drift_comp, clock_jitter, clock_stability, sys_poll); #ifdef DEBUG if (debug) printf( "local_clock: offset %.9f jit %.9f freq %.3f stab %.3f poll %d\n", clock_offset, clock_jitter, drift_comp * 1e6, clock_stability * 1e6, sys_poll); #endif /* DEBUG */ return (rval); #endif /* LOCKCLOCK */ } /* * adj_host_clock - Called once every second to update the local clock. * * LOCKCLOCK: The only thing this routine does is increment the * sys_rootdisp variable. */ void adj_host_clock( void ) { double offset_adj; double freq_adj; /* * Update the dispersion since the last update. In contrast to * NTPv3, NTPv4 does not declare unsynchronized after one day, * since the dispersion check serves this function. Also, * since the poll interval can exceed one day, the old test * would be counterproductive. During the startup clamp period, the * time constant is clamped at 2. */ sys_rootdisp += clock_phi; #ifndef LOCKCLOCK if (!ntp_enable || mode_ntpdate) return; /* * Determine the phase adjustment. The gain factor (denominator) * increases with poll interval, so is dominated by the FLL * above the Allan intercept. Note the reduced time constant at * startup. */ if (state != EVNT_SYNC) { offset_adj = 0.; } else if (freq_cnt > 0) { offset_adj = clock_offset / (CLOCK_PLL * ULOGTOD(1)); freq_cnt--; #ifdef KERNEL_PLL } else if (pll_control && kern_enable) { offset_adj = 0.; #endif /* KERNEL_PLL */ } else { offset_adj = clock_offset / (CLOCK_PLL * ULOGTOD(sys_poll)); } /* * If the kernel discipline is enabled the frequency correction * drift_comp has already been engaged via ntp_adjtime() in * set_freq(). Otherwise it is a component of the adj_systime() * offset. */ #ifdef KERNEL_PLL if (pll_control && kern_enable) freq_adj = 0.; else #endif /* KERNEL_PLL */ freq_adj = drift_comp; /* Bound absolute value of total adjustment to NTP_MAXFREQ. */ if (offset_adj + freq_adj > NTP_MAXFREQ) offset_adj = NTP_MAXFREQ - freq_adj; else if (offset_adj + freq_adj < -NTP_MAXFREQ) offset_adj = -NTP_MAXFREQ - freq_adj; clock_offset -= offset_adj; /* * Windows port adj_systime() must be called each second, * even if the argument is zero, to ease emulation of * adjtime() using Windows' slew API which controls the rate * but does not automatically stop slewing when an offset * has decayed to zero. */ adj_systime(offset_adj + freq_adj); #endif /* LOCKCLOCK */ }
// Print the estimated path loss to a text file. Either print a point, route (line) or grid of estimates void PrintPathLossToFile(void) { FILE *PathLossFile; char *FilenameBuffer; FilenameBuffer = (char*)malloc(100*sizeof(char)); sprintf_s(FilenameBuffer, 100*sizeof(char), "%s/%s_%s", FolderName, ProjectName, PathLossFilename); if (fopen_s(&PathLossFile, FilenameBuffer, "w") != 0) { printf("Could not open file '%s'\n", PathLossFilename); } else { printf("Printing path loss values to '%s'\n",PathLossFilename); PrintFileHeader(PathLossFile); int x, y, z; double PathLoss; // Z coordinate is constant z = PlaceWithinGridZ(RoundToNearest(PathLossParameters.Z1/GridSpacing)); switch (PathLossParameters.Type) { // Print the path loss at a single point case POINT: { // Details of the path loss estimates required and column titles fprintf(PathLossFile, "Point Analysis\nHeight = %f\n\nX\t\tY\t\tPL(dB)\n", z*GridSpacing); x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X1/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y1/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); break; } // Print the path loss along a route case ROUTE: { double length = sqrt(SQUARE(PathLossParameters.X2 - PathLossParameters.X1)+SQUARE(PathLossParameters.Y2 - PathLossParameters.Y1)); double nSamples = length/PathLossParameters.Spacing; double dx = (PathLossParameters.X2 - PathLossParameters.X1)/nSamples; double dy = (PathLossParameters.Y2 - PathLossParameters.Y1)/nSamples; // Details of the path loss estimates required and column titles fprintf(PathLossFile, "Route Analysis - %d samples\nHeight = %f\n\nX\t\tY\t\tPL(dB)\n", (int)nSamples == nSamples ? (int)nSamples+1 : (int)nSamples+2, z*GridSpacing); for (int i = 0; i < nSamples; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*i)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); break; } // Print the path loss in a 2d grid case GRID: { double nSamplesX = (PathLossParameters.X2-PathLossParameters.X1)/PathLossParameters.Spacing; double nSamplesY = (PathLossParameters.Y2-PathLossParameters.Y1)/PathLossParameters.SpacingY; double dx = (PathLossParameters.X2 - PathLossParameters.X1)/nSamplesX; double dy = (PathLossParameters.Y2 - PathLossParameters.Y1)/nSamplesY; // Details of the path loss estimates required and column titles fprintf(PathLossFile, "Grid Analysis - %d x %d samples\nHeight = %f\n\nX\t\tY\t\tPL(dB)\n", (int)nSamplesX == nSamplesX ? (int)nSamplesX+1 : (int)nSamplesX+2, (int)nSamplesY == nSamplesY ? (int)nSamplesY+1 : (int)nSamplesY+2, z*GridSpacing); for (int j=0; j < nSamplesY; j++) { for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } // Print a path loss estimate of the outer X row nearest X2,Y2 on the grid (may not be a whole sample space apart) for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); break; } // Print the path loss in a set of 2d grids case CUBE: { double nSamplesX = (PathLossParameters.X2-PathLossParameters.X1)/PathLossParameters.Spacing; double nSamplesY = (PathLossParameters.Y2-PathLossParameters.Y1)/PathLossParameters.SpacingY; double nSamplesZ = (PathLossParameters.Z2-PathLossParameters.Z1)/PathLossParameters.SpacingZ; double dx = (PathLossParameters.X2 - PathLossParameters.X1)/nSamplesX; double dy = (PathLossParameters.Y2 - PathLossParameters.Y1)/nSamplesY; double dz = (PathLossParameters.Z2 - PathLossParameters.Z1)/nSamplesZ; // Details of the path loss estimates required and column titles fprintf(PathLossFile, "Grid Analysis - %d x %d x %d samples\n", (int)nSamplesX == nSamplesX ? (int)nSamplesX+1 : (int)nSamplesX+2, (int)nSamplesY == nSamplesY ? (int)nSamplesY+1 : (int)nSamplesY+2, (int)nSamplesZ == nSamplesZ ? (int)nSamplesZ+1 : (int)nSamplesZ+2); for (int k=0; k < nSamplesZ; k++) { z = PlaceWithinGridZ(RoundToNearest((PathLossParameters.Z1+dz*k)/GridSpacing)); fprintf(PathLossFile, "\nHeight = %f\n\nX\t\tY\t\tPL(dB)\n", z*GridSpacing); for (int j=0; j < nSamplesY; j++) { for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } // Print a path loss estimate of the outer X row nearest X2,Y2 on the grid (may not be a whole sample space apart) for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } z = PlaceWithinGridZ(RoundToNearest((PathLossParameters.Z2)/GridSpacing)); fprintf(PathLossFile, "\nHeight = %f\n\nX\t\tY\t\tPL(dB)\n", z*GridSpacing); for (int j=0; j < nSamplesY; j++) { for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest((PathLossParameters.Y1+dy*j)/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } // Print a path loss estimate of the outer X row nearest X2,Y2 on the grid (may not be a whole sample space apart) for (int i=0; i < nSamplesX; i++) { x = PlaceWithinGridX(RoundToNearest((PathLossParameters.X1+dx*i)/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); } x = PlaceWithinGridX(RoundToNearest(PathLossParameters.X2/GridSpacing)); y = PlaceWithinGridY(RoundToNearest(PathLossParameters.Y2/GridSpacing)); PathLoss = VoltageToDB(SPEED_OF_LIGHT/Frequency * Grid[x][y][z].Vmax * KAPPA/4/M_PI/GridSpacing); fprintf(PathLossFile, "%f\t%f\t%f\n", x*GridSpacing, y*GridSpacing, PathLoss); break; } } if (fclose(PathLossFile)) { printf("Path loss file close unsuccessful\n"); } } free(FilenameBuffer); }
// // The main function for performing SLAM at the low level. The first argument will return // whether there is still information to be processed by SLAM (set to 1). The second and third // arguments return the corrected odometry for the time steps, and the corresponding list of // observations. This can be used for the higher level SLAM process when using hierarchical SLAM. // void LowSlam(TPath **path, TSenseLog **obs) { int cnt; int i, j, overflow = 0; char name[32]; TPath *tempPath; TSenseLog *tempObs; TAncestor *lineage; // Initialize the worldMap LowInitializeWorldMap(); // Initialize the ancestry and particles cleanID = ID_NUMBER - 2; // ID_NUMBER-1 is being used as the root of the ancestry tree. // Initialize all of our unused ancestor particles to look unused. for (i = 0; i < ID_NUMBER; i++) { availableID[i] = i; l_particleID[i].generation = -1; l_particleID[i].numChildren = 0; l_particleID[i].ID = -1; l_particleID[i].parent = NULL; l_particleID[i].mapEntries = NULL; l_particleID[i].path = NULL; l_particleID[i].seen = 0; l_particleID[i].total = 0; l_particleID[i].size = 0; } // Initialize the root of our ancestry tree. l_particleID[ID_NUMBER-1].generation = 0; l_particleID[ID_NUMBER-1].numChildren = 1; l_particleID[ID_NUMBER-1].size = 0; l_particleID[ID_NUMBER-1].total = 0; l_particleID[ID_NUMBER-1].ID = ID_NUMBER-1; l_particleID[ID_NUMBER-1].parent = NULL; l_particleID[ID_NUMBER-1].mapEntries = NULL; // Create all of our starting particles at the center of the map. for (i = 0; i < PARTICLE_NUMBER; i++) { l_particle[i].ancestryNode = &(l_particleID[ID_NUMBER-1]); l_particle[i].x = MAP_WIDTH / 2; l_particle[i].y = MAP_HEIGHT / 2; l_particle[i].theta = 0.001; l_particle[i].probability = 0; children[i] = 0; } // We really only use the first particle, since they are all essentially the same. l_particle[0].probability = 1; l_cur_particles_used = 1; children[0] = SAMPLE_NUMBER; // We don't need to initialize the savedParticles, since Localization will create them for us, and they first are used in // UpdateAncestry, which is called after Localization. This statement isn't necessary, then, but serves as a sort of placeholder // when reading the code. cur_saved_particles_used = 0; overflow = 1; for (i=0; i < START_ITERATION; i++) ReadLog(readFile, logfile_index, sense); curGeneration = 0; // Add the first thing that you see to the worldMap at the center. This gives us something to localize off of. ReadLog(readFile, logfile_index, sense); AddToWorldModel(sense, 0); curGeneration = 1; // Make a record of what the first odometry readings were, so that we can compute relative movement across time steps. lastX = odometry.x; lastY = odometry.y; lastTheta = odometry.theta; // Get our observation log started. (*obs) = (TSenseLog *)malloc(sizeof(TSenseLog)); for (i=0; i < SENSE_NUMBER; i++) { (*obs)->sense[i].distance = sense[i].distance; (*obs)->sense[i].theta = sense[i].theta; } (*obs)->next = NULL; cnt = 0; while (curGeneration < LEARN_DURATION) { // Collect information from the data log. If either reading returns 1, we've run out of log data, and // we need to stop now. if (ReadLog(readFile, logfile_index, sense) == 1) overflow = 0; else overflow = 1; // We don't necessarily want to use every last reading that comes in. This allows us to make certain that the // robot has moved at least a minimal amount (in terms of meters and radians) before we try to localize and update. if ((sqrt(SQUARE(odometry.x - lastX) + SQUARE(odometry.y - lastY)) < 0.10) && (fabs(odometry.theta - lastTheta) < 0.04)) overflow = 0; if (overflow > 0) { overflow--; // Wipe the slate clean LowInitializeFlags(); // Apply the localization procedure, which will give us the N best particles Localize(sense); // Add these maintained particles to the FamilyTree, so that ancestry can be determined, and then prune dead lineages UpdateAncestry(sense, l_particleID); // Update the observation log (used only by hierarchical SLAM) tempObs = (*obs); while (tempObs->next != NULL) tempObs = tempObs->next; tempObs->next = (TSenseLog *)malloc(sizeof(TSenseLog)); if (tempObs->next == NULL) fprintf(stderr, "Malloc failed in making a new observation!\n"); for (i=0; i < SENSE_NUMBER; i++) { tempObs->next->sense[i].distance = sense[i].distance; tempObs->next->sense[i].theta = sense[i].theta; } tempObs->next->next = NULL; curGeneration++; // Remember these odometry readings for next time. This is what lets us know the incremental motion. lastX = odometry.x; lastY = odometry.y; lastTheta = odometry.theta; } } // Find the most likely particle. Return its path j = 0; for (i=0; i < l_cur_particles_used; i++) if (l_particle[i].probability > l_particle[j].probability) j = i; (*path) = NULL; i = 0; lineage = l_particle[j].ancestryNode; while ((lineage != NULL) && (lineage->ID != ID_NUMBER-1)) { tempPath = lineage->path; i++; while (tempPath->next != NULL) { i++; tempPath = tempPath->next; } tempPath->next = (*path); (*path) = lineage->path; lineage->path = NULL; lineage = lineage->parent; } // Print out the map. sprintf(name, "map"); j = 0; for (i = 0; i < l_cur_particles_used; i++) if (l_particle[i].probability > l_particle[j].probability) j = i; PrintMap(name, l_particle[j].ancestryNode, FALSE, -1, -1, -1); sprintf(name, "rm map.ppm"); system(name); // Clean up the memory being used. DisposeAncestry(l_particleID); LowDestroyMap(); }
static void warpModifier_do(WarpModifierData *wmd, Object *ob, DerivedMesh *dm, float (*vertexCos)[3], int numVerts) { float obinv[4][4]; float mat_from[4][4]; float mat_from_inv[4][4]; float mat_to[4][4]; float mat_unit[4][4]; float mat_final[4][4]; float tmat[4][4]; const float falloff_radius_sq = SQUARE(wmd->falloff_radius); float strength = wmd->strength; float fac = 1.0f, weight; int i; int defgrp_index; MDeformVert *dvert, *dv = NULL; float (*tex_co)[3] = NULL; if (!(wmd->object_from && wmd->object_to)) return; modifier_get_vgroup(ob, dm, wmd->defgrp_name, &dvert, &defgrp_index); if (dvert == NULL) { defgrp_index = -1; } if (wmd->curfalloff == NULL) /* should never happen, but bad lib linking could cause it */ wmd->curfalloff = curvemapping_add(1, 0.0f, 0.0f, 1.0f, 1.0f); if (wmd->curfalloff) { curvemapping_initialize(wmd->curfalloff); } invert_m4_m4(obinv, ob->obmat); mul_m4_m4m4(mat_from, obinv, wmd->object_from->obmat); mul_m4_m4m4(mat_to, obinv, wmd->object_to->obmat); invert_m4_m4(tmat, mat_from); // swap? mul_m4_m4m4(mat_final, tmat, mat_to); invert_m4_m4(mat_from_inv, mat_from); unit_m4(mat_unit); if (strength < 0.0f) { float loc[3]; strength = -strength; /* inverted location is not useful, just use the negative */ copy_v3_v3(loc, mat_final[3]); invert_m4(mat_final); negate_v3_v3(mat_final[3], loc); } weight = strength; if (wmd->texture) { tex_co = MEM_mallocN(sizeof(*tex_co) * numVerts, "warpModifier_do tex_co"); get_texture_coords((MappingInfoModifierData *)wmd, ob, dm, vertexCos, tex_co, numVerts); modifier_init_texture(wmd->modifier.scene, wmd->texture); } for (i = 0; i < numVerts; i++) { float *co = vertexCos[i]; if (wmd->falloff_type == eWarp_Falloff_None || ((fac = len_squared_v3v3(co, mat_from[3])) < falloff_radius_sq && (fac = (wmd->falloff_radius - sqrtf(fac)) / wmd->falloff_radius))) { /* skip if no vert group found */ if (defgrp_index != -1) { dv = &dvert[i]; weight = defvert_find_weight(dv, defgrp_index) * strength; if (weight <= 0.0f) { continue; } } /* closely match PROP_SMOOTH and similar */ switch (wmd->falloff_type) { case eWarp_Falloff_None: fac = 1.0f; break; case eWarp_Falloff_Curve: fac = curvemapping_evaluateF(wmd->curfalloff, 0, fac); break; case eWarp_Falloff_Sharp: fac = fac * fac; break; case eWarp_Falloff_Smooth: fac = 3.0f * fac * fac - 2.0f * fac * fac * fac; break; case eWarp_Falloff_Root: fac = sqrtf(fac); break; case eWarp_Falloff_Linear: /* pass */ break; case eWarp_Falloff_Const: fac = 1.0f; break; case eWarp_Falloff_Sphere: fac = sqrtf(2 * fac - fac * fac); break; case eWarp_Falloff_InvSquare: fac = fac * (2.0f - fac); break; } fac *= weight; if (tex_co) { TexResult texres; texres.nor = NULL; BKE_texture_get_value(wmd->modifier.scene, wmd->texture, tex_co[i], &texres, false); fac *= texres.tin; } if (fac != 0.0f) { /* into the 'from' objects space */ mul_m4_v3(mat_from_inv, co); if (fac == 1.0f) { mul_m4_v3(mat_final, co); } else { if (wmd->flag & MOD_WARP_VOLUME_PRESERVE) { /* interpolate the matrix for nicer locations */ blend_m4_m4m4(tmat, mat_unit, mat_final, fac); mul_m4_v3(tmat, co); } else { float tvec[3]; mul_v3_m4v3(tvec, mat_final, co); interp_v3_v3v3(co, co, tvec, fac); } } /* out of the 'from' objects space */ mul_m4_v3(mat_from, co); } } } if (tex_co) MEM_freeN(tex_co); }
void CINTg0_2e_coulerf_simd1(double *g, Rys2eT *bc, CINTEnvVars *envs, int idsimd) { double aij, akl, a0, a1, fac1; ALIGNMM double x[SIMDD]; double *rij = envs->rij; double *rkl = envs->rkl; double rijrkl[3]; double rijrx[3]; double rklrx[3]; double *u = bc->u; double *w = bc->w; double omega = envs->env[PTR_RANGE_OMEGA]; int nroots = envs->nrys_roots; int i; aij = envs->ai[idsimd] + envs->aj[idsimd]; akl = envs->ak[idsimd] + envs->al[idsimd]; a1 = aij * akl; a0 = a1 / (aij + akl); double theta = 0; if (omega > 0) { // For long-range part of range-separated Coulomb operator theta = omega * omega / (omega * omega + a0); a0 *= theta; } fac1 = sqrt(a0 / (a1 * a1 * a1)) * envs->fac[idsimd]; rijrkl[0] = rij[0*SIMDD+idsimd] - rkl[0*SIMDD+idsimd]; rijrkl[1] = rij[1*SIMDD+idsimd] - rkl[1*SIMDD+idsimd]; rijrkl[2] = rij[2*SIMDD+idsimd] - rkl[2*SIMDD+idsimd]; x[0] = a0 * SQUARE(rijrkl); CINTrys_roots(nroots, x, u, w, 1); double *gx = g; double *gy = g + envs->g_size; double *gz = g + envs->g_size * 2; for (i = 0; i < nroots; i++) { gx[i] = 1; gy[i] = 1; gz[i] = w[i*SIMDD] * fac1; } if (envs->g_size == 1) { return; } if (omega > 0) { /* u[:] = tau^2 / (1 - tau^2) * transform u[:] to theta^-1 tau^2 / (theta^-1 - tau^2) * so the rest code can be reused. */ for (i = 0; i < nroots; i++) { u[i*SIMDD] /= u[i*SIMDD] + 1 - u[i*SIMDD] * theta; } } double u2, div, tmp1, tmp2, tmp3, tmp4; double *b00 = bc->b00; double *b10 = bc->b10; double *b01 = bc->b01; double *c00x = bc->c00x; double *c00y = bc->c00y; double *c00z = bc->c00z; double *c0px = bc->c0px; double *c0py = bc->c0py; double *c0pz = bc->c0pz; rijrx[0] = rij[0*SIMDD+idsimd] - envs->rx_in_rijrx[0]; rijrx[1] = rij[1*SIMDD+idsimd] - envs->rx_in_rijrx[1]; rijrx[2] = rij[2*SIMDD+idsimd] - envs->rx_in_rijrx[2]; rklrx[0] = rkl[0*SIMDD+idsimd] - envs->rx_in_rklrx[0]; rklrx[1] = rkl[1*SIMDD+idsimd] - envs->rx_in_rklrx[1]; rklrx[2] = rkl[2*SIMDD+idsimd] - envs->rx_in_rklrx[2]; for (i = 0; i < nroots; i++) { /* *t2 = u(i)/(1+u(i)) *u2 = aij*akl/(aij+akl)*t2/(1-t2) */ u2 = a0 * u[i*SIMDD]; div = 1 / (u2 * (aij + akl) + a1); tmp1 = u2 * div; tmp4 = .5 * div; b00[i] = 0.5 * tmp1; tmp2 = tmp1 * akl; tmp3 = tmp1 * aij; b10[i] = b00[i] + tmp4 * akl; b01[i] = b00[i] + tmp4 * aij; c00x[i] = rijrx[0] - tmp2 * rijrkl[0]; c00y[i] = rijrx[1] - tmp2 * rijrkl[1]; c00z[i] = rijrx[2] - tmp2 * rijrkl[2]; c0px[i] = rklrx[0] + tmp3 * rijrkl[0]; c0py[i] = rklrx[1] + tmp3 * rijrkl[1]; c0pz[i] = rklrx[2] + tmp3 * rijrkl[2]; } (*envs->f_g0_2d4d_simd1)(g, bc, envs); }
void load_fen(char* fen) { clear_board(); char* ptr = fen; uint8_t rank = 7; uint8_t file = 0; do { switch (*ptr) { case 'K': fill_square(WHITE, KING, SQUARE(rank, file)); file++; break; case 'Q': fill_square(WHITE, QUEEN, SQUARE(rank, file)); file++; break; case 'R': fill_square(WHITE, ROOK, SQUARE(rank, file)); file++; break; case 'B': fill_square(WHITE, BISHOP, SQUARE(rank, file)); file++; break; case 'N': fill_square(WHITE, KNIGHT, SQUARE(rank, file)); file++; break; case 'P': fill_square(WHITE, PAWN, SQUARE(rank, file)); file++; break; case 'k': fill_square(BLACK, KING, SQUARE(rank, file)); file++; break; case 'q': fill_square(BLACK, QUEEN, SQUARE(rank, file)); file++; break; case 'r': fill_square(BLACK, ROOK, SQUARE(rank, file)); file++; break; case 'b': fill_square(BLACK, BISHOP, SQUARE(rank, file)); file++; break; case 'n': fill_square(BLACK, KNIGHT, SQUARE(rank, file)); file++; break; case 'p': fill_square(BLACK, PAWN, SQUARE(rank, file)); file++; break; case '/': rank--; file = 0; break; case '1': file += 1; break; case '2': file += 2; break; case '3': file += 3; break; case '4': file += 4; break; case '5': file += 5; break; case '6': file += 6; break; case '7': file += 7; break; case '8': file += 8; break; }; ptr++; } while (*ptr != ' '); ptr++; if (*ptr == 'w') { b.stm = WHITE; } else { b.stm = BLACK; } ptr += 2; b.flags = 0; do { switch (*ptr) { case 'K': b.flags |= CASTLE_WK; break; case 'Q': b.flags |= CASTLE_WQ; break; case 'k': b.flags |= CASTLE_BK; break; case 'q': b.flags |= CASTLE_BQ; break; } ptr++; } while (*ptr != ' '); ptr++; if (*ptr != '-') { uint8_t file = ptr[0] - 'a'; uint8_t rank = ptr[1] - '1'; b.ep = SQUARE(rank, file); } do { ptr++; } while (*ptr != ' '); ptr++; int ply = 0; sscanf(ptr, "%d", &ply); b.ply = ply; positions[b.ply] = b.hash; for (square_t sq = 0; sq < 128; sq++) { if (!IS_SQUARE(sq)) continue; if (b.pieces[sq] != PIECE_EMPTY) { b.material_score += evaluate_piece(b.colors[sq], b.pieces[sq], sq); } } }
static int rubber_callback (const BoxType * b, void *cl) { LineTypePtr line = (LineTypePtr) b; struct rubber_info *i = (struct rubber_info *) cl; float x, y, rad, dist1, dist2; BDimension t; int touches = 0; t = line->Thickness / 2; if (TEST_FLAG (LOCKFLAG, line)) return 0; if (line == i->line) return 0; /* * Check to see if the line touches a rectangular region. * To do this we need to look for the intersection of a circular * region and a rectangular region. */ if (i->radius == 0) { int found = 0; if (line->Point1.X + t >= i->box.X1 && line->Point1.X - t <= i->box.X2 && line->Point1.Y + t >= i->box.Y1 && line->Point1.Y - t <= i->box.Y2) { if (((i->box.X1 <= line->Point1.X) && (line->Point1.X <= i->box.X2)) || ((i->box.Y1 <= line->Point1.Y) && (line->Point1.Y <= i->box.Y2))) { /* * The circle is positioned such that the closest point * on the rectangular region boundary is not at a corner * of the rectangle. i.e. the shortest line from circle * center to rectangle intersects the rectangle at 90 * degrees. In this case our first test is sufficient */ touches = 1; } else { /* * Now we must check the distance from the center of the * circle to the corners of the rectangle since the * closest part of the rectangular region is the corner. */ x = MIN (abs (i->box.X1 - line->Point1.X), abs (i->box.X2 - line->Point1.X)); x *= x; y = MIN (abs (i->box.Y1 - line->Point1.Y), abs (i->box.Y2 - line->Point1.Y)); y *= y; x = x + y - (t * t); if (x <= 0) touches = 1; } if (touches) { CreateNewRubberbandEntry (i->layer, line, &line->Point1); found++; } } if (line->Point2.X + t >= i->box.X1 && line->Point2.X - t <= i->box.X2 && line->Point2.Y + t >= i->box.Y1 && line->Point2.Y - t <= i->box.Y2) { if (((i->box.X1 <= line->Point2.X) && (line->Point2.X <= i->box.X2)) || ((i->box.Y1 <= line->Point2.Y) && (line->Point2.Y <= i->box.Y2))) { touches = 1; } else { x = MIN (abs (i->box.X1 - line->Point2.X), abs (i->box.X2 - line->Point2.X)); x *= x; y = MIN (abs (i->box.Y1 - line->Point2.Y), abs (i->box.Y2 - line->Point2.Y)); y *= y; x = x + y - (t * t); if (x <= 0) touches = 1; } if (touches) { CreateNewRubberbandEntry (i->layer, line, &line->Point2); found++; } } return found; } /* circular search region */ if (i->radius < 0) rad = 0; /* require exact match */ else rad = SQUARE(i->radius + t); x = (i->X - line->Point1.X); x *= x; y = (i->Y - line->Point1.Y); y *= y; dist1 = x + y - rad; x = (i->X - line->Point2.X); x *= x; y = (i->Y - line->Point2.Y); y *= y; dist2 = x + y - rad; if (dist1 > 0 && dist2 > 0) return 0; #ifdef CLOSEST_ONLY /* keep this to remind me */ if (dist1 < dist2) CreateNewRubberbandEntry (i->layer, line, &line->Point1); else CreateNewRubberbandEntry (i->layer, line, &line->Point2); #else if (dist1 <= 0) CreateNewRubberbandEntry (i->layer, line, &line->Point1); if (dist2 <= 0) CreateNewRubberbandEntry (i->layer, line, &line->Point2); #endif return 1; }
const double GfxConstants::EARTH_RADIUS = 6378137.0; /* This number is an impossible number for latitude as it is beyond the * poles. * For latitude it is a possible value and is situated at 180degrees * east or west */ const int32 GfxConstants::IMPOSSIBLE = MAX_INT32 - 1; const float64 GfxConstants::MC2SCALE_TO_METER = EARTH_RADIUS*2.0*M_PI / POW2_32; const float64 GfxConstants::SQUARE_MC2SCALE_TO_SQUARE_METER = SQUARE(MC2SCALE_TO_METER); const float64 GfxConstants::METER_TO_MC2SCALE = POW2_32 / (EARTH_RADIUS*2.0*M_PI); const float64 GfxConstants::SQUARE_METER_TO_SQUARE_MC2SCALE = SQUARE(METER_TO_MC2SCALE); const float64 GfxConstants::MC2SCALE_TO_CENTIMETER = MC2SCALE_TO_METER*100; const int32 GfxConstants::MC2SCALE_TO_CENTIMETER_INT32 = int32(MC2SCALE_TO_CENTIMETER); // 2^32/360 const double
int ns_data_print(pp_Data * p, double x[], const Exo_DB * exo, const double time_value, const double time_step_size) { const int quantity = p->data_type; int mat_num = p->mat_num; const int elemBlock_id = p->elem_blk_id; const int node_set_id = p->ns_id; const int species_id = p->species_number; const char * filenm = p->data_filenm; const char * qtity_str = p->data_type_name; const char * format_flag = p->format_flag; int * first_time = &(p->first_time); static int err=0; int num_nodes_on_side; int ebIndex_first = -1; int local_side[2]; int side_nodes[3]; /* Assume quad has no more than 3 per side. */ int elem_list[4], elem_ct=0, face, ielem, node2; int local_node[4]; int node = -1; int idx, idy, idz, id_var; int iprint; int nsp; /* node set pointer for this node set */ dbl x_pos, y_pos, z_pos; int j, wspec; int doPressure = 0; #ifdef PARALLEL double some_time=0.0; #endif double abscissa=0; double ordinate=0; double n1[3], n2[3]; double xi[3]; /* * Find an element block that has the desired material id. */ if (elemBlock_id != -1) { for (j = 0; j < exo->num_elem_blocks; j++) { if (elemBlock_id == exo->eb_id[j]) { ebIndex_first = j; break; } } if (ebIndex_first == -1) { sprintf(err_msg, "Can't find an element block with the elem Block id %d\n", elemBlock_id); if (Num_Proc == 1) { EH(-1, err_msg); } } mat_num = Matilda[ebIndex_first]; p->mat_num = mat_num; pd = pd_glob[mat_num]; } else { mat_num = -1; p->mat_num = -1; pd = pd_glob[0]; } nsp = match_nsid(node_set_id); if( nsp != -1 ) { node = Proc_NS_List[Proc_NS_Pointers[nsp]]; } else { sprintf(err_msg, "Node set ID %d not found.", node_set_id); if( Num_Proc == 1 ) EH(-1,err_msg); } /* first right time stamp or run stamp to separate the sets */ print_sync_start(FALSE); if (*first_time) { if ( format_flag[0] != '\0' ) { if (ProcID == 0) { uf = fopen(filenm,"a"); if (uf != NULL) { fprintf(uf,"# %s %s @ nsid %d node (%d) \n", format_flag, qtity_str, node_set_id, node ); *first_time = FALSE; fclose(uf); } } } } if (format_flag[0] == '\0') { if (ProcID == 0) { if ((uf = fopen(filenm,"a")) != NULL) { fprintf(uf,"Time/iteration = %e \n", time_value); fprintf(uf," %s Node_Set %d Species %d\n", qtity_str,node_set_id,species_id); fflush(uf); fclose(uf); } } } if (nsp != -1 ) { for (j = 0; j < Proc_NS_Count[nsp]; j++) { node = Proc_NS_List[Proc_NS_Pointers[nsp]+j]; if (node < num_internal_dofs + num_boundary_dofs ) { idx = Index_Solution(node, MESH_DISPLACEMENT1, 0, 0, -1); if (idx == -1) { x_pos = Coor[0][node]; WH(idx, "Mesh variable not found. May get undeformed coords."); } else { x_pos = Coor[0][node] + x[idx]; } idy = Index_Solution(node, MESH_DISPLACEMENT2, 0, 0, -1); if (idy == -1) { y_pos = Coor[1][node]; } else { y_pos = Coor[1][node] + x[idy]; } z_pos = 0.; if(pd->Num_Dim == 3) { idz = Index_Solution(node, MESH_DISPLACEMENT3, 0, 0, -1); if (idz == -1) { z_pos = Coor[2][node]; } else{ z_pos = Coor[2][node] + x[idz]; } } if (quantity == MASS_FRACTION) { id_var = Index_Solution(node, quantity, species_id, 0, mat_num); } else if (quantity < 0) { id_var = -1; } else { id_var = Index_Solution(node, quantity, 0, 0, mat_num); } /* * In the easy case, the variable can be found somewhere in the * big vector of unknowns. But sometimes we want a derived quantity * that requires a little more work than an array reference. * * For now, save the good result if we have it. */ if ( id_var != -1 ) { ordinate = x[id_var]; iprint = 1; } else { /* * If we have an element based interpolation, let's calculate the interpolated value */ if (quantity == PRESSURE) { if ((pd->i[PRESSURE] == I_P1) || ( (pd->i[PRESSURE] > I_PQ1) && (pd->i[PRESSURE] < I_Q2_HVG) )) { doPressure = 1; } } iprint = 0; } /* * If the quantity is "theta", an interior angle that only * makes sense at a point, in 2D, we'll need to compute it. */ if ( strncasecmp(qtity_str, "theta", 5 ) == 0 || doPressure) { /* * Look for the two sides connected to this node...? * * Premise: * 1. The node appears in only one element(removed-RBS,6/14/06) * 2. Exactly two sides emanate from the node. * 3. Quadrilateral. * * Apologies to people who wish to relax premise 1. I know * there are some obtuse angles out there that benefit from * having more than one element at a vertex. With care, this * procedure could be extended to cover that case as well. */ if ( ! exo->node_elem_conn_exists ) { EH(-1, "Cannot compute angle without node_elem_conn."); } elem_list[0] = exo->node_elem_list[exo->node_elem_pntr[node]]; /* * Find out where this node appears in the elements local * node ordering scheme... */ local_node[0] = in_list(node, exo->elem_node_pntr[elem_list[0]], exo->elem_node_pntr[elem_list[0]+1], exo->elem_node_list); EH(local_node[0], "Can not find node in elem node connectivity!?! "); local_node[0] -= exo->elem_node_pntr[elem_list[0]]; /* check for neighbors*/ if( mat_num == find_mat_number(elem_list[0], exo)) {elem_ct = 1;} else {WH(-1,"block id doesn't match first element");} for (face=0 ; face<ei->num_sides ; face++) { ielem = exo->elem_elem_list[exo->elem_elem_pntr[elem_list[0]]+face]; if (ielem != -1) { node2 = in_list(node, exo->elem_node_pntr[ielem], exo->elem_node_pntr[ielem+1], exo->elem_node_list); if (node2 != -1 && (mat_num == find_mat_number(ielem, exo))) { elem_list[elem_ct] = ielem; local_node[elem_ct] = node2; local_node[elem_ct] -= exo->elem_node_pntr[ielem]; elem_ct++; } } } /* * Note that indeces are zero based! */ ordinate = 0.0; for (ielem = 0 ; ielem < elem_ct ; ielem++) { if ( local_node[ielem] < 0 || local_node[ielem] > 3 ) { if (strncasecmp(qtity_str, "theta", 5 ) == 0) { EH(-1, "Node out of bounds."); } } /* * Now, determine the local name of the sides adjacent to this * node...this works for the exo patran convention for quads... * * Again, local_node and local_side are zero based... */ local_side[0] = (local_node[ielem]+3)%4; local_side[1] = local_node[ielem]; /* * With the side names, we can find the normal vector. * Again, assume the sides live on the same element. */ load_ei(elem_list[ielem], exo, 0); /* * We abuse the argument list under the conditions that * we're going to do read-only operations and that * we're not interested in old time steps, time derivatives * etc. */ if (x == x_static) /* be the least disruptive possible */ { err = load_elem_dofptr(elem_list[ielem], exo, x_static, x_old_static, xdot_static, xdot_old_static, x_static, 1); } else { err = load_elem_dofptr(elem_list[ielem], exo, x, x, x, x, x, 1); } /* * What are the local coordinates of the nodes in a quadrilateral? */ find_nodal_stu(local_node[ielem], ei->ielem_type, xi, xi+1, xi+2); err = load_basis_functions(xi, bfd); EH( err, "problem from load_basis_functions"); err = beer_belly(); EH( err, "beer_belly"); err = load_fv(); EH( err, "load_fv"); err = load_bf_grad(); EH( err, "load_bf_grad"); err = load_bf_mesh_derivs(); EH(err, "load_bf_mesh_derivs"); if (doPressure) { ordinate = fv->P; iprint = 1; } else { /* First, one side... */ get_side_info(ei->ielem_type, local_side[0]+1, &num_nodes_on_side, side_nodes); surface_determinant_and_normal(elem_list[ielem], exo->elem_node_pntr[elem_list[ielem]], ei->num_local_nodes, ei->ielem_dim-1, local_side[0]+1, num_nodes_on_side, side_nodes); n1[0] = fv->snormal[0]; n1[1] = fv->snormal[1]; /* Second, the adjacent side of the quad... */ get_side_info(ei->ielem_type, local_side[1]+1, &num_nodes_on_side, side_nodes); surface_determinant_and_normal(elem_list[ielem], exo->elem_node_pntr[elem_list[ielem]], ei->num_local_nodes, ei->ielem_dim-1, local_side[1]+1, num_nodes_on_side, side_nodes); n2[0] = fv->snormal[0]; n2[1] = fv->snormal[1]; /* cos (theta) = n1.n2 / ||n1|| ||n2|| */ ordinate += 180. - (180./M_PI)*acos((n1[0]*n2[0] + n1[1]*n2[1])/ (sqrt(n1[0]*n1[0]+n1[1]*n1[1])* sqrt(n2[0]*n2[0]+n2[1]*n2[1]))); } iprint = 1; } /*ielem loop */ } else if ( strncasecmp(qtity_str, "timestepsize", 12 ) == 0 ) { ordinate = time_step_size; iprint = 1; } else if ( strncasecmp(qtity_str, "cputime", 7 ) == 0 ) { ordinate = ut(); iprint = 1; } else if ( strncasecmp(qtity_str, "wallclocktime", 13 ) == 0 ) { /* Get these from extern via main...*/ #ifdef PARALLEL some_time = MPI_Wtime(); ordinate = some_time - time_goma_started; #endif #ifndef PARALLEL time_t now=0; (void)time(&now); ordinate = (double)(now) - time_goma_started; #endif iprint = 1; } else if ( strncasecmp(qtity_str, "speed", 5 ) == 0 ) { id_var = Index_Solution(node, VELOCITY1, 0, 0, mat_num); ordinate = SQUARE(x[id_var]); id_var = Index_Solution(node, VELOCITY2, 0, 0, mat_num); ordinate += SQUARE(x[id_var]); id_var = Index_Solution(node, VELOCITY3, 0, 0, mat_num); ordinate += SQUARE(x[id_var]); ordinate = sqrt(ordinate); iprint = 1; } else if ( strncasecmp(qtity_str, "ac_pres", 7 ) == 0 ) { id_var = Index_Solution(node, ACOUS_PREAL, 0, 0, mat_num); ordinate = SQUARE(x[id_var]); id_var = Index_Solution(node, ACOUS_PIMAG, 0, 0, mat_num); ordinate += SQUARE(x[id_var]); ordinate = sqrt(ordinate); iprint = 1; } else if ( strncasecmp(qtity_str, "light_comp", 10 ) == 0 ) { id_var = Index_Solution(node, LIGHT_INTP, 0, 0, mat_num); ordinate = x[id_var]; id_var = Index_Solution(node, LIGHT_INTM, 0, 0, mat_num); ordinate += x[id_var]; iprint = 1; } else if ( strncasecmp(qtity_str, "nonvolatile", 11 ) == 0 ) { ordinate = 1.0; for(wspec = 0 ; wspec < pd->Num_Species_Eqn ; wspec++) { id_var = Index_Solution(node, MASS_FRACTION, wspec, 0, mat_num); ordinate -= x[id_var]*mp_glob[mat_num]->molar_volume[wspec]; } iprint = 1; } else { WH(id_var, "Requested print variable is not defined at all nodes. May get 0."); if(id_var == -1) iprint = 0; } if ((uf=fopen(filenm,"a")) != NULL) { if ( format_flag[0] == '\0' ) { if (iprint) { fprintf(uf," %e %e %e %e \n", x_pos, y_pos, z_pos, ordinate); } } else { if ( strncasecmp(format_flag, "t", 1) == 0 ) { abscissa = time_value; } else if ( strncasecmp(format_flag, "x", 1) == 0 ) { abscissa = x_pos; } else if ( strncasecmp(format_flag, "y", 1) == 0 ) { abscissa = y_pos; } else if ( strncasecmp(format_flag, "z", 1) == 0 ) { abscissa = z_pos; } else { abscissa = 0; } if (iprint) { fprintf(uf, "%.16g\t%.16g\n", abscissa, ordinate); } } fclose(uf); } } } } print_sync_end(FALSE); return(1); } /* END of routine ns_data_print */