static
void fallbackQSort3 ( UInt32* fmap,
UInt32* eclass,
Int32 loSt,
Int32 hiSt )
{
Int32 unLo, unHi, ltLo, gtHi, n, m;
Int32 sp, lo, hi;
UInt32 med, r, r3;
Int32 stackLo[FALLBACK_QSORT_STACK_SIZE];
Int32 stackHi[FALLBACK_QSORT_STACK_SIZE];
r = 0;
sp = 0;
fpush ( loSt, hiSt );
while (sp > 0) {
AssertH ( sp < FALLBACK_QSORT_STACK_SIZE, 1004 );
fpop ( lo, hi );
if (hi - lo < FALLBACK_QSORT_SMALL_THRESH) {
fallbackSimpleSort ( fmap, eclass, lo, hi );
continue;
}
/* Random partitioning. Median of 3 sometimes fails to
avoid bad cases. Median of 9 seems to help but
looks rather expensive. This too seems to work but
is cheaper. Guidance for the magic constants
7621 and 32768 is taken from Sedgewick's algorithms
book, chapter 35.
*/
r = ((r * 7621) + 1) % 32768;
r3 = r % 3;
if (r3 == 0) med = eclass[fmap[lo]]; else
if (r3 == 1) med = eclass[fmap[(lo+hi)>>1]]; else
med = eclass[fmap[hi]];
unLo = ltLo = lo;
unHi = gtHi = hi;
while (1) {
while (1) {
if (unLo > unHi) break;
n = (Int32)eclass[fmap[unLo]] - (Int32)med;
if (n == 0) {
fswap(fmap[unLo], fmap[ltLo]);
ltLo++; unLo++;
continue;
};
if (n > 0) break;
unLo++;
}
while (1) {
if (unLo > unHi) break;
n = (Int32)eclass[fmap[unHi]] - (Int32)med;
if (n == 0) {
fswap(fmap[unHi], fmap[gtHi]);
gtHi--; unHi--;
continue;
};
if (n < 0) break;
unHi--;
}
if (unLo > unHi) break;
fswap(fmap[unLo], fmap[unHi]); unLo++; unHi--;
}
AssertD ( unHi == unLo-1, "fallbackQSort3(2)" );
if (gtHi < ltLo) continue;
n = fmin(ltLo-lo, unLo-ltLo); fvswap(lo, unLo-n, n);
m = fmin(hi-gtHi, gtHi-unHi); fvswap(unLo, hi-m+1, m);
n = lo + unLo - ltLo - 1;
m = hi - (gtHi - unHi) + 1;
if (n - lo > hi - m) {
fpush ( lo, n );
fpush ( m, hi );
} else {
fpush ( m, hi );
fpush ( lo, n );
}
}
void plotCubicBezierSegAA(int x0, int y0, double x1, double y1,
double x2, double y2, int x3, int y3)
{ /* plot limited anti-aliased cubic Bezier segment */
int f, fx, fy, leg = 1;
int sx = x0 < x3 ? 1 : -1, sy = y0 < y3 ? 1 : -1; /* step direction */
double xc = -fabs(x0+x1-x2-x3), xa = xc-4*sx*(x1-x2), xb = sx*(x0-x1-x2+x3);
double yc = -fabs(y0+y1-y2-y3), ya = yc-4*sy*(y1-y2), yb = sy*(y0-y1-y2+y3);
double ab, ac, bc, ba, xx, xy, yy, dx, dy, ex, px, py, ed, ip, EP = 0.01;
/* check for curve restrains */
/* slope P0-P1 == P2-P3 and (P0-P3 == P1-P2 or no slope change) */
assert((x1-x0)*(x2-x3) < EP && ((x3-x0)*(x1-x2) < EP || xb*xb < xa*xc+EP));
assert((y1-y0)*(y2-y3) < EP && ((y3-y0)*(y1-y2) < EP || yb*yb < ya*yc+EP));
if (xa == 0 && ya == 0) { /* quadratic Bezier */
sx = (int) floor((3*x1-x0+1)/2); sy = (int) floor((3*y1-y0+1)/2); /* new midpoint */
return plotQuadBezierSegAA(x0,y0, sx,sy, x3,y3);
}
x1 = (x1-x0)*(x1-x0)+(y1-y0)*(y1-y0)+1; /* line lengths */
x2 = (x2-x3)*(x2-x3)+(y2-y3)*(y2-y3)+1;
do { /* loop over both ends */
ab = xa*yb-xb*ya; ac = xa*yc-xc*ya; bc = xb*yc-xc*yb;
ip = 4*ab*bc-ac*ac; /* self intersection loop at all? */
ex = ab*(ab+ac-3*bc)+ac*ac; /* P0 part of self-intersection loop? */
f = (int) (ex > 0 ? 1 : sqrt(1+1024/x1)); /* calculate resolution */
ab *= f; ac *= f; bc *= f; ex *= f*f; /* increase resolution */
xy = 9*(ab+ac+bc)/8; ba = 8*(xa-ya); /* init differences of 1st degree */
dx = 27*(8*ab*(yb*yb-ya*yc)+ex*(ya+2*yb+yc))/64-ya*ya*(xy-ya);
dy = 27*(8*ab*(xb*xb-xa*xc)-ex*(xa+2*xb+xc))/64-xa*xa*(xy+xa);
/* init differences of 2nd degree */
xx = 3*(3*ab*(3*yb*yb-ya*ya-2*ya*yc)-ya*(3*ac*(ya+yb)+ya*ba))/4;
yy = 3*(3*ab*(3*xb*xb-xa*xa-2*xa*xc)-xa*(3*ac*(xa+xb)+xa*ba))/4;
xy = xa*ya*(6*ab+6*ac-3*bc+ba); ac = ya*ya; ba = xa*xa;
xy = 3*(xy+9*f*(ba*yb*yc-xb*xc*ac)-18*xb*yb*ab)/8;
if (ex < 0) { /* negate values if inside self-intersection loop */
dx = -dx; dy = -dy; xx = -xx; yy = -yy; xy = -xy; ac = -ac; ba = -ba;
} /* init differences of 3rd degree */
ab = 6*ya*ac; ac = -6*xa*ac; bc = 6*ya*ba; ba = -6*xa*ba;
dx += xy; ex = dx+dy; dy += xy; /* error of 1st step */
for (fx = fy = f; x0 != x3 && y0 != y3; ) {
y1 = fmin(fabs(xy-dx),fabs(dy-xy));
ed = fmax(fabs(xy-dx),fabs(dy-xy)); /* approximate error distance */
ed = f*(ed+2*ed*y1*y1/(4*ed*ed+y1*y1));
y1 = 255*fabs(ex-(f-fx+1)*dx-(f-fy+1)*dy+f*xy)/ed;
if (y1 < 256)
;//setPixelAA(x0, y0, y1); /* plot curve */
px = fabs(ex-(f-fx+1)*dx+(fy-1)*dy); /* pixel intensity x move */
py = fabs(ex+(fx-1)*dx-(f-fy+1)*dy); /* pixel intensity y move */
y2 = y0;
do { /* move sub-steps of one pixel */
if (ip >= -EP) /* intersection possible? -> check.. */
if (dx+xx > xy || dy+yy < xy) goto exit; /* two x or y steps */
y1 = 2*ex+dx; /* save value for test of y step */
if (2*ex+dy > 0) { /* x sub-step */
fx--; ex += dx += xx; dy += xy += ac; yy += bc; xx += ab;
} else if (y1 > 0) goto exit; /* tiny nearly cusp */
if (y1 <= 0) { /* y sub-step */
fy--; ex += dy += yy; dx += xy += bc; xx += ac; yy += ba;
}
} while (fx > 0 && fy > 0); /* pixel complete? */
if (2*fy <= f) { /* x+ anti-aliasing pixel */
if (py < ed)
;//setPixelAA(x0+sx, y0, 255*py/ed); /* plot curve */
y0 += sy; fy += f; /* y step */
}
if (2*fx <= f) { /* y+ anti-aliasing pixel */
if (px < ed)
;//setPixelAA(x0, y2+sy, 255*px/ed); /* plot curve */
x0 += sx; fx += f; /* x step */
}
}
break; /* finish curve by line */
exit:
if (2*ex < dy && 2*fy <= f+2) { /* round x+ approximation pixel */
if (py < ed)
;//setPixelAA(x0+sx, y0, 255*py/ed); /* plot curve */
y0 += sy;
}
if (2*ex > dx && 2*fx <= f+2) { /* round y+ approximation pixel */
if (px < ed)
;//setPixelAA(x0, y2+sy, 255*px/ed); /* plot curve */
x0 += sx;
}
xx = x0; x0 = x3; x3 = (int) xx; sx = -sx; xb = -xb; /* swap legs */
yy = y0; y0 = y3; y3 = (int) yy; sy = -sy; yb = -yb; x1 = x2;
} while (leg--); /* try other end */
plotLineAA(x0,y0, x3,y3); /* remaining part in case of cusp or crunode */
}
void Route::RenderSegment( ocpnDC& dc, int xa, int ya, int xb, int yb, ViewPort &VP,
bool bdraw_arrow, int hilite_width )
{
// Get the dc boundary
int sx, sy;
dc.GetSize( &sx, &sy );
// Try to exit early if the segment is nowhere near the screen
wxRect r( 0, 0, sx, sy );
wxRect s( xa, ya, 1, 1 );
wxRect t( xb, yb, 1, 1 );
s.Union( t );
if( !r.Intersects( s ) ) return;
// Clip the line segment to the dc boundary
int x0 = xa;
int y0 = ya;
int x1 = xb;
int y1 = yb;
// If hilite is desired, use a Native Graphics context to render alpha colours
// That is, if wxGraphicsContext is available.....
if( hilite_width ) {
if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) ) {
wxPen psave = dc.GetPen();
wxColour y = GetGlobalColor( _T ( "YELO1" ) );
wxColour hilt( y.Red(), y.Green(), y.Blue(), 128 );
wxPen HiPen( hilt, hilite_width, wxSOLID );
dc.SetPen( HiPen );
dc.StrokeLine( x0, y0, x1, y1 );
dc.SetPen( psave );
dc.StrokeLine( x0, y0, x1, y1 );
}
} else {
if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) )
dc.StrokeLine( x0, y0, x1, y1 );
}
if( bdraw_arrow ) {
// Draw a direction arrow
double theta = atan2( (double) ( yb - ya ), (double) ( xb - xa ) );
theta -= PI / 2.;
wxPoint icon[10];
double icon_scale_factor = 100 * VP.view_scale_ppm;
icon_scale_factor = fmin ( icon_scale_factor, 1.5 ); // Sets the max size
icon_scale_factor = fmax ( icon_scale_factor, .10 );
// Get the absolute line length
// and constrain the arrow to be no more than xx% of the line length
double nom_arrow_size = 20.;
double max_arrow_to_leg = .20;
double lpp = sqrt( pow( (double) ( xa - xb ), 2 ) + pow( (double) ( ya - yb ), 2 ) );
double icon_size = icon_scale_factor * nom_arrow_size;
if( icon_size > ( lpp * max_arrow_to_leg ) ) icon_scale_factor = ( lpp * max_arrow_to_leg )
/ nom_arrow_size;
for( int i = 0; i < 7; i++ ) {
int j = i * 2;
double pxa = (double) ( s_arrow_icon[j] );
double pya = (double) ( s_arrow_icon[j + 1] );
pya *= icon_scale_factor;
pxa *= icon_scale_factor;
double px = ( pxa * sin( theta ) ) + ( pya * cos( theta ) );
double py = ( pya * sin( theta ) ) - ( pxa * cos( theta ) );
icon[i].x = (int) ( px ) + xb;
icon[i].y = (int) ( py ) + yb;
}
wxPen savePen = dc.GetPen();
dc.SetPen( *wxTRANSPARENT_PEN );
dc.StrokePolygon( 6, &icon[0], 0, 0 );
dc.SetPen( savePen );
}
}
void Synchronizer::ReadAudioSamples(unsigned int channels, unsigned int sample_rate, AVSampleFormat format, unsigned int sample_count, const uint8_t* data, int64_t timestamp) {
assert(m_output_format->m_audio_enabled);
// sanity check
if(sample_count == 0)
return;
// add new block to sync diagram
if(m_sync_diagram != NULL)
m_sync_diagram->AddBlock(1, (double) timestamp * 1.0e-6, (double) timestamp * 1.0e-6 + (double) sample_count / (double) sample_rate, QColor(0, 255, 0));
AudioLock audiolock(&m_audio_data);
// check the timestamp
if(timestamp < audiolock->m_last_timestamp) {
if(timestamp < audiolock->m_last_timestamp - 10000)
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Received audio samples with non-monotonic timestamp."));
timestamp = audiolock->m_last_timestamp;
}
// update the timestamps
int64_t previous_timestamp;
if(audiolock->m_first_timestamp == (int64_t) AV_NOPTS_VALUE) {
audiolock->m_filtered_timestamp = timestamp;
audiolock->m_first_timestamp = timestamp;
previous_timestamp = timestamp;
} else {
previous_timestamp = audiolock->m_last_timestamp;
}
audiolock->m_last_timestamp = timestamp;
// filter the timestamp
int64_t timestamp_delta = (int64_t) sample_count * (int64_t) 1000000 / (int64_t) sample_rate;
audiolock->m_filtered_timestamp += (timestamp - audiolock->m_filtered_timestamp) / AUDIO_TIMESTAMP_FILTER;
// calculate drift
double current_drift = GetAudioDrift(audiolock.get());
// if there are too many audio samples, drop some of them (unlikely unless you use PulseAudio)
if(current_drift > DRIFT_ERROR_THRESHOLD && !audiolock->m_drop_samples) {
audiolock->m_drop_samples = true;
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Too many audio samples, dropping samples to keep the audio in sync with the video."));
}
// if there are not enough audio samples, insert zeros
if(current_drift < -DRIFT_ERROR_THRESHOLD && !audiolock->m_insert_samples) {
audiolock->m_insert_samples = true;
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Not enough audio samples, inserting silence to keep the audio in sync with the video."));
}
// reset filter and recalculate drift if necessary
if(audiolock->m_drop_samples || audiolock->m_insert_samples) {
audiolock->m_filtered_timestamp = timestamp;
current_drift = GetAudioDrift(audiolock.get());
}
// drop samples
if(audiolock->m_drop_samples) {
audiolock->m_drop_samples = false;
// drop samples
int n = (int) round(current_drift * (double) sample_rate);
if(n > 0) {
if(n >= (int) sample_count) {
audiolock->m_drop_samples = true;
return; // drop all samples
}
if(format == AV_SAMPLE_FMT_FLT) {
data += n * channels * sizeof(float);
} else if(format == AV_SAMPLE_FMT_S16) {
data += n * channels * sizeof(int16_t);
} else {
assert(false);
}
sample_count -= n;
}
}
// insert zeros
unsigned int sample_count_out = 0;
if(audiolock->m_insert_samples) {
audiolock->m_insert_samples = false;
// how many samples should be inserted?
int n = (int) round(-current_drift * (double) sample_rate);
if(n > 0) {
// insert zeros
audiolock->m_temp_input_buffer.Alloc(n * m_output_format->m_audio_channels);
std::fill_n(audiolock->m_temp_input_buffer.GetData(), n * m_output_format->m_audio_channels, 0.0f);
sample_count_out = audiolock->m_fast_resampler->Resample((double) sample_rate / (double) m_output_format->m_audio_sample_rate, 1.0,
audiolock->m_temp_input_buffer.GetData(), n, &audiolock->m_temp_output_buffer, sample_count_out);
// recalculate drift
current_drift = GetAudioDrift(audiolock.get(), sample_count_out);
}
}
// increase filtered timestamp
audiolock->m_filtered_timestamp += timestamp_delta;
// do drift correction
// The point of drift correction is to keep video and audio in sync even when the clocks are not running at exactly the same speed.
// This can happen because the sample rate of the sound card is not always 100% accurate. Even a 0.1% error will result in audio that is
// seconds too early or too late at the end of a one hour video. This problem doesn't occur on all computers though (I'm not sure why).
// Another cause of desynchronization is problems/glitches with PulseAudio (e.g. jumps in time when switching between sources).
double drift_correction_dt = fmin((double) (timestamp - previous_timestamp) * 1.0e-6, DRIFT_MAX_BLOCK);
audiolock->m_average_drift = clamp(audiolock->m_average_drift + DRIFT_CORRECTION_I * current_drift * drift_correction_dt, -0.5, 0.5);
if(audiolock->m_average_drift < -0.02 && audiolock->m_warn_desync) {
audiolock->m_warn_desync = false;
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Audio input is more than 2% too slow!"));
}
if(audiolock->m_average_drift > 0.02 && audiolock->m_warn_desync) {
audiolock->m_warn_desync = false;
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Audio input is more than 2% too fast!"));
}
double length = (double) sample_count / (double) sample_rate;
double drift_correction = clamp(DRIFT_CORRECTION_P * current_drift + audiolock->m_average_drift, -0.5, 0.5) * fmin(1.0, DRIFT_MAX_BLOCK / length);
//qDebug() << "current_drift" << current_drift << "average_drift" << audiolock->m_average_drift << "drift_correction" << drift_correction;
// convert the samples
const float *data_float = NULL; // to keep GCC happy
if(format == AV_SAMPLE_FMT_FLT) {
if(channels == m_output_format->m_audio_channels) {
data_float = (const float*) data;
} else {
audiolock->m_temp_input_buffer.Alloc(sample_count * m_output_format->m_audio_channels);
data_float = audiolock->m_temp_input_buffer.GetData();
SampleChannelRemap(sample_count, (const float*) data, channels, audiolock->m_temp_input_buffer.GetData(), m_output_format->m_audio_channels);
}
} else if(format == AV_SAMPLE_FMT_S16) {
audiolock->m_temp_input_buffer.Alloc(sample_count * m_output_format->m_audio_channels);
data_float = audiolock->m_temp_input_buffer.GetData();
SampleChannelRemap(sample_count, (const int16_t*) data, channels, audiolock->m_temp_input_buffer.GetData(), m_output_format->m_audio_channels);
} else {
assert(false);
}
// resample
sample_count_out = audiolock->m_fast_resampler->Resample((double) sample_rate / (double) m_output_format->m_audio_sample_rate, 1.0 / (1.0 - drift_correction),
data_float, sample_count, &audiolock->m_temp_output_buffer, sample_count_out);
audiolock->m_samples_written += sample_count_out;
SharedLock lock(&m_shared_data);
// avoid memory problems by limiting the audio buffer size
if(lock->m_audio_buffer.GetSize() / m_output_format->m_audio_channels >= MAX_AUDIO_SAMPLES_BUFFERED) {
if(lock->m_segment_video_started) {
Logger::LogWarning("[Synchronizer::ReadAudioSamples] " + Logger::tr("Warning: Audio buffer overflow, starting new segment to keep the audio in sync with the video "
"(some video and/or audio may be lost). The video input seems to be too slow."));
NewSegment(lock.get());
} else {
// If the video hasn't started yet, it makes more sense to drop the oldest samples.
// Shifting the start time like this isn't completely accurate, but this shouldn't happen often anyway.
// The number of samples dropped is calculated so that the buffer will be 90% full after this.
size_t n = lock->m_audio_buffer.GetSize() / m_output_format->m_audio_channels - MAX_AUDIO_SAMPLES_BUFFERED * 9 / 10;
lock->m_audio_buffer.Pop(n * m_output_format->m_audio_channels);
lock->m_segment_audio_start_time += (int64_t) round((double) n / (double) m_output_format->m_audio_sample_rate * 1.0e6);
}
}
// start audio
if(!lock->m_segment_audio_started) {
lock->m_segment_audio_started = true;
lock->m_segment_audio_start_time = timestamp;
lock->m_segment_audio_stop_time = timestamp;
}
// store the samples
lock->m_audio_buffer.Push(audiolock->m_temp_output_buffer.GetData(), sample_count_out * m_output_format->m_audio_channels);
// increase segment stop time
double new_sample_length = (double) (lock->m_segment_audio_samples_read + lock->m_audio_buffer.GetSize() / m_output_format->m_audio_channels) / (double) m_output_format->m_audio_sample_rate;
lock->m_segment_audio_stop_time = lock->m_segment_audio_start_time + (int64_t) round(new_sample_length * 1.0e6);
}
double twoAlleleSpecialCase(int * m) {
unsigned ntables = fmin(m[0], m[1])/2 + 1;
return (double)ntables;
}
SEXP call_rkAuto(SEXP Xstart, SEXP Times, SEXP Func, SEXP Initfunc,
SEXP Parms, SEXP eventfunc, SEXP elist, SEXP Nout, SEXP Rho,
SEXP Rtol, SEXP Atol, SEXP Tcrit, SEXP Verbose,
SEXP Hmin, SEXP Hmax, SEXP Hini, SEXP Rpar, SEXP Ipar,
SEXP Method, SEXP Maxsteps, SEXP Flist) {
/** Initialization **/
long int old_N_Protect = save_N_Protected();
double *tt = NULL, *xs = NULL;
double *y, *f, *Fj, *tmp, *FF, *rr;
SEXP R_yout;
double *y0, *y1, *y2, *dy1, *dy2, *out, *yout;
double errold = 0.0, t, dt, tmax;
SEXP R_FSAL, Alpha, Beta;
int fsal = FALSE; /* assume no FSAL */
/* Use polynomial interpolation if not disabled by the method
or when events come in to play (stop-and-go mode).
Methods with dense output interpolate by default,
all others do not.
*/
int interpolate = TRUE;
int i = 0, j = 0, it = 0, it_tot = 0, it_ext = 0, nt = 0, neq = 0, it_rej = 0;
int isForcing, isEvent;
/*------------------------------------------------------------------------*/
/* Processing of Arguments */
/*------------------------------------------------------------------------*/
int lAtol = LENGTH(Atol);
double *atol = (double*) R_alloc((int) lAtol, sizeof(double));
int lRtol = LENGTH(Rtol);
double *rtol = (double*) R_alloc((int) lRtol, sizeof(double));
for (j = 0; j < lRtol; j++) rtol[j] = REAL(Rtol)[j];
for (j = 0; j < lAtol; j++) atol[j] = REAL(Atol)[j];
double tcrit = REAL(Tcrit)[0];
double hmin = REAL(Hmin)[0];
double hmax = REAL(Hmax)[0];
double hini = REAL(Hini)[0];
int maxsteps = INTEGER(Maxsteps)[0];
int nout = INTEGER(Nout)[0]; /* number of global outputs is func is in a DLL */
int verbose = INTEGER(Verbose)[0];
int stage = (int)REAL(getListElement(Method, "stage"))[0];
SEXP R_A, R_B1, R_B2, R_C, R_D, R_densetype;
double *A, *bb1, *bb2 = NULL, *cc = NULL, *dd = NULL;
PROTECT(R_A = getListElement(Method, "A")); incr_N_Protect();
A = REAL(R_A);
PROTECT(R_B1 = getListElement(Method, "b1")); incr_N_Protect();
bb1 = REAL(R_B1);
PROTECT(R_B2 = getListElement(Method, "b2")); incr_N_Protect();
if (length(R_B2)) bb2 = REAL(R_B2);
PROTECT(R_C = getListElement(Method, "c")); incr_N_Protect();
if (length(R_C)) cc = REAL(R_C);
PROTECT(R_D = getListElement(Method, "d")); incr_N_Protect();
if (length(R_D)) dd = REAL(R_D);
/* dense output Cash-Karp: densetype = 2 */
int densetype = 0;
PROTECT(R_densetype = getListElement(Method, "densetype")); incr_N_Protect();
if (length(R_densetype)) densetype = INTEGER(R_densetype)[0];
double qerr = REAL(getListElement(Method, "Qerr"))[0];
double beta = 0; /* 0.4/qerr; */
PROTECT(Beta = getListElement(Method, "beta")); incr_N_Protect();
if (length(Beta)) beta = REAL(Beta)[0];
double alpha = 1/qerr - 0.75 * beta;
PROTECT(Alpha = getListElement(Method, "alpha")); incr_N_Protect();
if (length(Alpha)) alpha = REAL(Alpha)[0];
PROTECT(R_FSAL = getListElement(Method, "FSAL")); incr_N_Protect();
if (length(R_FSAL)) fsal = INTEGER(R_FSAL)[0];
PROTECT(Times = AS_NUMERIC(Times)); incr_N_Protect();
tt = NUMERIC_POINTER(Times);
nt = length(Times);
PROTECT(Xstart = AS_NUMERIC(Xstart)); incr_N_Protect();
xs = NUMERIC_POINTER(Xstart);
neq = length(Xstart);
/*------------------------------------------------------------------------*/
/* timesteps (for advection computation in ReacTran) */
/*------------------------------------------------------------------------*/
for (i = 0; i < 2; i++) timesteps[i] = 0;
/*------------------------------------------------------------------------*/
/* DLL, ipar, rpar (for compatibility with lsoda) */
/*------------------------------------------------------------------------*/
int isDll = FALSE;
int lrpar= 0, lipar = 0;
int *ipar = NULL;
/* code adapted from lsoda to improve compatibility */
if (inherits(Func, "NativeSymbol")) {
/* function is a dll */
isDll = TRUE;
if (nout > 0) isOut = TRUE;
//ntot = neq + nout; /* length of yout */
lrpar = nout + LENGTH(Rpar); /* length of rpar; LENGTH(Rpar) is always >0 */
lipar = 3 + LENGTH(Ipar); /* length of ipar */
} else {
/* function is not a dll */
isDll = FALSE;
isOut = FALSE;
//ntot = neq;
lipar = 3; /* in lsoda = 1 */
lrpar = nout; /* in lsoda = 1 */
}
out = (double*) R_alloc(lrpar, sizeof(double));
ipar = (int *) R_alloc(lipar, sizeof(int));
/* first 3 elements of ipar are special */
ipar[0] = nout;
ipar[1] = lrpar;
ipar[2] = lipar;
if (isDll == 1) {
/* other elements of ipar are set in R-function lsodx via argument "ipar" */
for (j = 0; j < LENGTH(Ipar); j++) ipar[j+3] = INTEGER(Ipar)[j];
/* out: first nout elements of out are reserved for output variables
other elements are set via argument "rpar" */
for (j = 0; j < nout; j++) out[j] = 0.0;
for (j = 0; j < LENGTH(Rpar); j++) out[nout+j] = REAL(Rpar)[j];
}
/*------------------------------------------------------------------------*/
/* Allocation of Workspace */
/*------------------------------------------------------------------------*/
y0 = (double*) R_alloc(neq, sizeof(double));
y1 = (double*) R_alloc(neq, sizeof(double));
y2 = (double*) R_alloc(neq, sizeof(double));
dy1 = (double*) R_alloc(neq, sizeof(double));
dy2 = (double*) R_alloc(neq, sizeof(double));
f = (double*) R_alloc(neq, sizeof(double));
y = (double*) R_alloc(neq, sizeof(double));
Fj = (double*) R_alloc(neq, sizeof(double));
tmp = (double*) R_alloc(neq, sizeof(double));
FF = (double*) R_alloc(neq * stage, sizeof(double));
rr = (double*) R_alloc(neq * 5, sizeof(double));
/* matrix for polynomial interpolation */
SEXP R_nknots;
int nknots = 6; /* 6 = 5th order polynomials by default*/
int iknots = 0; /* counter for knots buffer */
double *yknots;
PROTECT(R_nknots = getListElement(Method, "nknots")); incr_N_Protect();
if (length(R_nknots)) nknots = INTEGER(R_nknots)[0] + 1;
if (nknots < 2) {nknots = 1; interpolate = FALSE;}
if (densetype > 0) interpolate = TRUE;
yknots = (double*) R_alloc((neq + 1) * (nknots + 1), sizeof(double));
/* matrix for holding states and global outputs */
PROTECT(R_yout = allocMatrix(REALSXP, nt, neq + nout + 1)); incr_N_Protect();
yout = REAL(R_yout);
/* initialize outputs with NA first */
for (i = 0; i < nt * (neq + nout + 1); i++) yout[i] = NA_REAL;
/* attribute that stores state information, similar to lsoda */
SEXP R_istate;
int *istate;
PROTECT(R_istate = allocVector(INTSXP, 22)); incr_N_Protect();
istate = INTEGER(R_istate);
istate[0] = 0; /* assume succesful return */
for (i = 0; i < 22; i++) istate[i] = 0;
/*------------------------------------------------------------------------*/
/* Initialization of Parameters (for DLL functions) */
/*------------------------------------------------------------------------*/
PROTECT(Y = allocVector(REALSXP,(neq))); incr_N_Protect();
initParms(Initfunc, Parms);
isForcing = initForcings(Flist);
isEvent = initEvents(elist, eventfunc, 0);
if (isEvent) interpolate = FALSE;
/*------------------------------------------------------------------------*/
/* Initialization of Integration Loop */
/*------------------------------------------------------------------------*/
yout[0] = tt[0]; /* initial time */
yknots[0] = tt[0]; /* for polynomial interpolation */
for (i = 0; i < neq; i++) {
y0[i] = xs[i]; /* initial values */
yout[(i + 1) * nt] = y0[i]; /* output array */
yknots[iknots + nknots * (i + 1)] = xs[i]; /* for polynomials */
}
iknots++;
t = tt[0];
tmax = fmax(tt[nt - 1], tcrit);
dt = fmin(hmax, hini);
hmax = fmin(hmax, tmax - t);
/* Initialize work arrays (to be on the safe side, remove this later) */
for (i = 0; i < neq; i++) {
y1[i] = 0;
y2[i] = 0;
Fj[i] = 0;
for (j= 0; j < stage; j++) {
FF[i + j * neq] = 0;
}
}
/*------------------------------------------------------------------------*/
/* Main Loop */
/*------------------------------------------------------------------------*/
it = 1; /* step counter; zero element is initial state */
it_ext = 0; /* counter for external time step (dense output) */
it_tot = 0; /* total number of time steps */
it_rej = 0;
if (interpolate) {
/* integrate over the whole time step and interpolate internally */
rk_auto(
fsal, neq, stage, isDll, isForcing, verbose, nknots, interpolate,
densetype, maxsteps, nt,
&iknots, &it, &it_ext, &it_tot, &it_rej,
istate, ipar,
t, tmax, hmin, hmax, alpha, beta,
&dt, &errold,
tt, y0, y1, y2, dy1, dy2, f, y, Fj, tmp, FF, rr, A,
out, bb1, bb2, cc, dd, atol, rtol, yknots, yout,
Func, Parms, Rho
);
} else {
/* integrate separately between external time steps; do not interpolate */
for (int j = 0; j < nt - 1; j++) {
t = tt[j];
tmax = fmin(tt[j + 1], tcrit);
dt = tmax - t;
if (isEvent) {
updateevent(&t, y0, istate);
}
if (verbose) Rprintf("\n %d th time interval = %g ... %g", j, t, tmax);
rk_auto(
fsal, neq, stage, isDll, isForcing, verbose, nknots, interpolate,
densetype, maxsteps, nt,
&iknots, &it, &it_ext, &it_tot, &it_rej,
istate, ipar,
t, tmax, hmin, hmax, alpha, beta,
&dt, &errold,
tt, y0, y1, y2, dy1, dy2, f, y, Fj, tmp, FF, rr, A,
out, bb1, bb2, cc, dd, atol, rtol, yknots, yout,
Func, Parms, Rho
);
/* in this mode, internal interpolation is skipped,
so we can simply store the results at the end of each call */
yout[j + 1] = tmax;
for (i = 0; i < neq; i++) yout[j + 1 + nt * (1 + i)] = y2[i];
}
}
/*====================================================================*/
/* call derivs again to get global outputs */
/* j = -1 suppresses unnecessary internal copying */
/*====================================================================*/
if (nout > 0) {
for (int j = 0; j < nt; j++) {
t = yout[j];
for (i = 0; i < neq; i++) tmp[i] = yout[j + nt * (1 + i)];
derivs(Func, t, tmp, Parms, Rho, FF, out, -1, neq, ipar, isDll, isForcing);
for (i = 0; i < nout; i++) {
yout[j + nt * (1 + neq + i)] = out[i];
}
}
}
/* attach diagnostic information (codes are compatible to lsoda) */
setIstate(R_yout, R_istate, istate, it_tot, stage, fsal, qerr, it_rej);
if (densetype == 2) istate[12] = it_tot * stage + 2; /* number of function evaluations */
/* verbose printing in debugging mode*/
if (verbose)
Rprintf("\nNumber of time steps it = %d, it_ext = %d, it_tot = %d it_rej %d\n",
it, it_ext, it_tot, it_rej);
/* release R resources */
timesteps[0] = 0;
timesteps[1] = 0;
restore_N_Protected(old_N_Protect);
return(R_yout);
}
int main (int argc, char *argv[])
{
char headerless;
double obstime;
char string[80];
long seed=-1;
float fval;
unsigned char A,B,C,D;
char help=0;
uint_fast32_t i;
/* set up default variables */
strcpy(inpfile,"stdin");
strcpy(outfile,"stdout");
headerless=0;
machine_id=telescope_id=0;
machine_id=10;
telescope_id=4;
nchans=1024;
nbits=2;
tstart=56000.0;
tsamp=64.0; // microseconds.. will be converted to seconds later
fch1=1581.804688;
foff=-0.390625;
nifs=1;
nbeams=1;
ibeam=1;
obstime=270.0;
output=stdout;
help=getB("--help","-h",argc,argv,0);
obstime=getF("--tobs","-T",argc,argv,obstime);
telescope_id=getI("--tid","",argc,argv,telescope_id);
machine_id=getI("--bid","",argc,argv,machine_id);
tsamp=getF("--tsamp","-t",argc,argv,tsamp);
tstart=getF("--mjd","-m",argc,argv,tstart);
fch1=getF("--fch1","-F",argc,argv,fch1);
foff=getF("--foff","-f",argc,argv,foff);
nbits=getI("--nbits","-b",argc,argv,nbits);
nchans=getI("--nchans","-c",argc,argv,nchans);
seed=getI("--seed","-S",argc,argv,seed);
char test_mode=getB("--test","-0",argc,argv,0);
strcpy(outfile,getS("--out","-o",argc,argv,"stdout"));
strcpy(source_name,getS("--name","-s",argc,argv,"FAKE"));
getArgs(&argc,argv);
if (help || argc > 1){
for(i=1; i < argc; i++)logerr("Unknown argument '%s'",argv[i]);
fastfake_help();
}
logmsg("FASTFAKE - M.Keith 2014");
time_t t0 = time(NULL);
if (seed<0)seed=t0;
mjk_rand_t *rnd = mjk_rand_init(seed);
logmsg("tobs = %lfs",obstime);
logmsg("tsamp = %lfus",tsamp);
logmsg("mjdstart = %lf",tstart);
logmsg("freq chan 1 = %lfMHz",fch1);
logmsg("freq offset = %lfMHz",foff);
logmsg("output nbits = %d",nbits);
logmsg("random seed = %ld",seed);
logmsg("output file = '%s'",outfile);
tsamp*=1e-6; // convert tsamp to us
if(STREQ(outfile,"stdout")){
output=stdout;
} else{
output=fopen(outfile,"w");
}
if (!headerless) {
logmsg("write header");
send_string("HEADER_START");
send_string("source_name");
send_string(source_name);
send_int("machine_id",machine_id);
send_int("telescope_id",telescope_id);
send_int("data_type",1);
send_double("fch1",fch1);
send_double("foff",foff);
send_int("nchans",nchans);
send_int("nbits",nbits);
send_int("nbeams",nbeams);
send_int("ibeam",ibeam);
send_double("tstart",tstart);
send_double("tsamp",tsamp);
send_int("nifs",nifs);
if (nbits==8){
send_char("signed",OSIGN);
}
send_string("HEADER_END");
}
int bits_per_sample = (nbits * nchans * nifs);
if (bits_per_sample % 8 != 0){
logerr("bits per sample is not a multiple of 8");
exit(1);
}
int bytes_per_sample = bits_per_sample / 8.0;
uint64_t nsamples = (uint64_t)(obstime/tsamp+0.5);
logmsg("Generate %lld samples, %.3lf GiB",nsamples,nsamples*bytes_per_sample/pow(2,30));
uint64_t onepercent = nsamples/100;
int percent=0;
unsigned char* buffer = (unsigned char*) malloc(sizeof(unsigned char)*bytes_per_sample);
if (nbits==1 || nbits==2 || nbits==4 | nbits==8){
// integer samples
for(uint64_t samp = 0; samp < nsamples; samp++){
if (samp%onepercent ==0){
double t1=(double)(time(NULL)-t0)+1e-3;
double bytespersec = samp*bytes_per_sample/t1;
fprintf(stderr,"Complete: % 3d%%. Sample: % 9lld Real time % 6.1lfs, Sim time % 6.1lfs. Speed % 4.2lfMiB/s\r",percent,samp,t1,(double)samp*tsamp,bytespersec/pow(2,20));
fflush(stderr);
percent+=1;
}
mjk_rand_gauss_atleast(rnd,nchans);
const int chanskip = 8/nbits;
//#pragma omp parallel for schedule(dynamic,1)
for(uint64_t chan = 0; chan < nchans; chan+=chanskip){
switch(nbits){
case 1:
buffer[chan/8] = mjk_rand(rnd)&0xFF;
break;
case 2:
fval = (mjk_rand_gauss(rnd)+1.5);
fval = fmax(fval,0);
fval = fmin(fval,3.0);
A = (unsigned char)(fval)&0x3;
fval = (mjk_rand_gauss(rnd)+1.5);
fval = fmax(fval,0);
fval = fmin(fval,3.0);
B = (unsigned char)(fval)&0x3 << 2;
fval = (mjk_rand_gauss(rnd)+1.5);
fval = fmax(fval,0);
fval = fmin(fval,3.0);
C = (unsigned char)(fval)&0x3 << 4;
fval = (mjk_rand_gauss(rnd)+1.5);
fval = fmax(fval,0);
fval = fmin(fval,3.0);
D = (unsigned char)(fval)&0x3 << 6;
buffer[chan/4]=A|B|C|D;
break;
case 4:
fval = (mjk_rand_gauss(rnd)*3.0+7.5);
fval = fmax(fval,0);
fval = fmin(fval,15.0);
A = (unsigned char)(fval)&0xF;
fval = (mjk_rand_gauss(rnd)*3.0+7.5);
fval = fmax(fval,0);
fval = fmin(fval,15.0);
B = ((unsigned char)(fval)&0xF)<<4;
buffer[chan/2] = A|B;
break;
case 8:
fval = (mjk_rand_gauss(rnd)*24.0+96.0); // more headroom.
fval = fmax(fval,0);
fval = fmin(fval,255.0);
buffer[chan] = (unsigned char)fval;
break;
}
}
if(!test_mode)fwrite(buffer,sizeof(unsigned char),bytes_per_sample,output);
}
} else if (nbits==32){
// float samples
float* fbuf = (float*)buffer;
for(uint64_t samp = 0; samp < nsamples; samp++){
if (samp%onepercent ==0){
double t1=(double)(time(NULL)-t0)+1e-3;
double bytespersec = samp*bytes_per_sample/t1;
fprintf(stderr,"Complete: % 3d%%. Sample: % 9lld Real time % 6.1lfs, Sim time % 6.1lfs. Speed % 4.2lfMiB/s\r",percent,samp,t1,(double)samp*tsamp,bytespersec/pow(2,20));
fflush(stderr);
percent+=1;
}
for(uint64_t chan = 0; chan < nchans; chan++){
fbuf[chan] = mjk_rand_gauss(rnd);
}
if(!test_mode)fwrite(buffer,sizeof(unsigned char),bytes_per_sample,output);
}
}
fprintf(stderr,"\n");
free(buffer);
mjk_rand_free(rnd);
logmsg("Done!");
fclose(output);
return 0;
}
int main(int argc, char ** argv)
{
int nLevels = 10;
int levelCube = 8;
vmml::vector<3,int> offset(0,0,0);
lunchbox::Clock clock;
std::vector<std::string> parameters;
parameters.push_back(std::string(argv[1]));
parameters.push_back(std::string(argv[2]));
if (argc == 4)
{
std::string n(argv[3]);
mO = boost::lexical_cast<double>(n);
}
hdf5File.init(parameters);
if (!ccc.initParameter(parameters, mO))
{
std::cerr<<"Error init control cube cpu cache"<<std::endl;
}
vmml::vector<3, int> dim = hdf5File.getRealDimension();
std::cout<<"Checking errors........."<<std::endl;
bool error = false;
for(int i=0; i<10 && !error; i++)
{
vmml::vector<3,int> s;
vmml::vector<3,int> e;
int nLevels = 0;
int dimA = 0;
int dimV = 0;
do
{
s.set(rand() % dim.x(), 0, rand() % dim.z());
do
{
e.set(rand() % (dim.x() - s.x()) + s.x(), rand() % (dim.y() - s.y()) + s.y(), rand() % (dim.z() - s.z()) + s.z());
}
while(s.x() >= e.x() || s.y() >= e.y() || s.z() >= e.z());
/* Calcular dimension del árbol*/
dimA = fmin(e.x()-s.x(), fmin(e.y() - s.y(), e.z() - s.z()));
float aux = logf(dimA)/logf(2.0);
float aux2 = aux - floorf(aux);
nLevels = aux2>0.0 ? aux+1 : aux;
dimV = exp2(nLevels);
}
while(nLevels <= 1 || s.x()+dimV >= dim.x() || s.y()+dimV >= dim.y() || s.z()+dimV >= dim.z());
int levelCube = rand() % (nLevels - 1) + 1;
std::cout<<"Test "<<i<<" nLevels "<<nLevels<<" levelCube "<<levelCube<<" dimension "<<exp2(nLevels - levelCube)<<" offset "<<s<<" : "<<std::endl;
error = test(nLevels, levelCube, s);
if (error)
std::cout<<"Test Fail!"<<std::endl;
else
{
std::cout<<"Test OK"<<std::endl;
}
}
std::cout<<"Checking performance........."<<std::endl;
for(int i=0; i<10 && !error; i++)
{
vmml::vector<3,int> s;
vmml::vector<3,int> e;
int nLevels = 0;
int dimA = 0;
int dimV = 0;
do
{
s.set(rand() % dim.x(), 0, rand() % dim.z());
do
{
e.set(rand() % (dim.x() - s.x()) + s.x(), rand() % (dim.y() - s.y()) + s.y(), rand() % (dim.z() - s.z()) + s.z());
}
while(s.x() >= e.x() || s.y() >= e.y() || s.z() >= e.z());
dimA = fmin(e.x()-s.x(), fmin(e.y() - s.y(), e.z() - s.z()));;
/* Calcular dimension del árbol*/
float aux = logf(dimA)/logf(2.0);
float aux2 = aux - floorf(aux);
nLevels = aux2>0.0 ? aux+1 : aux;
dimV = exp2(nLevels);
}
while(nLevels <= 1 || s.x()+dimV >= dim.x() || s.y()+dimV >= dim.y() || s.z()+dimV >= dim.z());
int levelCube = rand() % (nLevels - 1) + 1;
std::cout<<"Test "<<i<<" nLevels "<<nLevels<<" levelCube "<<levelCube<<" dimension "<<exp2(nLevels - levelCube)<<" offset "<<s<<" : "<<std::endl;
double time = 0.0;
clock.reset();
testPerf(nLevels, levelCube, s);
time = clock.getTimed()/1000.0;
double bw = ((((dimV-s.x())*(dimV-s.y())*(dimV-s.z()))*sizeof(float))/1204.0/1024.0)/time;
std::cout<<"Test "<<s<<" "<<e<<": "<<time<<" seconds ~ "<<bw<<" MB/s"<<std::endl;
}
if (!error)
{
int dimA = fmax(dim.x(), fmaxf(dim.y(), dim.z()));
nLevels = 0;
/* Calcular dimension del árbol*/
float aux = logf(dimA)/logf(2.0);
float aux2 = aux - floorf(aux);
nLevels = aux2>0.0 ? aux+1 : aux;
levelCube = rand() % (nLevels - 4) + 4;
std::cout<<"Test reading complete volume"<<std::endl;
if (test(nLevels, levelCube, vmml::vector<3,int>(0,0,0)))
{
std::cerr<<"Test Fail!!"<<std::endl;
}
else
{
double time = 0.0;
clock.reset();
testPerf(nLevels, levelCube, vmml::vector<3,int>(0,0,0));
time = clock.getTimed()/1000.0;
double bw = (((dim.x()*dim.y()*dim.z())*sizeof(float))/1204.0/1024.0)/time;
std::cout<<"Read complete volume "<<dim<<" : "<<time<<" seconds ~ "<<bw<<" MB/s"<<std::endl;
}
}
ccc.stopCache();
hdf5File.close();
return 0;
}
float2 fmin(float2 p, float2 q)
{
return float2(fmin(p.x, q.x), fmin(p.y, q.y));
}
void AdvectionManager::advance(double dt, const TimeLevelIndex<2> &newTimeIdx,
const VelocityField &velocity) {
TimeLevelIndex<2> oldTimeIdx = newTimeIdx-1;
TimeLevelIndex<2> halfTimeIdx = newTimeIdx-0.5;
const double eps = 1.0e-80;
const double R = domain->getRadius();
for (int s = 0; s < Q.size(); ++s) {
ScalarField &q = *Q[s];
double dQ;
int J;
//#define ONLY_LAX_WENDROFF
//#define ONLY_UPWIND
#ifndef ONLY_UPWIND
// ---------------------------------------------------------------------
// Lax-Wendroff pass
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, HALF); ++i) {
double f1 = dt/(R*dlon[i]);
double f2 = f1/cosLatFull[j];
double u = velocity(0)(halfTimeIdx, i, j);
double q1 = q(oldTimeIdx, i, j);
double q2 = q(oldTimeIdx, i+1, j);
FX(i, j) = 0.5*f1*(u*(q2+q1)-u*u*f2*(q2-q1));
}
}
FX.applyBndCond();
for (int j = 0; j < mesh->getNumGrid(1, HALF); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double f1 = dt/(R*dlat[j]);
double f2 = f1/cosLatHalf[j];
double v = velocity(1)(halfTimeIdx, i, j)*cosLatHalf[j];
double q1 = q(oldTimeIdx, i, j );
double q2 = q(oldTimeIdx, i, j+1);
FY(i, j) = 0.5*f1*(v*(q2+q1)-v*v*f2*(q2-q1));
}
}
FY.applyBndCond();
#endif
#if (!defined ONLY_LAX_WENDROFF && !defined ONLY_UPWIND)
// ---------------------------------------------------------------------
// calculate intermediate Qstar
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double fx1 = dt/(R*dlon[i]*cosLatFull[i]); // TODO: Should we support irregular lon grids?
double fx2 = dt/(R*dlon[i]*cosLatFull[i]);
double fy1 = dt/(R*dlat[j-1]*cosLatHalf[j-1]);
double fy2 = dt/(R*dlat[j ]*cosLatHalf[j ]);
double u1 = velocity(0)(halfTimeIdx, i-1, j);
double u2 = velocity(0)(halfTimeIdx, i, j);
double v1 = velocity(1)(halfTimeIdx, i, j-1);
double v2 = velocity(1)(halfTimeIdx, i, j );
double tmp1 = fabs(u1*fx1)*(1-fabs(u1*fx1));
double tmp2 = fabs(u2*fx2)*(1-fabs(u2*fx2));
double tmp3 = fabs(v1*fy1)*(1-fabs(v1*fy1));
double tmp4 = fabs(v2*fy2)*(1-fabs(v2*fy2));
double gamma = fmax(fmax(tmp1, tmp2), fmax(tmp3, tmp4));
B(i, j) = 2/(2-2*gamma);
}
}
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, j) = q(oldTimeIdx, i, j)-B(i, j)/cosLatFull[j]*
(FX(i, j)-FX(i-1, j)+FY(i, j)-FY(i, j-1));
}
}
// handle poles
// south pole
dQ = 0; J = 0;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += B(i, J+1)*FY(i, J);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, J) = q(oldTimeIdx, i, J)-dQ;
}
// north pole
dQ = 0; J = mesh->getNumGrid(1, FULL)-1;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += B(i, J-1)*FY(i, J-1);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J-1];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, J) = q(oldTimeIdx, i, J)+dQ;
}
// ---------------------------------------------------------------------
// shape-preserving rule (A <= 0 is good)
for (int j = 0; j < mesh->getNumGrid(1, FULL); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double Qmin = 1.0e+15;
double Qmax = -1.0e+15;
if (j == 0) {
Qmin = fmin(fmin(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j+1)), Qmin);
Qmax = fmax(fmax(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j+1)), Qmax);
} else if (j == mesh->getNumGrid(1, FULL)-1) {
Qmin = fmin(fmin(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j-1)), Qmin);
Qmax = fmax(fmax(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j-1)), Qmax);
} else {
Qmin = fmin(fmin(fmin(q(oldTimeIdx, i-1, j), q(oldTimeIdx, i+1, j)),
fmin(q(oldTimeIdx, i, j-1), q(oldTimeIdx, i, j+1))),
fmin(q(oldTimeIdx, i, j), Qmin));
Qmax = fmax(fmax(fmax(q(oldTimeIdx, i-1, j), q(oldTimeIdx, i+1, j)),
fmax(q(oldTimeIdx, i, j-1), q(oldTimeIdx, i, j+1))),
fmax(q(oldTimeIdx, i, j), Qmax));
}
A(i, j) = (Qstar(i, j)-Qmax)*(Qstar(i, j)-Qmin);
}
}
A.applyBndCond();
#endif
#ifndef ONLY_LAX_WENDROFF
// ---------------------------------------------------------------------
// upwind pass
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, HALF); ++i) {
#ifndef ONLY_UPWIND
double tmp1 = (fabs(A(i, j))+A(i, j))/(fabs(A(i, j))+eps);
double tmp2 = (fabs(A(i+1, j))+A(i+1, j))/(fabs(A(i+1, j))+eps);
double tmp3 = (fabs(A(i+1, j))+A(i+1, j))*(fabs(A(i, j))+A(i, j));
double tmp4 = fabs(A(i, j))*fabs(A(i+1, j))+eps;
double cxstar = 0.5*(tmp1+tmp2)-0.25*tmp3/tmp4;
#else
double cxstar = 1;
#endif
double f = dt/(R*dlon[i]);
double u = velocity(0)(halfTimeIdx, i, j);
double q1 = q(oldTimeIdx, i, j);
double q2 = q(oldTimeIdx, i+1, j);
double cx = cxstar+(1-cxstar)*fabs(u*f/cosLatFull[j]);
FX(i, j) = 0.5*f*(u*(q2+q1)-fabs(cx*u)*(q2-q1));
}
}
FX.applyBndCond();
for (int j = 0; j < mesh->getNumGrid(1, HALF); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
#ifndef ONLY_UPWIND
double tmp1 = (fabs(A(i, j ))+A(i, j ))/(fabs(A(i, j ))+eps);
double tmp2 = (fabs(A(i, j+1))+A(i, j+1))/(fabs(A(i, j+1))+eps);
double tmp3 = (fabs(A(i, j+1))+A(i, j+1))*(fabs(A(i, j))+A(i, j));
double tmp4 = fabs(A(i, j))*fabs(A(i, j+1))+eps;
double cystar = 0.5*(tmp1+tmp2)-0.25*tmp3/tmp4;
#else
double cystar = 1;
#endif
double f = dt/(R*dlat[j]);
double v = velocity(1)(halfTimeIdx, i, j)*cosLatHalf[j];
double q1 = q(oldTimeIdx, i, j );
double q2 = q(oldTimeIdx, i, j+1);
double cy = cystar+(1-cystar)*fabs(v*f/cosLatHalf[j]);
FY(i, j) = 0.5*f*(v*(q2+q1)-fabs(cy*v)*(q2-q1));
}
}
FY.applyBndCond();
#endif
// ---------------------------------------------------------------------
// calculate final Q
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, j) = q(oldTimeIdx, i, j)-
(FX(i, j)-FX(i-1, j)+FY(i, j)-FY(i, j-1))/cosLatFull[j];
}
}
// handle poles
// south pole
dQ = 0; J = 0;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += FY(i, J);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, J) = q(oldTimeIdx, i, J)-dQ;
}
// north pole
dQ = 0; J = mesh->getNumGrid(1, FULL)-1;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += FY(i, J-1);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J-1];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, J) = q(oldTimeIdx, i, J)+dQ;
}
q.applyBndCond(newTimeIdx);
}
diagnose(newTimeIdx);
}
void drawClock(NVGcontext* vg, int screenWidth, int screenHeight) {
int clockRadius = (fmin(screenWidth, screenHeight) - 10) / 2;
NVGcolor baseColor = nvgRGB(145, 100, 0);
NVGcolor lighterColor = nvgRGB(200, 140, 0);
NVGcolor dblLighterColor = nvgRGB(255, 177, 0);
NVGcolor lightestColor = nvgRGB(255, 195, 60);
int widthLight = fmax(clockRadius * 0.0075, 1);
int widthMedium = fmax(clockRadius * 0.013, 1);
int widthHeavy = fmax(clockRadius * 0.025, 1);
time_t t = time(NULL);
struct tm localTime = *localtime(&t);
nvgTranslate(vg, screenWidth / 2, screenHeight / 2);
nvgStrokeColor(vg, baseColor);
// Frame
nvgSave(vg);
nvgBeginPath(vg);
nvgCircle(vg, 0, 0, clockRadius);
nvgStrokeWidth(vg, widthMedium);
nvgStroke(vg);
nvgBeginPath(vg);
nvgStrokeColor(vg, lighterColor);
for (int i = 0; i < 12; i++) {
nvgMoveTo(vg, 0, -clockRadius * 0.98);
nvgLineTo(vg, 0, -clockRadius * 0.90);
for (int j = 0; j < 5; j++) {
nvgRotate(vg, 2 * M_PI / 12 / 5);
nvgMoveTo(vg, 0, -clockRadius * 0.98);
nvgLineTo(vg, 0, -clockRadius * 0.95);
}
}
nvgStroke(vg);
nvgBeginPath(vg);
nvgFontFace(vg, "bold");
int digitsRadius = clockRadius * 0.79;
int digitLargeSize = clockRadius * 0.29;
int digitSmallSize = clockRadius * 0.2;
int clockTitleSize = clockRadius * 0.06;
nvgFillColor(vg, dblLighterColor);
char* nineTitle = "9";
nvgFontSize(vg, digitLargeSize);
nvgTranslate(vg, -digitsRadius, 0);
nvgRotate(vg, - 3 * 2 * M_PI / 12);
//nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, nineTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, lighterColor);
char* tenTitle = "10";
nvgFontSize(vg, digitSmallSize);
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, tenTitle, 0, 0);
nvgFill(vg);
char* elevenTitle = "11";
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, elevenTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, dblLighterColor);
char* twelveTitle = "12";
nvgFontSize(vg, digitLargeSize);
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, twelveTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, lighterColor);
char* oneTitle = "1";
nvgFontSize(vg, digitSmallSize);
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, oneTitle, 0, 0);
nvgFill(vg);
char* twoTitle = "2";
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, twoTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, dblLighterColor);
char* threeTitle = "3";
nvgFontSize(vg, digitLargeSize);
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, -digitsRadius);
drawTextCenter(vg, threeTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, lighterColor);
char* fourTitle = "4";
nvgFontSize(vg, digitSmallSize);
nvgTranslate(vg, 0, digitsRadius);
nvgRotate(vg, 2 * M_PI / 12 - M_PI);
nvgTranslate(vg, 0, digitsRadius);
drawTextCenter(vg, fourTitle, 0, 0);
nvgFill(vg);
char* fiveTitle = "5";
nvgTranslate(vg, 0, -digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, digitsRadius);
drawTextCenter(vg, fiveTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, dblLighterColor);
char* sixTitle = "6";
nvgFontSize(vg, digitLargeSize);
nvgTranslate(vg, 0, -digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, digitsRadius);
drawTextCenter(vg, sixTitle, 0, 0);
nvgFill(vg);
nvgFillColor(vg, lighterColor);
char* sevenTitle = "7";
nvgFontSize(vg, digitSmallSize);
nvgTranslate(vg, 0, -digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, digitsRadius);
drawTextCenter(vg, sevenTitle, 0, 0);
nvgFill(vg);
char* eightTitle = "8";
nvgTranslate(vg, 0, -digitsRadius);
nvgRotate(vg, 2 * M_PI / 12);
nvgTranslate(vg, 0, digitsRadius);
drawTextCenter(vg, eightTitle, 0, 0);
nvgFill(vg);
nvgRestore(vg);
nvgSave(vg);
nvgFontFace(vg, "black");
char* clockTitle = "ALPHA";
nvgFontSize(vg, clockTitleSize);
nvgFillColor(vg, baseColor);
nvgTranslate(vg, 0, clockTitleSize);
nvgTextLetterSpacing(vg, clockTitleSize);
drawTextCenter(vg, clockTitle, 0, -2 * clockTitleSize);
nvgFill(vg);
nvgRestore(vg);
double secAngle = 2 * M_PI / 60 * localTime.tm_sec;
double minAngle = 2 * M_PI / 60 * localTime.tm_min + secAngle / 60;
double hrAngle = 2 * M_PI / 12 * localTime.tm_hour + minAngle / 60;
// Hour hand
nvgStrokeColor(vg, lighterColor);
nvgSave(vg);
nvgRotate(vg, hrAngle);
nvgBeginPath(vg);
nvgMoveTo(vg, 0, clockRadius * 0.02);
nvgLineTo(vg, 0, -clockRadius * 0.5);
nvgStrokeWidth(vg, widthHeavy);
nvgStroke(vg);
nvgRestore(vg);
// Minute hand
nvgStrokeColor(vg, lighterColor);
nvgSave(vg);
nvgRotate(vg, minAngle);
nvgBeginPath(vg);
nvgMoveTo(vg, 0, clockRadius * 0.04);
nvgLineTo(vg, 0, -clockRadius * 0.8);
nvgStrokeWidth(vg, widthMedium);
nvgStroke(vg);
nvgRestore(vg);
// Second hand
nvgStrokeColor(vg, lightestColor);
nvgSave(vg);
nvgRotate(vg, secAngle);
nvgBeginPath(vg);
nvgMoveTo(vg, 0, clockRadius * 0.05);
nvgLineTo(vg, 0, -clockRadius * 0.9);
nvgStrokeWidth(vg, widthLight);
nvgStroke(vg);
nvgRestore(vg);
}
/* Calculate change in momentum when there is contact. Return true or false if an adjustment was calculated
If BC at the node, the delta momemtum should be zero in fixed direction
Only called if both verified are verified and have 1 or more particles
This method should ignore material that are ignoring cracks
*/
bool CrackSurfaceContact::GetDeltaMomentum(NodalPoint *np,Vector *delPa,CrackVelocityField *cva,CrackVelocityField *cvb,
Vector *normin,int number,bool postUpdate,double deltime,int *inContact)
{
// first determine if there is contact
*inContact=IN_CONTACT;
// velocities above and below
bool hasParticles;
double massa,massb;
Vector pka=cva->GetCMatMomentum(hasParticles,&massa);
Vector pkb=cvb->GetCMatMomentum(hasParticles,&massb);
double mnode=1./(massa+massb);
// screen low masses
double aratio=massa*mnode;
if(aratio<1.e-6 || aratio>0.999999) return false;
//if(aratio<1.e-3 || aratio>0.999) return FALSE;
// find Delta p_a (see notes)
CopyScaleVector(delPa,&pkb,massa*mnode);
AddScaledVector(delPa,&pka,-massb*mnode);
// get normalized normal vector and find Delta p_a . n (actual (vb-va).n = dotn*(ma+mb)/(ma*mb))
Vector norm;
CopyScaleVector(&norm,normin,1./sqrt(DotVectors2D(normin,normin)));
double dotn=DotVectors2D(delPa,&norm);
// With the first check, any movement apart will be taken as noncontact
// Also, frictional contact assume dvel<0.
if(dotn>=0.)
*inContact=SEPARATED;
else
{ // if approaching, check displacements
// (Note: to use only velocity, skip the following displacement check)
Vector dispa=cva->GetCMDisplacement(np,true);
dispa.x/=massa;
dispa.y/=massa;
Vector dispb=cvb->GetCMDisplacement(np,true);
dispb.x/=massb;
dispb.y/=massb;
// normal cod
double dnorm=(dispb.x-dispa.x)*norm.x + (dispb.y-dispa.y)*norm.y
- mpmgrid.GetNormalCODAdjust(&norm,NULL,0);
if(postUpdate)
{ double dvel=(massa+massb)*dotn/(massa*massb);
dnorm+=dvel*deltime;
}
// if current displacement positive then no contact
if(dnorm >= 0.) *inContact=SEPARATED;
}
// if separated, then no contact unless possibly needed for an imperfect interface
if(crackContactLaw[number]->ContactIsDone(*inContact==IN_CONTACT)) return false;
// Now need to change momentum. For imperfect interface, change only for perfect directions
double mredDE;
if(crackContactLaw[number]->IsFrictionalContact())
{ bool getHeating = postUpdate && ConductionTask::crackContactHeating;
double mred = (massa*massb)/(massa+massb);
double contactArea = 1.;
if(crackContactLaw[number]->FrictionLawNeedsContactArea())
{ // Angled path correction (2D only)
double dist = mpmgrid.GetPerpendicularDistance(&norm, NULL, 0.);
// Area correction method (new): sqrt(2*vmin/vtot)*vtot/dist = sqrt(2*vmin*vtot)/dist
double vola = cva->GetVolumeNonrigid(true),volb = cvb->GetVolumeNonrigid(true),voltot=vola+volb;
contactArea = sqrt(2.0*fmin(vola,volb)*voltot)/dist;
if(fmobj->IsAxisymmetric()) contactArea *= np->x;
}
if(!crackContactLaw[number]->GetFrictionalDeltaMomentum(delPa,&norm,dotn,&mredDE,mred,
getHeating,contactArea,*inContact==IN_CONTACT,deltime,NULL))
{ return false;
}
if(mredDE>0.)
{ double qrate = mredDE/mred;
NodalPoint::frictionWork += qrate;
// As heat source need nJ/sec or multiply by 1/timestep
// Note that this is after transport rates are calculated (by true in last parameter)
conduction->AddFluxCondition(np,fabs(qrate/deltime),true);
}
}
else
{
// Contact handled here only perfect interface (Dt or Dn < 0)
// Imperfect interfaces are handled as forces later
if(crackContactLaw[number]->IsPerfectTangentialInterface())
{ if(!crackContactLaw[number]->IsPerfectNormalInterface(*inContact==IN_CONTACT))
{ // prefect in tangential, but imperfect in normal direction
// make stick in tangential direction only
AddScaledVector(delPa,&norm,-dotn);
}
// else perfect in both so return with the stick conditions already in delPa
}
else if(crackContactLaw[number]->IsPerfectNormalInterface(*inContact==IN_CONTACT))
{ // perfect in normal direction, but imperfect in tangential direction
// make stick in normal direction only
CopyScaleVector(delPa,&norm,dotn);
}
else
{ // no change in momentum, just imperfect interface forces later and nothing changed here
return false;
}
}
return true;
}
sd->val = from;
pd->intvl_to = to;
if (pd->intvl_from < sd->val_min) {
pd->intvl_from = sd->val_min;
sd->val = sd->val_min;
}
if (pd->intvl_to > sd->val_max) pd->intvl_to = sd->val_max;
efl_ui_slider_val_set(obj);
}
EOLIAN static void
_efl_ui_slider_interval_interval_value_get(const Eo *obj EINA_UNUSED, Efl_Ui_Slider_Interval_Data *pd, double *from, double *to)
{
if (from) *from = fmin(pd->intvl_from, pd->intvl_to);
if (to) *to = fmax(pd->intvl_from, pd->intvl_to);
}
EOLIAN static Efl_Object *
_efl_ui_slider_interval_efl_object_constructor(Eo *obj, Efl_Ui_Slider_Interval_Data *pd EINA_UNUSED)
{
ELM_WIDGET_DATA_GET_OR_RETURN(obj, wd, NULL);
if (!elm_widget_theme_klass_get(obj))
elm_widget_theme_klass_set(obj, "slider_interval");
obj = efl_constructor(efl_super(obj, MY_CLASS));
return obj;
}
/*
* Core function for computing the ROM waveform.
* Interpolate projection coefficient data and evaluate coefficients at desired (q, chi).
* Construct 1D splines for amplitude and phase.
* Compute strain waveform from amplitude and phase.
*/
static int SEOBNRv1ROMDoubleSpinCore(
COMPLEX16FrequencySeries **hptilde,
COMPLEX16FrequencySeries **hctilde,
double phiRef,
double fRef,
double distance,
double inclination,
double Mtot_sec,
double q,
double chi1,
double chi2,
const REAL8Sequence *freqs_in, /* Frequency points at which to evaluate the waveform (Hz) */
double deltaF
/* If deltaF > 0, the frequency points given in freqs are uniformly spaced with
* spacing deltaF. Otherwise, the frequency points are spaced non-uniformly.
* Then we will use deltaF = 0 to create the frequency series we return. */
)
{
/* Check output arrays */
if(!hptilde || !hctilde)
XLAL_ERROR(XLAL_EFAULT);
SEOBNRROMdataDS *romdata=&__lalsim_SEOBNRv1ROMDS_data;
if(*hptilde || *hctilde) {
XLALPrintError("(*hptilde) and (*hctilde) are supposed to be NULL, but got %p and %p",(*hptilde),(*hctilde));
XLAL_ERROR(XLAL_EFAULT);
}
int retcode=0;
// 'Nudge' parameter values to allowed boundary values if close by
if (q < 1.0) nudge(&q, 1.0, 1e-6);
if (q > 10.0) nudge(&q, 10.0, 1e-6);
if (chi1 < -1.0) nudge(&chi1, -1.0, 1e-6);
if (chi1 > 0.6) nudge(&chi1, 0.6, 1e-6);
if (chi2 < -1.0) nudge(&chi2, -1.0, 1e-6);
if (chi2 > 0.6) nudge(&chi2, 0.6, 1e-6);
/* If either spin > 0.6, model not available, exit */
if ( chi1 < -1.0 || chi2 < -1.0 || chi1 > 0.6 || chi2 > 0.6 ) {
XLALPrintError( "XLAL Error - %s: chi1 or chi2 smaller than -1 or larger than 0.6!\nSEOBNRv1ROMDoubleSpin is only available for spins in the range -1 <= a/M <= 0.6.\n", __func__);
XLAL_ERROR( XLAL_EDOM );
}
if (q > 10) {
XLALPrintError( "XLAL Error - %s: q=%lf larger than 10!\nSEOBNRv1ROMDoubleSpin is only available for q in the range 1 <= q <= 10.\n", __func__, q);
XLAL_ERROR( XLAL_EDOM );
}
/* Find frequency bounds */
if (!freqs_in) XLAL_ERROR(XLAL_EFAULT);
double fLow = freqs_in->data[0];
double fHigh = freqs_in->data[freqs_in->length - 1];
if(fRef==0.0)
fRef=fLow;
/* Convert to geometric units for frequency */
double Mf_ROM_min = fmax(gA[0], gPhi[0]); // lowest allowed geometric frequency for ROM
double Mf_ROM_max = fmin(gA[nk_amp-1], gPhi[nk_phi-1]); // highest allowed geometric frequency for ROM
double fLow_geom = fLow * Mtot_sec;
double fHigh_geom = fHigh * Mtot_sec;
double fRef_geom = fRef * Mtot_sec;
double deltaF_geom = deltaF * Mtot_sec;
// Enforce allowed geometric frequency range
if (fLow_geom < Mf_ROM_min)
XLAL_ERROR(XLAL_EDOM, "Starting frequency Mflow=%g is smaller than lowest frequency in ROM Mf=%g.\n", fLow_geom, Mf_ROM_min);
if (fHigh_geom == 0)
fHigh_geom = Mf_ROM_max;
else if (fHigh_geom > Mf_ROM_max) {
XLALPrintWarning("Maximal frequency Mf_high=%g is greater than highest ROM frequency Mf_ROM_Max=%g. Using Mf_high=Mf_ROM_Max.", fHigh_geom, Mf_ROM_max);
fHigh_geom = Mf_ROM_max;
}
else if (fHigh_geom < Mf_ROM_min)
XLAL_ERROR(XLAL_EDOM, "End frequency %g is smaller than starting frequency %g!\n", fHigh_geom, fLow_geom);
if (fRef_geom > Mf_ROM_max) {
XLALPrintWarning("Reference frequency Mf_ref=%g is greater than maximal frequency in ROM Mf=%g. Starting at maximal frequency in ROM.\n", fRef_geom, Mf_ROM_max);
fRef_geom = Mf_ROM_max; // If fref > fhigh we reset fref to default value of cutoff frequency.
}
if (fRef_geom < Mf_ROM_min) {
XLALPrintWarning("Reference frequency Mf_ref=%g is smaller than lowest frequency in ROM Mf=%g. Starting at lowest frequency in ROM.\n", fRef_geom, Mf_ROM_min);
fRef_geom = Mf_ROM_min;
}
/* Internal storage for w.f. coefficiencts */
SEOBNRROMdataDS_coeff *romdata_coeff=NULL;
SEOBNRROMdataDS_coeff_Init(&romdata_coeff);
REAL8 amp_pre;
/* Interpolate projection coefficients and evaluate them at (q,chi1,chi2) */
retcode=TP_Spline_interpolation_3d(
q, // Input: q-value for which projection coefficients should be evaluated
chi1, // Input: chi1-value for which projection coefficients should be evaluated
chi2, // Input: chi2-value for which projection coefficients should be evaluated
romdata->cvec_amp, // Input: data for spline coefficients for amplitude
romdata->cvec_phi, // Input: data for spline coefficients for phase
romdata->cvec_amp_pre, // Input: data for spline coefficients for amplitude prefactor
romdata_coeff->c_amp, // Output: interpolated projection coefficients for amplitude
romdata_coeff->c_phi, // Output: interpolated projection coefficients for phase
&_pre // Output: interpolated amplitude prefactor
);
if(retcode!=0) {
SEOBNRROMdataDS_coeff_Cleanup(romdata_coeff);
XLAL_ERROR(retcode, "Parameter-space interpolation failed.");
}
// Compute function values of amplitude an phase on sparse frequency points by evaluating matrix vector products
// amp_pts = B_A^T . c_A
// phi_pts = B_phi^T . c_phi
gsl_vector* amp_f = gsl_vector_alloc(nk_amp);
gsl_vector* phi_f = gsl_vector_alloc(nk_phi);
gsl_blas_dgemv(CblasTrans, 1.0, romdata->Bamp, romdata_coeff->c_amp, 0.0, amp_f);
gsl_blas_dgemv(CblasTrans, 1.0, romdata->Bphi, romdata_coeff->c_phi, 0.0, phi_f);
// Setup 1d splines in frequency
gsl_interp_accel *acc_amp = gsl_interp_accel_alloc();
gsl_spline *spline_amp = gsl_spline_alloc(gsl_interp_cspline, nk_amp);
gsl_spline_init(spline_amp, gA, gsl_vector_const_ptr(amp_f,0), nk_amp);
gsl_interp_accel *acc_phi = gsl_interp_accel_alloc();
gsl_spline *spline_phi = gsl_spline_alloc(gsl_interp_cspline, nk_phi);
gsl_spline_init(spline_phi, gPhi, gsl_vector_const_ptr(phi_f,0), nk_phi);
size_t npts = 0;
LIGOTimeGPS tC = {0, 0};
UINT4 offset = 0; // Index shift between freqs and the frequency series
REAL8Sequence *freqs = NULL;
if (deltaF > 0) { // freqs contains uniform frequency grid with spacing deltaF; we start at frequency 0
/* Set up output array with size closest power of 2 */
npts = NextPow2(fHigh_geom / deltaF_geom) + 1;
if (fHigh_geom < fHigh * Mtot_sec) /* Resize waveform if user wants f_max larger than cutoff frequency */
npts = NextPow2(fHigh * Mtot_sec / deltaF_geom) + 1;
XLALGPSAdd(&tC, -1. / deltaF); /* coalesce at t=0 */
*hptilde = XLALCreateCOMPLEX16FrequencySeries("hptilde: FD waveform", &tC, 0.0, deltaF, &lalStrainUnit, npts);
*hctilde = XLALCreateCOMPLEX16FrequencySeries("hctilde: FD waveform", &tC, 0.0, deltaF, &lalStrainUnit, npts);
// Recreate freqs using only the lower and upper bounds
// Use fLow, fHigh and deltaF rather than geometric frequencies for numerical accuracy
double fHigh_temp = fHigh_geom / Mtot_sec;
UINT4 iStart = (UINT4) ceil(fLow / deltaF);
UINT4 iStop = (UINT4) ceil(fHigh_temp / deltaF);
freqs = XLALCreateREAL8Sequence(iStop - iStart);
if (!freqs) {
XLAL_ERROR(XLAL_EFUNC, "Frequency array allocation failed.");
}
for (UINT4 i=iStart; i<iStop; i++)
freqs->data[i-iStart] = i*deltaF_geom;
offset = iStart;
} else { // freqs contains frequencies with non-uniform spacing; we start at lowest given frequency
npts = freqs_in->length;
*hptilde = XLALCreateCOMPLEX16FrequencySeries("hptilde: FD waveform", &tC, fLow, 0, &lalStrainUnit, npts);
*hctilde = XLALCreateCOMPLEX16FrequencySeries("hctilde: FD waveform", &tC, fLow, 0, &lalStrainUnit, npts);
offset = 0;
freqs = XLALCreateREAL8Sequence(freqs_in->length);
if (!freqs) {
XLAL_ERROR(XLAL_EFUNC, "Frequency array allocation failed.");
}
for (UINT4 i=0; i<freqs_in->length; i++)
freqs->data[i] = freqs_in->data[i] * Mtot_sec;
}
if (!(*hptilde) || !(*hctilde))
{
XLALDestroyREAL8Sequence(freqs);
gsl_spline_free(spline_amp);
gsl_spline_free(spline_phi);
gsl_interp_accel_free(acc_amp);
gsl_interp_accel_free(acc_phi);
gsl_vector_free(amp_f);
gsl_vector_free(phi_f);
SEOBNRROMdataDS_coeff_Cleanup(romdata_coeff);
XLAL_ERROR(XLAL_EFUNC, "Waveform allocation failed.");
}
memset((*hptilde)->data->data, 0, npts * sizeof(COMPLEX16));
memset((*hctilde)->data->data, 0, npts * sizeof(COMPLEX16));
XLALUnitMultiply(&(*hptilde)->sampleUnits, &(*hptilde)->sampleUnits, &lalSecondUnit);
XLALUnitMultiply(&(*hctilde)->sampleUnits, &(*hctilde)->sampleUnits, &lalSecondUnit);
COMPLEX16 *pdata=(*hptilde)->data->data;
COMPLEX16 *cdata=(*hctilde)->data->data;
REAL8 cosi = cos(inclination);
REAL8 pcoef = 0.5*(1.0 + cosi*cosi);
REAL8 ccoef = cosi;
REAL8 s = 0.5; // Scale polarization amplitude so that strain agrees with FFT of SEOBNRv1
double Mtot = Mtot_sec / LAL_MTSUN_SI;
double amp0 = Mtot * amp_pre * Mtot_sec * LAL_MRSUN_SI / (distance); // Correct overall amplitude to undo mass-dependent scaling used in ROM
// Evaluate reference phase for setting phiRef correctly
double phase_change = gsl_spline_eval(spline_phi, fRef_geom, acc_phi) - 2*phiRef;
// Assemble waveform from aplitude and phase
for (UINT4 i=0; i<freqs->length; i++) { // loop over frequency points in sequence
double f = freqs->data[i];
if (f > Mf_ROM_max) continue; // We're beyond the highest allowed frequency; since freqs may not be ordered, we'll just skip the current frequency and leave zero in the buffer
int j = i + offset; // shift index for frequency series if needed
double A = gsl_spline_eval(spline_amp, f, acc_amp);
double phase = gsl_spline_eval(spline_phi, f, acc_phi) - phase_change;
COMPLEX16 htilde = s*amp0*A * cexp(I*phase);
pdata[j] = pcoef * htilde;
cdata[j] = -I * ccoef * htilde;
}
/* Correct phasing so we coalesce at t=0 (with the definition of the epoch=-1/deltaF above) */
// Get SEOBNRv1 ringdown frequency for 22 mode
double Mf_final = SEOBNRROM_Ringdown_Mf_From_Mtot_q(Mtot_sec, q, chi1, chi2, SEOBNRv1);
UINT4 L = freqs->length;
// prevent gsl interpolation errors
if (Mf_final > freqs->data[L-1])
Mf_final = freqs->data[L-1];
if (Mf_final < freqs->data[0])
{
XLALDestroyREAL8Sequence(freqs);
gsl_spline_free(spline_amp);
gsl_spline_free(spline_phi);
gsl_interp_accel_free(acc_amp);
gsl_interp_accel_free(acc_phi);
gsl_vector_free(amp_f);
gsl_vector_free(phi_f);
SEOBNRROMdataDS_coeff_Cleanup(romdata_coeff);
XLAL_ERROR(XLAL_EDOM, "f_ringdown < f_min");
}
// Time correction is t(f_final) = 1/(2pi) dphi/df (f_final)
// We compute the dimensionless time correction t/M since we use geometric units.
REAL8 t_corr = gsl_spline_eval_deriv(spline_phi, Mf_final, acc_phi) / (2*LAL_PI);
// Now correct phase
for (UINT4 i=0; i<freqs->length; i++) { // loop over frequency points in sequence
double f = freqs->data[i] - fRef_geom;
int j = i + offset; // shift index for frequency series if needed
pdata[j] *= cexp(-2*LAL_PI * I * f * t_corr);
cdata[j] *= cexp(-2*LAL_PI * I * f * t_corr);
}
XLALDestroyREAL8Sequence(freqs);
gsl_spline_free(spline_amp);
gsl_spline_free(spline_phi);
gsl_interp_accel_free(acc_amp);
gsl_interp_accel_free(acc_phi);
gsl_vector_free(amp_f);
gsl_vector_free(phi_f);
SEOBNRROMdataDS_coeff_Cleanup(romdata_coeff);
return(XLAL_SUCCESS);
}
zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
{
////////////////////////////////////////////////////////
// step 1. threshold the image, creating the edge image.
int w = im->width, h = im->height, s = im->stride;
image_u8_t *threshim = threshold(td, im);
assert(threshim->stride == s);
image_u8_t *edgeim = image_u8_create(w, h);
if (1) {
image_u8_t *sumim = image_u8_create(w, h);
// apply a horizontal sum kernel of width 3
for (int y = 0; y < h; y++) {
for (int x = 1; x+1 < w; x++) {
sumim->buf[y*s + x] =
threshim->buf[y*s + x - 1] +
threshim->buf[y*s + x + 0] +
threshim->buf[y*s + x + 1];
}
}
timeprofile_stamp(td->tp, "sumim");
// deglitch
if (td->qtp.deglitch) {
for (int y = 1; y+1 < h; y++) {
for (int x = 1; x+1 < w; x++) {
// edge: black pixel next to white pixel
if (threshim->buf[y*s + x] == 0 &&
sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 8) {
threshim->buf[y*s + x] = 1;
sumim->buf[y*s + x - 1]++;
sumim->buf[y*s + x + 0]++;
sumim->buf[y*s + x + 1]++;
}
if (threshim->buf[y*s + x] == 1 &&
sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 1) {
threshim->buf[y*s + x] = 0;
sumim->buf[y*s + x - 1]--;
sumim->buf[y*s + x + 0]--;
sumim->buf[y*s + x + 1]--;
}
}
}
timeprofile_stamp(td->tp, "deglitch");
}
// apply a vertical sum kernel of width 3; check if any
// over-threshold pixels are adjacent to an under-threshold
// pixel.
//
// There are two types of edges: white pixels neighboring a
// black pixel, and black pixels neighboring a white pixel. We
// label these separately. (Values 0xc0 and 0x3f are picked
// such that they add to 255 (see below) and so that they can be
// viewed as pixel intensities for visualization purposes.)
//
// symmetry of detection. We don't want to use JUST "black
// near white" (or JUST "white near black"), because that
// biases the detection towards one side of the edge. This
// measurably reduces detection performance.
//
// On large tags, we could treat "neighbor" pixels the same
// way. But on very small tags, there may be other edges very
// near the tag edge. Since each of these edges is effectively
// two pixels thick (the white pixel near the black pixel, and
// the black pixel near the white pixel), it becomes likely
// that these two nearby edges will actually touch.
//
// A partial solution to this problem is to define edges to be
// adjacent white-near-black and black-near-white pixels.
//
for (int y = 1; y+1 < h; y++) {
for (int x = 1; x+1 < w; x++) {
if (threshim->buf[y*s + x] == 0) {
// edge: black pixel next to white pixel
if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] > 0)
edgeim->buf[y*s + x] = 0xc0;
} else {
// edge: white pixel next to black pixel when both
// edge types are on, we get less bias towards one
// side of the edge.
if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] < 9)
edgeim->buf[y*s + x] = 0x3f;
}
}
}
if (td->debug) {
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
threshim->buf[y*s + x] *= 255;
}
}
image_u8_write_pnm(threshim, "debug_threshold.pnm");
image_u8_write_pnm(edgeim, "debug_edge.pnm");
// image_u8_destroy(edgeim2);
}
image_u8_destroy(threshim);
image_u8_destroy(sumim);
}
timeprofile_stamp(td->tp, "edges");
////////////////////////////////////////////////////////
// step 2. find connected components.
unionfind_t *uf = unionfind_create(w * h);
for (int y = 1; y < h - 1; y++) {
for (int x = 1; x < w -1; x++) {
uint8_t v = edgeim->buf[y*s + x];
if (v==0)
continue;
// (dx,dy) pairs for 8 connectivity:
// (REFERENCE) (1, 0)
// (-1, 1) (0, 1) (1, 1)
//
// i.e., the minimum value of dx should be:
// y=0: 1
// y=1: -1
for (int dy = 0; dy <= 1; dy++) {
for (int dx = 1-2*dy; dx <= 1; dx++) {
if (edgeim->buf[(y+dy)*s + (x+dx)] == v) {
unionfind_connect(uf, y*w + x, (y+dy)*w + x + dx);
}
}
}
}
}
timeprofile_stamp(td->tp, "unionfind");
zhash_t *clustermap = zhash_create(sizeof(uint64_t), sizeof(zarray_t*),
zhash_uint64_hash, zhash_uint64_equals);
for (int y = 1; y < h-1; y++) {
for (int x = 1; x < w-1; x++) {
uint8_t v0 = edgeim->buf[y*s + x];
if (v0 == 0)
continue;
uint64_t rep0 = unionfind_get_representative(uf, y*w + x);
// 8 connectivity. (4 neighbors to check).
// for (int dy = 0; dy <= 1; dy++) {
// for (int dx = 1-2*dy; dx <= 1; dx++) {
// 4 connectivity. (2 neighbors to check)
for (int n = 1; n <= 2; n++) {
int dy = n & 1;
int dx = (n & 2) >> 1;
uint8_t v1 = edgeim->buf[(y+dy)*s + x + dx];
if (v0 + v1 != 255)
continue;
uint64_t rep1 = unionfind_get_representative(uf, (y+dy)*w + x+dx);
uint64_t clusterid;
if (rep0 < rep1)
clusterid = (rep1 << 32) + rep0;
else
clusterid = (rep0 << 32) + rep1;
zarray_t *cluster = NULL;
if (!zhash_get(clustermap, &clusterid, &cluster)) {
cluster = zarray_create(sizeof(struct pt));
zhash_put(clustermap, &clusterid, &cluster, NULL, NULL);
}
// NB: We will add some points multiple times to a
// given cluster. I don't know an efficient way to
// avoid that here; we remove them later on when we
// sort points by pt_compare_theta.
if (1) {
struct pt p = { .x = x, .y = y};
zarray_add(cluster, &p);
}
if (1) {
struct pt p = { .x = x+dx, .y = y+dy};
zarray_add(cluster, &p);
}
}
}
}
// make segmentation image.
if (td->debug) {
image_u8_t *d = image_u8_create(w, h);
assert(d->stride == s);
uint8_t *colors = (uint8_t*) calloc(w*h, 1);
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
uint32_t v = unionfind_get_representative(uf, y*w+x);
uint32_t sz = unionfind_get_set_size(uf, y*w+x);
if (sz < td->qtp.min_cluster_pixels)
continue;
uint8_t color = colors[v];
if (color == 0) {
const int bias = 20;
color = bias + (random() % (255-bias));
colors[v] = color;
}
float mix = 0.7;
mix = 1.0;
d->buf[y*d->stride + x] = mix*color + (1-mix)*im->buf[y*im->stride + x];
}
}
free(colors);
image_u8_write_pnm(d, "debug_segmentation.pnm");
image_u8_destroy(d);
}
timeprofile_stamp(td->tp, "make clusters");
////////////////////////////////////////////////////////
// step 3. process each connected component.
zarray_t *clusters = zhash_values(clustermap);
zhash_destroy(clustermap);
zarray_t *quads = zarray_create(sizeof(struct quad));
int sz = zarray_size(clusters);
int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
struct quad_task tasks[sz / chunksize + 1];
int ntasks = 0;
for (int i = 0; i < sz; i += chunksize) {
tasks[ntasks].td = td;
tasks[ntasks].cidx0 = i;
tasks[ntasks].cidx1 = imin(sz, i + chunksize);
tasks[ntasks].h = h;
tasks[ntasks].w = w;
tasks[ntasks].quads = quads;
tasks[ntasks].clusters = clusters;
tasks[ntasks].im = im;
workerpool_add_task(td->wp, do_quad_task, &tasks[ntasks]);
ntasks++;
}
workerpool_run(td->wp);
timeprofile_stamp(td->tp, "fit quads to clusters");
if (td->debug) {
FILE *f = fopen("debug_lines.ps", "w");
fprintf(f, "%%!PS\n\n");
image_u8_t *im2 = image_u8_copy(im);
image_u8_darken(im2);
image_u8_darken(im2);
// assume letter, which is 612x792 points.
double scale = fmin(612.0/im->width, 792.0/im2->height);
fprintf(f, "%.15f %.15f scale\n", scale, scale);
fprintf(f, "0 %d translate\n", im2->height);
fprintf(f, "1 -1 scale\n");
postscript_image(f, im);
for (int i = 0; i < zarray_size(quads); i++) {
struct quad *q;
zarray_get_volatile(quads, i, &q);
float rgb[3];
int bias = 100;
for (int i = 0; i < 3; i++)
rgb[i] = bias + (random() % (255-bias));
fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
fprintf(f, "%.15f %.15f moveto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto stroke\n",
q->p[0][0], q->p[0][1],
q->p[1][0], q->p[1][1],
q->p[2][0], q->p[2][1],
q->p[3][0], q->p[3][1],
q->p[0][0], q->p[0][1]);
}
fclose(f);
}
// printf(" %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
/*
if (td->debug) {
for (int i = 0; i < 4; i++) {
int i0 = indices[i];
int i1 = indices[(i+1)&3];
if (i1 < i0)
i1 += zarray_size(cluster);
for (int j = i0; j <= i1; j++) {
struct pt *p;
zarray_get_volatile(cluster, j % zarray_size(cluster), &p);
edgeim->buf[p->y*edgeim->stride + p->x] = 30+64*i;
}
}
} */
unionfind_destroy(uf);
for (int i = 0; i < zarray_size(clusters); i++) {
zarray_t *cluster;
zarray_get(clusters, i, &cluster);
zarray_destroy(cluster);
}
zarray_destroy(clusters);
image_u8_destroy(edgeim);
return quads;
}
float3 fmin(float3 p, float3 q)
{
return float3(fmin(p.x, q.x), fmin(p.y, q.y), fmin(p.z, q.z));
}
void evolve(double *G, double *h, double *u, double g, double dx, double dt, int nBC, int n,double *nG, double *nh)
{
//modifies nh and nG to give the new values of h and G after a single time step
double idx = 1.0 / dx;
double ithree = 1.0 / 3.0;
int i = nBC - 1;
//calculate values at right of i cell
double hir = h[i];
double Gir = G[i];
double uir = u[i];
//calculate values at left of i+1 cell
double hip1l = h[i+1];
double Gip1l = G[i+1];
double uip1l = u[i+1];
//right force
double duer = idx*(u[i+2] - u[i+1]);
double duel = idx*(u[i] - u[i-1]);
double sqrtghel = sqrt(g* hir);
double sqrtgher = sqrt(g* hip1l);
double sl = fmin(0,fmin(uir - sqrtghel, uip1l - sqrtgher));
double sr = fmax(0,fmax(uir + sqrtghel, uip1l + sqrtgher));
double felh = uir*hir;
double felG = Gir*uir + 0.5*g*hir*hir - 2*ithree*hir*hir*hir*duel*duel;
double ferh = uip1l*hip1l;
double ferG = Gip1l*uip1l + 0.5*g*hip1l*hip1l -2*ithree*hip1l*hip1l*hip1l*duer*duer;
double isrmsl = 0.0;
if(sr != sl) isrmsl = 1.0 / (sr - sl);
double foh = isrmsl*(sr*felh - sl*ferh + sl*sr*(hip1l - hir));
double foG = isrmsl*(sr*felG - sl*ferG + sl*sr*(Gip1l - Gir));
double fih = foh;
double fiG = foG;
for (i = nBC ; i < n +nBC;i++)
{
//i right
hir = h[i];
Gir = G[i];
uir = u[i];
//i+1 left
hip1l = h[i+1];
Gip1l = G[i+1];
uip1l = u[i+1];
duer = idx*(u[i+2] - u[i+1]);
duel = idx*(u[i] - u[i-1]);
sqrtghel = sqrt(g*hir);
sqrtgher = sqrt(g*hip1l);
sl = fmin(0,fmin(uir - sqrtghel, uip1l - sqrtgher));
sr = fmax(0,fmax(uir + sqrtghel, uip1l + sqrtgher));
felh = uir*hir;
felG = Gir*uir + 0.5*g*hir*hir - 2*ithree*hir*hir*hir*duel*duel;
ferh = uip1l*hip1l;
ferG = Gip1l*uip1l + 0.5*g*hip1l*hip1l -2*ithree*hip1l*hip1l*hip1l*duer*duer;
isrmsl = 0.0;
if(sr != sl) isrmsl = 1.0 / (sr - sl);
foh = isrmsl*(sr*felh - sl*ferh + sl*sr*(hip1l - hir));
foG = isrmsl*(sr*felG - sl*ferG + sl*sr*(Gip1l - Gir));
//source term
nh[i -nBC] = h[i] -dt*idx*(foh - fih);
nG[i -nBC] = G[i] -dt*idx*(foG -fiG);
fih = foh;
fiG = foG;
}
}
float4 fmin(float4 p, float4 q)
{
return float4(fmin(p.x, q.x), fmin(p.y, q.y), fmin(p.z, q.z), fmin(p.w, q.w));
}
/**
* \brief Callback for the joystick topic.
*/
void joyCb(const sensor_msgs::Joy::ConstPtr& msg)
{
last_recv_ = ros::Time::now();
deadman_ = msg->buttons[deadman_button_];
deadman_head_ = msg->buttons[deadman_head_button_];
deadman_arm_angular_ = msg->buttons[deadman_arm_angular_button_];
deadman_arm_linear_ = msg->buttons[deadman_arm_linear_button_];
if (!deadman_)
{
controller_.stop();
return;
}
controller_.start();
if (deadman_arm_angular_ || deadman_arm_linear_)
{
// Stop moving base
vx = 0.0;
vw = 0.0;
// Stop head
pan = 0.0;
tilt = 0.0;
// Stop moving torso
torso = 0.0;
if (deadman_arm_linear_)
{
// Linear takes precedence over angular
arm_lx = msg->axes[arm_lx_axis_]*arm_linear_scale_;
arm_ly = msg->axes[arm_ly_axis_]*arm_linear_scale_;
arm_lz = msg->axes[arm_lz_axis_]*arm_linear_scale_;
arm_ax = 0.0;
arm_ay = 0.0;
arm_az = 0.0;
}
else
{
arm_lx = 0.0;
arm_ly = 0.0;
arm_lz = 0.0;
arm_ax = -msg->axes[arm_ax_axis_]*arm_angular_scale_;
arm_ay = msg->axes[arm_ay_axis_]*arm_angular_scale_;
arm_az = msg->axes[arm_az_axis_]*arm_angular_scale_;
}
controller_.setArmTwist(arm_lx, arm_ly, arm_lz, arm_ax, arm_ay, arm_az);
}
else if (deadman_head_)
{
// Stop moving base
vx = 0.0;
vw = 0.0;
// Get head
pan = msg->axes[head_pan_axis_]*head_pan_step_;
tilt = msg->axes[head_tilt_axis_]*head_tilt_step_;
// Stop moving torso
torso = 0.0;
// Stop moving arm
controller_.disableArm();
}
else
{
// Get base velocities
vx = msg->axes[base_vx_axis_] * base_max_vel_x_;
vw = msg->axes[base_vw_axis_] * base_max_vel_w_;
// Limit velocities
vx = fmax(fmin(vx, base_max_vel_x_), -base_max_vel_x_);
vw = fmax(fmin(vw, base_max_vel_w_), -base_max_vel_w_);
// Stop moving head
pan = 0.0;
tilt = 0.0;
// Get torso
if (msg->buttons[torso_down_button_])
torso = -torso_step_;
else if (msg->buttons[torso_up_button_])
torso = torso_step_;
else
torso = 0.0;
// Get Gripper
if (msg->buttons[gripper_open_button_])
controller_.openGripper();
else if (msg->buttons[gripper_close_button_])
controller_.closeGripper();
// Stop moving arm
controller_.disableArm();
}
}
void psi_rtree_split(psi_rtree_node* newnodes[2], psi_rtree_data* entries) {
psi_int lohi, splitind, group, i, j, c;
psi_real mtmp, atmp[2];
psi_real sortdata[NODE_MAX+1];
psi_int sortinds[NODE_MAX+1];
psi_rvec splitboxes[2][2];
// for keeping track of the best possible split
psi_int axis, best_axis;
psi_real margin, overlap, area, best_margin, best_overlap, best_area;
psi_int best_sorts[PSI_NDIM][NODE_MAX+1];
psi_int best_split_inds[PSI_NDIM];
psi_rtree_sort_helper_data = sortdata;
best_margin = 1.0e30;
best_axis = -1;
for(axis = 0; axis < PSI_NDIM; ++axis) {
margin = 0.0;
best_overlap = 1.0e30;
best_area = 1.0e30;
for(lohi = 0; lohi < 1; ++ lohi) {
// sort by the low or hi box bounds
for(c = 0; c < NODE_MAX+1; ++c) {
sortdata[c] = entries[c].rbox[lohi].xyz[axis];
sortinds[c] = c;
}
qsort(sortinds, NODE_MAX+1, sizeof(psi_int), psi_rtree_sort_helper);
// divide into several distributions based on the sort
// splitind determines the split within the sorted group
for(splitind = NODE_MIN; splitind < (NODE_MAX-NODE_MIN+1); ++splitind) {
// get the two bounding boxes for this split
for(group = 0; group < 2; ++group)
for(i = 0; i < PSI_NDIM; ++i) {
splitboxes[group][0].xyz[i] = 1.0e30;
splitboxes[group][1].xyz[i] = -1.0e30;
}
for(c = 0; c < NODE_MAX+1; ++c) {
group = (c >= splitind);
for(i = 0; i < PSI_NDIM; ++i) {
if(entries[sortinds[c]].rbox[0].xyz[i] < splitboxes[group][0].xyz[i])
splitboxes[group][0].xyz[i] = entries[sortinds[c]].rbox[0].xyz[i];
if(entries[sortinds[c]].rbox[1].xyz[i] > splitboxes[group][1].xyz[i])
splitboxes[group][1].xyz[i] = entries[sortinds[c]].rbox[1].xyz[i];
}
}
// sum the margin-values for each box
// TODO: This is wrong for dimensions higher than 3
for(group = 0; group < 2; ++group)
for(i = 0; i < PSI_NDIM; ++i) {
mtmp = 1.0;
for(j = 0; j < PSI_NDIM-1; ++j)
mtmp *= splitboxes[group][1].xyz[(i+j)%PSI_NDIM]-splitboxes[group][0].xyz[(i+j)%PSI_NDIM];
margin += 2.0*mtmp;
}
// get the area value
for(group = 0; group < 2; ++group) {
atmp[group] = 1.0;
for(i = 0; i < PSI_NDIM; ++i)
atmp[group] *= splitboxes[group][1].xyz[i]-splitboxes[group][0].xyz[i];
}
area = atmp[0] + atmp[1];
// get the overlap between the two boxes
overlap = 1.0;
for(i = 0; i < PSI_NDIM; ++i) {
if(splitboxes[0][0].xyz[i] > splitboxes[1][1].xyz[i] ||
splitboxes[0][1].xyz[i] < splitboxes[1][0].xyz[i]) {
overlap = 0.0;
break;
}
overlap *= fmin(splitboxes[0][1].xyz[i], splitboxes[1][1].xyz[i])
- fmax(splitboxes[0][0].xyz[i], splitboxes[1][0].xyz[i]);
}
if(overlap < best_overlap || (overlap == best_overlap && area < best_area)) {
best_overlap = overlap;
best_area = area;
memcpy(best_sorts[axis], sortinds, (NODE_MAX+1)*sizeof(psi_int));
best_split_inds[axis] = splitind;
}
}
}
if(margin < best_margin) {
best_margin = margin;
best_axis = axis;
}
}
// now, we have information on the best split in hand
// use the indices we saved to redistribute the entries
newnodes[0]->nchildren = 0;
newnodes[1]->nchildren = 0;
for(c = 0; c < NODE_MAX+1; ++c) {
group = (c >= best_split_inds[best_axis]);
newnodes[group]->children[newnodes[group]->nchildren++] = entries[best_sorts[best_axis][c]];
}
}
void compute_tangents_C(double *tangents, double *y, double *energies,
int n_dofs_image, int n_images
) {
/* Compute the tangents for every degree of freedom of a NEBM band,
* following the rules from Henkelman and Jonsson [Henkelman et al.,
* Journal of Chemical Physics 113, 22 (2000)]
*
* We assume we have n_images images in the band, which is represented by
* the *y array. the *y array has the structure
* y = [ theta0_0 phi0_0 theta0_1, phi0_1 ... phi0_(n_dofs_images),
* theta1_0 phi1_0 theta1_1, phi1_1 ... phi1_(n_dofs_images),
* ...
* theta(n_images -1)_0 phi(n_images -1)_0 ... phi(n_images - 1)_(n_dofs_images)]
* where (theta(i)_j, phi(i)_j) are the spherical coordinates of the j-th
* spin in the i-th image of the band. Thus we have n_dofs_image spins per
* image. Images at the extremes, i=0,(n_images-1), are kept fixed, thus we
* do not compute the tangents for them.
*
* The *energies array contains the energy of every image, thus its length
* is n_images.
*
* The tangents have the same length of an image, thus the *tangents array
* has length (n_images * n_dofs_image), just as the *y array, and we keep
* the first and last n_dofs_image components as zero (they correspond to
* the extreme images)
*
* To compute the tangents, we use the t+ and t- vectors. Denoting the i-th
* image by Y_i, these vectors are defined by
* t+_i = Y_(i+1) - Y_i t-_i = Y_i - Y_(i-1)
* Then, when an image is at a saddle point, we have, using the energies
* of the neighbouring images,:
* __
* t_i = | t+_i if E_(i+1) > E_i > E_(i-1)
* <
* |_ t-_i if E_(i+1) < E_i < E_(i-1)
* Otherwise, if we have a maximum or a minimum (E_(i+1) < E_i > E_(i-1)
* or E_(i+1) > E_i < E_(i-1) respectively), we use an average of the
* vectors:
*
__
* t_i = | t+_i * dE_max + t-_i * dE_min if E_(i+1) > E_(i-1)
* <
* |_ t+_i * dE_min + t-_i * dE_max if E_(i+1) < E_(i-1)
* where dE_max = max( |E_(i+1) - E_i|, |E_i - E_(i-1)|) and
* dE_min = min( |E_(i+1) - E_i|, |E_i - E_(i-1)|)
*
*/
int i, j;
// Index where the components of an image start in the *y array,
int im_idx;
// And also the previous and next images:
int next_im_idx, prev_im_idx;
double *t_plus;
double *t_minus;
t_plus = malloc(n_dofs_image * sizeof(double));
t_minus = malloc(n_dofs_image * sizeof(double));
double deltaE_plus, deltaE_minus;
double deltaE_MAX, deltaE_MIN;
for(i = 1; i < n_images - 1; i++){
im_idx = i * (n_dofs_image);
next_im_idx = (i + 1) * (n_dofs_image);
prev_im_idx = (i - 1) * (n_dofs_image);
// Tangents of the i-th image
double * t = &tangents[im_idx];
// Compute the t+ and t- vectors for the i-th image of the band, which
// is given by the difference of the Y_i image with its neighbours
for(j = 0; j < n_dofs_image; j++){
t_plus[j] = y[next_im_idx + j] - y[im_idx + j];
t_minus[j] = y[im_idx + j] - y[prev_im_idx + j];
}
// Similarly, compute the energy differences
deltaE_plus = energies[i + 1] - energies[i];
deltaE_minus = energies[i] - energies[i - 1];
/* Now we follow Henkelman and Jonsson rules [Henkelman et al., Journal
* of Chemical Physics 113, 22 (2000)] for the tangent directions
* (remember that there is a tangent for every spin (degree of
* freedom))
*
* The first two cases are straightforward: If the energy has a
* positive (negative) slope, just make the difference between the spin
* directions with respect to the right (left) image.
*
* The other case is when the image is an energy maximum, or minimum
*/
/* ----------------------------------------------------------------- */
if(deltaE_plus > 0 && deltaE_minus > 0) {
for(j = 0; j < n_dofs_image; j++) t[j] = t_plus[j];
}
/* ----------------------------------------------------------------- */
else if(deltaE_plus < 0 && deltaE_minus < 0) {
for(j = 0; j < n_dofs_image; j++) t[j] = t_minus[j];
}
/* ----------------------------------------------------------------- */
else {
/* According to the energy of the neighbouring images, the tangent
* of the i-th image will be a combination of the differences wrt
* to the left and right images components weighted according to
* the neighbours energies
*/
deltaE_MAX = fmax(fabs(deltaE_plus), fabs(deltaE_minus));
deltaE_MIN = fmin(fabs(deltaE_plus), fabs(deltaE_minus));
if (energies[i + 1] > energies[i - 1]) {
for(j = 0; j < n_dofs_image; j++) {
t[j] = t_plus[j] * deltaE_MAX + t_minus[j] * deltaE_MIN;
}
}
else {
for(j = 0; j < n_dofs_image; j++) {
t[j] = t_plus[j] * deltaE_MIN + t_minus[j] * deltaE_MAX;
}
}
}
/* ----------------------------------------------------------------- */
} // Close loop in images
free(t_plus);
free(t_minus);
} // Close main function
struct flow_interpolation_line_contributions *
flow_interpolation_line_contributions_create(flow_c * context, const uint32_t output_line_size,
const uint32_t input_line_size,
const struct flow_interpolation_details * details)
{
const double sharpen_ratio = flow_interpolation_details_percent_negative_weight(details);
const double desired_sharpen_ratio = details->sharpen_percent_goal / 100.0;
const double scale_factor = (double)output_line_size / (double)input_line_size;
const double downscale_factor = fmin(1.0, scale_factor);
const double half_source_window = (details->window + 0.5) / downscale_factor;
const uint32_t allocated_window_size = (int)ceil(2 * (half_source_window - TONY)) + 1;
uint32_t u, ix;
struct flow_interpolation_line_contributions * res
= LineContributions_alloc(context, output_line_size, allocated_window_size);
if (res == NULL) {
FLOW_add_to_callstack(context);
return NULL;
}
double negative_area = 0;
double positive_area = 0;
for (u = 0; u < output_line_size; u++) {
const double center_src_pixel = ((double)u + 0.5) / scale_factor - 0.5;
const int left_edge = (int)floor(center_src_pixel) - ((allocated_window_size - 1) / 2);
const int right_edge = left_edge + allocated_window_size - 1;
const uint32_t left_src_pixel = (uint32_t)int_max(0, left_edge);
const uint32_t right_src_pixel = (uint32_t)int_min(right_edge, (int)input_line_size - 1);
double total_weight = 0.0;
double total_negative_weight = 0.0;
const uint32_t source_pixel_count = right_src_pixel - left_src_pixel + 1;
if (source_pixel_count > allocated_window_size) {
flow_interpolation_line_contributions_destroy(context, res);
FLOW_error(context, flow_status_Invalid_internal_state);
return NULL;
}
res->ContribRow[u].Left = left_src_pixel;
res->ContribRow[u].Right = right_src_pixel;
float * weights = res->ContribRow[u].Weights;
// commented: additional weight for edges (doesn't seem to be too effective)
// for (ix = left_edge; ix <= right_edge; ix++) {
for (ix = left_src_pixel; ix <= right_src_pixel; ix++) {
int tx = ix - left_src_pixel;
// int tx = min(max(ix, left_src_pixel), right_src_pixel) - left_src_pixel;
double add = (*details->filter)(details, downscale_factor *((double)ix - center_src_pixel));
if (fabs(add) <= 0.00000002){
add = 0.0;
// Weights below a certain threshold make consistent x-plat
// integration test results impossible. pos/neg zero, etc.
// They should be rounded down to zero at the threshold at which results are consistent.
}
weights[tx] = (float)add;
total_weight += add;
total_negative_weight -= fmin(0,add);
}
float neg_factor, pos_factor;
if (total_weight <= 0 || desired_sharpen_ratio > sharpen_ratio){
float total_positive_weight = total_weight + total_negative_weight;
float target_negative_weight = desired_sharpen_ratio * total_positive_weight;
pos_factor = 1;
neg_factor = target_negative_weight / total_negative_weight;
}else{
neg_factor = pos_factor = (float)(1.0f / total_weight);
}
for (ix = 0; ix < source_pixel_count; ix++) {
if (weights[ix] < 0) {
weights[ix] *= neg_factor;
negative_area -= weights[ix];
} else {
weights[ix] *= pos_factor;
positive_area += weights[ix];
}
}
//Shrink to improve perf & result consistency
int32_t iix;
//Shrink region from the right
for (iix = source_pixel_count - 1; iix >= 0; iix--){
if (weights[iix] != 0) break;
res->ContribRow[u].Right--;
}
//Shrink region from the left
for (iix = 0; iix < (int32_t)source_pixel_count; iix++){
if (weights[0] != 0) break;
res->ContribRow[u].Weights++;
weights++;
res->ContribRow[u].Left++;
}
}
res->percent_negative = negative_area / positive_area;
return res;
}
void heterozygote (unsigned r, unsigned c, COUNTTYPE * R)
{
if(tableCount < 0) return; // aborted because of time limit
COUNTTYPE *res, *resn;
int lower, upper;
unsigned ntables;
unsigned i, arc, ar1, ar2, a32, a31;
COUNTTYPE * Rnew = R + nAlleles;
double countsSoFar;
unsigned long long hash;
// NSNumber * n;
res = R-1; // to make res a 1-based version of R
resn = Rnew-1; // so resn is 1-based for Rnew
lower = res[r];
for (i = 1; i < c; i++) lower -= res[i];
lower = fmax(0, lower);
upper = fmin(res[r], res[c]);
if(c > 2) for (arc = lower; arc <= upper; arc++) {
memcpy(Rnew, R, Rbytes); // Put a fresh set of residuals from R into Rnew
// decrement residuals for the current value of arc.
resn[r] -= arc;
resn[c] -= arc;
heterozygote(r, c-1, Rnew);
}
if(c==2){
if(r > 3) for (ar2= lower; ar2 <= upper; ar2++) {
memcpy(Rnew, R, Rbytes); // Put a fresh set of residuals from R into Rnew
// decrement residuals for the current value of arc.
resn[r] -= ar2;
resn[c] -= ar2;
// The value of ar1 is now fixed, so no need for any more calls to heterozygote in this row
ar1 = fmin(resn[r], resn[1]);
resn[1] -= ar1;
resn[r] -= ar1;
// Before calling homozygote, see if we have visited this node before by comparing its hash tag.
hash = makeHash(r-1, Rnew);
i = 0;
// Search list of old nodes
while (hash != nodez[i].hash && i < nextNode) i++;
if(i < nextNode) {
// old node was found, no need to go any further.
tableCount += nodez[i].count;
} else {
// new node
countsSoFar = tableCount;
homozygote(r-1, Rnew);
if (nextNode < MAXNODE) {
// Make a new node
nodez[i].hash = hash;
nodez[i].count = tableCount - countsSoFar;
nextNode++;
}
} // new node
}
if(r==3) // and c = 2, then we can handle a series of two-allele cases with no deeper recursion
{
for(a32 = lower; a32 <= upper; a32++) {
a31 = fmin(res[1], res[3]-a32); //Value of a31 is now fixed for each a32
ntables = (fmin(res[1] - a31, res[2]-a32))/2 + 1;
tableCount += ntables;
}
} // if r == 3
} // if c == 2
} // heterozygote
/**
* Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
* linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
*/
double ratioToScore(double ratio)
{
return (fmax(0, fmin(100*(1-fabs(1-ratio)), 100)));
}
void mcmc(tree *tr, analdef *adef)
{
int i=0;
tr->startLH = tr->likelihood;
printBothOpen("start minimalistic search with LH %f\n", tr->likelihood);
printBothOpen("tr LH %f, startLH %f\n", tr->likelihood, tr->startLH);
int insert_id;
int j;
int maxradius = 30;
int accepted_spr = 0, accepted_nni = 0, accepted_bl = 0, accepted_model = 0, accepted_gamma = 0, inserts = 0;
int rejected_spr = 0, rejected_nni = 0, rejected_bl = 0, rejected_model = 0, rejected_gamma = 0;
int num_moves = 10000;
boolean proposalAccepted;
boolean proposalSuccess;
prop which_proposal;
double testr;
double acceptance;
srand (440);
double totalTime = 0.0, proposalTime = 0.0, blTime = 0.0, printTime = 0.0;
double t_start = gettime();
double t;
//allocate states
double bl_prior_exp_lambda = 0.1;
double bl_sliding_window_w = 0.005;
double gm_sliding_window_w = 0.75;
double rt_sliding_window_w = 0.5;
state *curstate = state_init(tr, adef, maxradius, bl_sliding_window_w, rt_sliding_window_w, gm_sliding_window_w, bl_prior_exp_lambda);
printStateFileHeader(curstate);
set_start_bl(curstate);
printf("start bl_prior: %f\n",curstate->bl_prior);
set_start_prior(curstate);
curstate->hastings = 1;//needs to be set by the proposal when necessary
/* Set the starting LH with a full traversal */
evaluateGeneric(tr, tr->start, TRUE);
tr->startLH = tr->likelihood;
printBothOpen("Starting with tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
/* Set reasonable model parameters */
evaluateGeneric(curstate->tr, curstate->tr->start, FALSE); // just for validation
printBothOpen("tr LH before modOpt %f\n",curstate->tr->likelihood);
printSubsRates(curstate->tr, curstate->model, curstate->numSubsRates);
/* optimize the model with Brents method for reasonable starting points */
modOpt(curstate->tr, curstate->adef, 5.0); /* not by proposal, just using std raxml machinery... */
evaluateGeneric(curstate->tr, curstate->tr->start, FALSE); // just for validation
printBothOpen("tr LH after modOpt %f\n",curstate->tr->likelihood);
printSubsRates(curstate->tr, curstate->model, curstate->numSubsRates);
recordSubsRates(curstate->tr, curstate->model, curstate->numSubsRates, curstate->curSubsRates);
int first = 1;
/* beginning of the MCMC chain */
for(j=0; j<num_moves; j++)
{
//printBothOpen("iter %d, tr LH %f, startLH %f\n",j, tr->likelihood, tr->startLH);
//printRecomTree(tr, TRUE, "startiter");
proposalAccepted = FALSE;
t = gettime();
/*
evaluateGeneric(tr, tr->start); // just for validation
printBothOpen("before proposal, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
*/
which_proposal = proposal(curstate);
if (first == 1)
{
first = 0;
curstate->curprior = curstate->newprior;
}
//printBothOpen("proposal done, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
assert(which_proposal == SPR || which_proposal == stNNI ||
which_proposal == UPDATE_ALL_BL ||
which_proposal == UPDATE_MODEL || which_proposal == UPDATE_GAMMA);
proposalTime += gettime() - t;
/* decide upon acceptance */
testr = (double)rand()/(double)RAND_MAX;
//should look something like
acceptance = fmin(1,(curstate->hastings) *
(exp(curstate->newprior-curstate->curprior)) * (exp(curstate->tr->likelihood-curstate->tr->startLH)));
/*
//printRecomTree(tr, FALSE, "after proposal");
printBothOpen("after proposal, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
*/
if(testr < acceptance)
{
proposalAccepted = TRUE;
switch(which_proposal)
{
case SPR:
//printRecomTree(tr, TRUE, "after accepted");
// printBothOpen("SPR new topology , iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
accepted_spr++;
break;
case stNNI:
printBothOpen("NNI new topology , iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
accepted_nni++;
break;
case UPDATE_ALL_BL:
// printBothOpen("BL new , iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
accepted_bl++;
break;
case UPDATE_MODEL:
// printBothOpen("Model new, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
accepted_model++;
break;
case UPDATE_GAMMA:
// printBothOpen("Gamma new, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
accepted_gamma++;
break;
default:
assert(0);
}
curstate->tr->startLH = curstate->tr->likelihood; //new LH
curstate->curprior = curstate->newprior;
}
else
{
//printBothOpen("rejected , iter %d tr LH %f, startLH %f, %i \n", j, tr->likelihood, tr->startLH, which_proposal);
resetState(which_proposal,curstate);
switch(which_proposal)
{
case SPR:
rejected_spr++;
break;
case stNNI:
rejected_nni++;
break;
case UPDATE_ALL_BL:
rejected_bl++;
break;
case UPDATE_MODEL:
rejected_model++;
break;
case UPDATE_GAMMA:
rejected_gamma++;
break;
default:
assert(0);
}
evaluateGeneric(tr, tr->start, FALSE);
// just for validation
if(fabs(curstate->tr->startLH - tr->likelihood) > 1.0E-10)
{
printBothOpen("WARNING: LH diff %.10f\n", curstate->tr->startLH - tr->likelihood);
}
//printRecomTree(tr, TRUE, "after reset");
//printBothOpen("after reset, iter %d tr LH %f, startLH %f\n", j, tr->likelihood, tr->startLH);
assert(fabs(curstate->tr->startLH - tr->likelihood) < 1.0E-10);
}
inserts++;
/* need to print status */
if (j % 50 == 0)
{
t = gettime();
printBothOpen("sampled at iter %d, tr LH %f, startLH %f, prior %f, incr %f\n",j, tr->likelihood, tr->startLH, curstate->curprior, tr->likelihood - tr->startLH);
boolean printBranchLengths = TRUE;
/*printSimpleTree(tr, printBranchLengths, adef);*/
//TODO: print some parameters to a file
printStateFile(j,curstate);
printTime += gettime() - t;
}
}
t = gettime();
treeEvaluate(tr, 1);
blTime += gettime() - t;
printBothOpen("accepted SPR %d, accepted stNNI %d, accepted BL %d, accepted model %d, accepted gamma %d, num moves tried %d, SPRs with max radius %d\n",
accepted_spr, accepted_nni, accepted_bl, accepted_model, accepted_gamma, num_moves, maxradius);
printBothOpen("rejected SPR %d, rejected stNNI %d, rejected BL %d, rejected model %d, rejected gamma %d\n",
rejected_spr, rejected_nni, rejected_bl, rejected_model, rejected_gamma);
printBothOpen("ratio SPR %f, ratio stNNI %f, ratio BL %f, ratio model %f, ratio gamma %f\n",
accepted_spr/(double)(rejected_spr+accepted_spr), accepted_nni/(double)(rejected_nni+accepted_nni), accepted_bl/(double)(rejected_bl+accepted_bl),
accepted_model/(double)(rejected_model+accepted_model), accepted_gamma/(double)(rejected_gamma+accepted_gamma));
printBothOpen("total %f, BL %f, printing %f, proposal %f\n", gettime()- t_start, blTime, printTime, proposalTime);
assert(inserts == num_moves);
state_free(curstate);
}
int
DjVuPalette::compute_palette(int maxcolors, int minboxsize)
{
if (!hist)
G_THROW( ERR_MSG("DjVuPalette.no_color") );
if (maxcolors<1 || maxcolors>MAXPALETTESIZE)
G_THROW( ERR_MSG("DjVuPalette.many_colors") );
// Paul Heckbert: "Color Image Quantization for Frame Buffer Display",
// SIGGRAPH '82 Proceedings, page 297. (also in ppmquant)
// Collect histogram colors
int sum = 0;
int ncolors = 0;
GTArray<PData> pdata;
{ // extra nesting for windows
for (GPosition p = *hist; p; ++p)
{
pdata.touch(ncolors);
PData &data = pdata[ncolors++];
int k = hist->key(p);
data.p[0] = (k>>16) & 0xff;
data.p[1] = (k>>8) & 0xff;
data.p[2] = (k) & 0xff;
data.w = (*hist)[p];
sum += data.w;
}
}
// Create first box
GList<PBox> boxes;
PBox newbox;
newbox.data = pdata;
newbox.colors = ncolors;
newbox.boxsize = 256;
newbox.sum = sum;
boxes.append(newbox);
// Repeat spliting boxes
while (boxes.size() < maxcolors)
{
// Find suitable box
GPosition p;
for (p=boxes; p; ++p)
if (boxes[p].colors>=2 && boxes[p].boxsize>minboxsize)
break;
if (! p)
break;
// Find box boundaries
PBox &splitbox = boxes[p];
unsigned char pmax[3];
unsigned char pmin[3];
pmax[0] = pmin[0] = splitbox.data->p[0];
pmax[1] = pmin[1] = splitbox.data->p[1];
pmax[2] = pmin[2] = splitbox.data->p[2];
{ // extra nesting for windows
for (int j=1; j<splitbox.colors; j++)
{
pmax[0] = umax(pmax[0], splitbox.data[j].p[0]);
pmax[1] = umax(pmax[1], splitbox.data[j].p[1]);
pmax[2] = umax(pmax[2], splitbox.data[j].p[2]);
pmin[0] = umin(pmin[0], splitbox.data[j].p[0]);
pmin[1] = umin(pmin[1], splitbox.data[j].p[1]);
pmin[2] = umin(pmin[2], splitbox.data[j].p[2]);
}
}
// Determine split direction and sort
int bl = pmax[0]-pmin[0];
int gl = pmax[1]-pmin[1];
int rl = pmax[2]-pmin[2];
splitbox.boxsize = (bl>gl ? (rl>bl ? rl : bl) : (rl>gl ? rl : gl));
if (splitbox.boxsize <= minboxsize)
continue;
if (gl == splitbox.boxsize)
qsort(splitbox.data, splitbox.colors, sizeof(PData), gcomp);
else if (rl == splitbox.boxsize)
qsort(splitbox.data, splitbox.colors, sizeof(PData), rcomp);
else
qsort(splitbox.data, splitbox.colors, sizeof(PData), bcomp);
// Find median
int lowercolors = 0;
int lowersum = 0;
while (lowercolors<splitbox.colors-1 && lowersum+lowersum<splitbox.sum)
lowersum += splitbox.data[lowercolors++].w;
// Compute new boxes
newbox.data = splitbox.data + lowercolors;
newbox.colors = splitbox.colors - lowercolors;
newbox.sum = splitbox.sum - lowersum;
splitbox.colors = lowercolors;
splitbox.sum = lowersum;
// Insert boxes at proper location
GPosition q;
for (q=p; q; ++q)
if (boxes[q].sum < newbox.sum)
break;
boxes.insert_before(q, newbox);
for (q=p; q; ++q)
if (boxes[q].sum < splitbox.sum)
break;
boxes.insert_before(q, boxes, p);
}
// Fill palette array
ncolors = 0;
palette.empty();
palette.resize(0,boxes.size()-1);
{ // extra nesting for windows
for (GPosition p=boxes; p; ++p)
{
PBox &box = boxes[p];
// Compute box representative color
float bsum = 0;
float gsum = 0;
float rsum = 0;
for (int j=0; j<box.colors; j++)
{
float w = (float)box.data[j].w;
bsum += box.data[j].p[0] * w;
gsum += box.data[j].p[1] * w;
rsum += box.data[j].p[2] * w;
}
PColor &color = palette[ncolors++];
color.p[0] = (unsigned char) fmin(255, bsum/box.sum);
color.p[1] = (unsigned char) fmin(255, gsum/box.sum);
color.p[2] = (unsigned char) fmin(255, rsum/box.sum);
color.p[3] = ( color.p[0]*BMUL + color.p[1]*GMUL + color.p[2]*RMUL) / SMUL;
}
}
// Save dominant color
PColor dcolor = palette[0];
// Sort palette colors in luminance order
qsort((PColor*)palette, ncolors, sizeof(PColor), lcomp);
// Clear invalid data
colordata.empty();
delete pmap;
pmap = 0;
// Return dominant color
return color_to_index_slow(dcolor.p);
}
/** Compare two REAL4 vectors using various different comparison metrics
*/
int
XLALCompareREAL4Vectors ( VectorComparison *result, ///< [out] return comparison results
const REAL4Vector *x, ///< [in] first input vector
const REAL4Vector *y, ///< [in] second input vector
const VectorComparison *tol ///< [in] accepted tolerances on comparisons, or NULL for no check
)
{
XLAL_CHECK ( result != NULL, XLAL_EINVAL );
XLAL_CHECK ( x != NULL, XLAL_EINVAL );
XLAL_CHECK ( y != NULL, XLAL_EINVAL );
XLAL_CHECK ( x->data != NULL, XLAL_EINVAL );
XLAL_CHECK ( y->data != NULL, XLAL_EINVAL );
XLAL_CHECK ( x->length > 0, XLAL_EINVAL );
XLAL_CHECK ( x->length == y->length, XLAL_EINVAL );
REAL8 x_L1 = 0, x_L2 = 0;
REAL8 y_L1 = 0, y_L2 = 0;
REAL8 diff_L1 = 0, diff_L2 = 0;
REAL8 scalar = 0;
REAL4 maxAbsx = 0, maxAbsy = 0;
REAL4 x_atMaxAbsx = 0, y_atMaxAbsx = 0;
REAL4 x_atMaxAbsy = 0, y_atMaxAbsy = 0;
UINT4 numSamples = x->length;
for ( UINT4 i = 0; i < numSamples; i ++ )
{
REAL4 x_i = x->data[i];
REAL4 y_i = y->data[i];
XLAL_CHECK ( isfinite ( x_i ), XLAL_EFPINVAL, "non-finite element: x(%d) = %g\n", i, x_i );
XLAL_CHECK ( isfinite ( y_i ), XLAL_EFPINVAL, "non-finite element: y(%d) = %g\n", i, y_i );
REAL4 xAbs_i = fabs ( x_i );
REAL4 yAbs_i = fabs ( y_i );
REAL8 absdiff = fabs ( x_i - y_i );
diff_L1 += absdiff;
diff_L2 += SQ(absdiff);
x_L1 += xAbs_i;
y_L1 += yAbs_i;
x_L2 += SQ(xAbs_i);
y_L2 += SQ(yAbs_i);
scalar += x_i * y_i;
if ( xAbs_i > maxAbsx ) {
maxAbsx = xAbs_i;
x_atMaxAbsx = x_i;
y_atMaxAbsx = y_i;
}
if ( yAbs_i > maxAbsy ) {
maxAbsy = yAbs_i;
x_atMaxAbsy = x_i;
y_atMaxAbsy = y_i;
}
} // for i < numSamples
// complete L2 norms by taking sqrt
x_L2 = sqrt ( x_L2 );
y_L2 = sqrt ( y_L2 );
diff_L2 = sqrt ( diff_L2 );
// compute and return comparison results
result->relErr_L1 = diff_L1 / ( 0.5 * (x_L1 + y_L1 ) );
result->relErr_L2 = diff_L2 / ( 0.5 * (x_L2 + y_L2 ) );
REAL8 cosTheta = fmin ( 1, scalar / (x_L2 * y_L2) );
result->angleV = acos ( cosTheta );
result->relErr_atMaxAbsx = fRELERR ( x_atMaxAbsx, y_atMaxAbsx );
result->relErr_atMaxAbsy = fRELERR ( x_atMaxAbsy, y_atMaxAbsy );;
XLAL_CHECK ( XLALCheckVectorComparisonTolerances ( result, tol ) == XLAL_SUCCESS, XLAL_EFUNC );
return XLAL_SUCCESS;
} // XLALCompareREAL4Vectors()
void character_addStatus(character_t* c, int s, float q)
{
float max = character_maxOfStatus(c, s);
float n = c->statuses[s] + q;
c->statuses[s] = fmin(fmax(n, 0), max);
}
float fminf (float x, float y)
{
return (float) fmin( (double)x, (double)y );
}
extern float4 __attribute__((overloadable)) min(float4 v1, float v2) {
return fmin(v1, v2);
}