/*
* 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 */
