// Find the interval in pBWT corresponding to w
// using a cache of short k-mer intervals to avoid
// some of the iterations
BWTInterval BWTAlgorithms::findIntervalWithCache(const BWT* pBWT, const BWTIntervalCache* pIntervalCache, const std::string& w)
{
size_t cacheLen = pIntervalCache->getCachedLength();
if(w.size() < cacheLen)
return findInterval(pBWT, w);
// Compute the interval using the cache for the last k bases
int len = w.size();
int j = len - cacheLen;
// Check whether the input string has a '$' in it.
// We don't cache these strings so if it does
// we have to do a direct lookup
if(index(w.c_str() + j, '$') != NULL)
return findInterval(pBWT, w);
BWTInterval interval = pIntervalCache->lookup(w.c_str() + j);
j -= 1;
for(;j >= 0; --j)
{
char curr = w[j];
updateInterval(interval, curr, pBWT);
if(!interval.isValid())
return interval;
}
return interval;
}
// Count the number of occurrences of string w, including the reverse complement
size_t BWTAlgorithms::countSequenceOccurrences(const std::string& w, const BWT* pBWT)
{
BWTInterval fwd_interval = findInterval(pBWT, w);
BWTInterval rc_interval = findInterval(pBWT, reverseComplement(w));
size_t count = 0;
if(fwd_interval.isValid())
count += fwd_interval.size();
if(rc_interval.isValid())
count += rc_interval.size();
return count;
}
/* generates the LZ parsing of filename and returns the number of phrases
* creates 3 different files (arrays):
* filename.start (4*len bytes)
* filename.len (4*len bytes)
* filename.char (1*len bytes) */
unsigned int LZ77::parse(){
/*report(1,2,(unsigned char)'a');
std::cout<<this->filename<<std::endl;
this->close();
return 1;*/
//startTime();
unsigned int factor_pos=0;
unsigned int factor_len;
unsigned int source_start;
unsigned int start,end, min;
while(factor_pos<this->text_len){
factor_len = 0;
start = 1;
end = 0;
source_start = 0;
do{
findInterval(&start, &end, this->text[factor_pos+factor_len],factor_len);
factor_len++;
min = sa[rmq->minpos(start,end)];
if(start>end || min>=factor_pos || min+factor_len>factor_pos)break;//revisar la ultima condicion: es mayor o mayor o igual?
source_start = min;
}while(start<=end);
this->report(source_start,factor_len,this->text[factor_pos+factor_len-1],factor_pos);
factor_pos+=factor_len;
}
this->close();
//endTime();
return this->factors;
}
// Delegate the findInterval call based on what indices are loaded
BWTInterval BWTAlgorithms::findInterval(const BWTIndexSet& indices, const std::string& w)
{
if(indices.pCache != NULL)
return findIntervalWithCache(indices.pBWT, indices.pCache, w);
else
return findInterval(indices.pBWT, w);
}
METHODPREFIX
typename SplineCurve<ScalarParam,dimensionParam>::Point
SplineCurve<ScalarParam,dimensionParam>::evaluate(
typename SplineCurve<ScalarParam,dimensionParam>::Scalar u,
typename SplineCurve<ScalarParam,dimensionParam>::EvaluationCache* cache,
typename SplineCurve<ScalarParam,dimensionParam>::Vector& deriv1) const
{
/* Find the knot interval containing the given parameter: */
int iv=findInterval(u);
/* Copy the control points defining the respective curve segment into the evaluation cache: */
const Point* pointBase=&points[iv-degree+1];
for(int i=0;i<=degree;++i)
cache->points[i]=pointBase[i];
/* Perform degree-1 stages of deBoor's algorithm: */
for(int k=0;k<degree-1;++k)
deBoorStage(u,cache,iv,k);
/* Calculate the first derivative: */
deriv1=cache->points[1]-cache->points[0];
deriv1*=Scalar(degree)/(knots[iv+1]-knots[iv]);
/* Perform the last stage of deBoor's algorithm: */
deBoorStage(u,cache,iv,degree-1);
/* Return the result point: */
return cache->points[0];
}
double
SplineInterpolation::sample2ndDerivative(double x) const
{
unsigned int klo, khi;
findInterval(x, klo, khi);
double h, a, b;
computeCoeffs(klo, khi, x, h, a, b);
return a * _y2[klo] + b * _y2[khi];
}
double
SplineInterpolation::sampleDerivative(double x) const
{
unsigned int klo, khi;
findInterval(x, klo, khi);
double h, a, b;
computeCoeffs(klo, khi, x, h, a, b);
return (_y[khi] - _y[klo]) / h - (((3.0 * a*a - 1.0) * _y2[klo] + (3.0 * b*b - 1.0) * -_y2[khi]) * h / 6.0);
}
Real
SplineInterpolationBase::sample(const std::vector<Real> & x, const std::vector<Real> & y, const std::vector<Real> & y2, Real x_int) const
{
unsigned int klo, khi;
findInterval(x, x_int, klo, khi);
Real h, a, b;
computeCoeffs(x, klo, khi, x_int, h, a, b);
return a * y[klo] + b * y[khi] + ((a*a*a - a) * y2[klo] + (b*b*b - b) * y2[khi]) * (h*h) / 6.0;
}
Real
SplineInterpolationBase::sampleDerivative(const std::vector<Real> & x, const std::vector<Real> & y, const std::vector<Real> & y2, Real x_int) const
{
unsigned int klo, khi;
findInterval(x, x_int, klo, khi);
Real h, a, b;
computeCoeffs(x, klo, khi, x_int, h, a, b);
return (y[khi] - y[klo]) / h - (((3.0 * a*a - 1.0) * y2[klo] + (3.0 * b*b - 1.0) * -y2[khi]) * h / 6.0);
}
Real
SplineInterpolationBase::sample2ndDerivative(const std::vector<Real> & x, const std::vector<Real> & y, const std::vector<Real> & y2, Real x_int) const
{
unsigned int klo, khi;
findInterval(x, x_int, klo, khi);
Real h, a, b;
computeCoeffs(x, klo, khi, x_int, h, a, b);
return a * y2[klo] + b * y2[khi];
}
double
SplineInterpolation::sample(double x) const
{
unsigned int klo, khi;
findInterval(x, klo, khi);
double h, a, b;
computeCoeffs(klo, khi, x, h, a, b);
return a * _y[klo] + b * _y[khi] + ((a*a*a - a) * _y2[klo] + (b*b*b - b) * _y2[khi]) * (h*h) / 6.0;
}
/* This one to be called from R {via .C(..)} :
FIXME: Replace by a .Call()able version!
Done for R 2.15.2, no longer used in R, but used in IBDsim and timeSeries
*/
void find_interv_vec(double *xt, int *n, double *x, int *nx,
int *rightmost_closed, int *all_inside, int *indx)
{
int i, ii, mfl;
ii = 1;
for(i=0; i < *nx; i++) {
mfl = *all_inside;
ii = findInterval(xt, *n, x[i],
*rightmost_closed, *all_inside, ii, &mfl);
indx[i] = ii;
}
}
size_t BWTAlgorithms::countSequenceOccurrencesSingleStrand(const std::string& w, const BWTIndexSet& indices)
{
assert(indices.pBWT != NULL);
//assert(indices.pCache != NULL);
BWTInterval interval;
if(indices.pCache != NULL)
interval = findIntervalWithCache(indices.pBWT, indices.pCache, w);
else
interval = findInterval(indices.pBWT, w);
return interval.isValid() ? interval.size() : 0;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// operator()
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double LinearInterpolator::operator()( double x ) const
{
if ( x <= m_x.front() )
{
return m_y.front();
}
if ( x >= m_x.back() )
{
return m_y.back();
}
size_t iInterval = findInterval( x );
return interpolate( x, m_x[ iInterval ], m_x[ iInterval + 1 ], m_y[ iInterval ], m_y[ iInterval + 1 ] );
}
void addNum(int val) {
int idx = findInterval(val);
if (idx != -1 && val <= intervals[idx].end) //contain val in interval
return;
if (idx != -1 && val - 1 == intervals[idx].end) //set current interval
intervals[idx].end = val;
else if (idx != intervals.size() - 1 && val + 1 == intervals[idx + 1].start) //set next interval
intervals[idx + 1].start = val;
else //add new interval
intervals.insert(intervals.begin() + idx + 1, Interval(val, val));
if (idx != -1 && intervals[idx].end + 1 == intervals[idx + 1].start) { //merge intervals
intervals[idx + 1].start = intervals[idx].start;
intervals.erase(intervals.begin() + idx);
}
}
METHODPREFIX
typename SplineCurve<ScalarParam,dimensionParam>::Point
SplineCurve<ScalarParam,dimensionParam>::evaluate(
typename SplineCurve<ScalarParam,dimensionParam>::Scalar u,
typename SplineCurve<ScalarParam,dimensionParam>::EvaluationCache* cache) const
{
/* Find the knot interval containing the given parameter: */
int iv=findInterval(u);
/* Copy the control points defining the respective curve segment into the evaluation cache: */
const Point* pointBase=&points[iv-degree+1];
for(int i=0;i<=degree;++i)
cache->points[i]=pointBase[i];
/* Perform degree stages of deBoor's algorithm: */
for(int k=0;k<degree;++k)
deBoorStage(u,cache,iv,k);
/* Return the result point: */
return cache->points[0];
}
bool FGTrim::DoTrim(void) {
trim_failed=false;
int i;
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->DisableOutput();
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
fgic->SetPRadpsIC(0.0);
fgic->SetQRadpsIC(0.0);
fgic->SetRRadpsIC(0.0);
//clear the sub iterations counts & zero out the controls
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
if(TrimAxes[current_axis]->GetStateType() == tQdot) {
if(mode == tGround) {
TrimAxes[current_axis]->initTheta();
}
}
xlo=TrimAxes[current_axis]->GetControlMin();
xhi=TrimAxes[current_axis]->GetControlMax();
TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
TrimAxes[current_axis]->Run();
//TrimAxes[current_axis]->AxisReport();
sub_iterations[current_axis]=0;
successful[current_axis]=0;
solution[current_axis]=false;
}
if(mode == tPullup ) {
cout << "Setting pitch rate and nlf... " << endl;
setupPullup();
cout << "pitch rate done ... " << endl;
TrimAxes[0]->SetStateTarget(targetNlf);
cout << "nlf done" << endl;
} else if (mode == tTurn) {
setupTurn();
//TrimAxes[0]->SetStateTarget(targetNlf);
}
do {
axis_count=0;
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
setDebug();
updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits()) {
solution[current_axis]=true;
solve();
}
} else if(findInterval()) {
solve();
} else {
solution[current_axis]=false;
}
sub_iterations[current_axis]+=Nsub;
}
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(Debug > 0) TrimAxes[current_axis]->AxisReport();
if(TrimAxes[current_axis]->InTolerance()) {
axis_count++;
successful[current_axis]++;
}
}
if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
//cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
//At this point we can check the input limits of the failed axis
//and declare the trim failed if there is no sign change. If there
//is, keep going until success or max iteration count
//Oh, well: two out of three ain't bad
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(!TrimAxes[current_axis]->InTolerance()) {
if(!checkLimits()) {
// special case this for now -- if other cases arise proper
// support can be added to FGTrimAxis
if( (gamma_fallback) &&
(TrimAxes[current_axis]->GetStateType() == tUdot) &&
(TrimAxes[current_axis]->GetControlType() == tThrottle)) {
cout << " Can't trim udot with throttle, trying flight"
<< " path angle. (" << N << ")" << endl;
if(TrimAxes[current_axis]->GetState() > 0)
TrimAxes[current_axis]->SetControlToMin();
else
TrimAxes[current_axis]->SetControlToMax();
TrimAxes[current_axis]->Run();
delete TrimAxes[current_axis];
TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
tGamma );
} else {
cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
<< " doesn't appear to be trimmable" << endl;
//total_its=k;
trim_failed=true; //force the trim to fail
} //gamma_fallback
}
} //solution check
} //for loop
} //all-but-one check
N++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
fdmex->SetTrimStatus(false);
fdmex->ResumeIntegration();
fdmex->EnableOutput();
return !trim_failed;
}
/* This is called from stats/src/bvalue.f, and packages gam and mda */
int F77_SUB(interv)(double *xt, int *n, double *x,
Rboolean *rightmost_closed, Rboolean *all_inside,
int *ilo, int *mflag)
{
return findInterval(xt, *n, *x, *rightmost_closed, *all_inside, *ilo, mflag);
}
size_t BWTAlgorithms::countSequenceOccurrencesSingleStrand(const std::string& w, const BWT* pBWT)
{
BWTInterval interval = findInterval(pBWT, w);
return interval.isValid() ? interval.size() : 0;
}
bool FGTrim::DoTrim(void) {
bool trim_failed=false;
unsigned int N = 0;
unsigned int axis_count = 0;
FGFCS *FCS = fdmex->GetFCS();
vector<double> throttle0 = FCS->GetThrottleCmd();
double elevator0 = FCS->GetDeCmd();
double aileron0 = FCS->GetDaCmd();
double rudder0 = FCS->GetDrCmd();
double PitchTrim0 = FCS->GetPitchTrimCmd();
fgic = *fdmex->GetIC();
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
fgic.SetPRadpsIC(0.0);
fgic.SetQRadpsIC(0.0);
fgic.SetRRadpsIC(0.0);
if (mode == tGround) {
trimOnGround();
double theta = fgic.GetThetaRadIC();
double phi = fgic.GetPhiRadIC();
// Take opportunity of the first approx. found by trimOnGround() to
// refine the control limits.
TrimAxes[0].SetControlLimits(0., fgic.GetAltitudeAGLFtIC());
TrimAxes[1].SetControlLimits(theta - 5.0 * degtorad, theta + 5.0 * degtorad);
TrimAxes[2].SetControlLimits(phi - 30.0 * degtorad, phi + 30.0 * degtorad);
}
//clear the sub iterations counts & zero out the controls
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
xlo=TrimAxes[current_axis].GetControlMin();
xhi=TrimAxes[current_axis].GetControlMax();
TrimAxes[current_axis].SetControl((xlo+xhi)/2);
TrimAxes[current_axis].Run();
//TrimAxes[current_axis].AxisReport();
sub_iterations[current_axis]=0;
successful[current_axis]=0;
solution[current_axis]=false;
}
if(mode == tPullup ) {
cout << "Setting pitch rate and nlf... " << endl;
setupPullup();
cout << "pitch rate done ... " << endl;
TrimAxes[0].SetStateTarget(targetNlf);
cout << "nlf done" << endl;
} else if (mode == tTurn) {
setupTurn();
//TrimAxes[0].SetStateTarget(targetNlf);
}
do {
axis_count=0;
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
setDebug(TrimAxes[current_axis]);
updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits(TrimAxes[current_axis])) {
solution[current_axis]=true;
solve(TrimAxes[current_axis]);
}
} else if(findInterval(TrimAxes[current_axis])) {
solve(TrimAxes[current_axis]);
} else {
solution[current_axis]=false;
}
sub_iterations[current_axis]+=Nsub;
}
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(Debug > 0) TrimAxes[current_axis].AxisReport();
if(TrimAxes[current_axis].InTolerance()) {
axis_count++;
successful[current_axis]++;
}
}
if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
//cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
//At this point we can check the input limits of the failed axis
//and declare the trim failed if there is no sign change. If there
//is, keep going until success or max iteration count
//Oh, well: two out of three ain't bad
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(!TrimAxes[current_axis].InTolerance()) {
if(!checkLimits(TrimAxes[current_axis])) {
// special case this for now -- if other cases arise proper
// support can be added to FGTrimAxis
if( (gamma_fallback) &&
(TrimAxes[current_axis].GetStateType() == tUdot) &&
(TrimAxes[current_axis].GetControlType() == tThrottle)) {
cout << " Can't trim udot with throttle, trying flight"
<< " path angle. (" << N << ")" << endl;
if(TrimAxes[current_axis].GetState() > 0)
TrimAxes[current_axis].SetControlToMin();
else
TrimAxes[current_axis].SetControlToMax();
TrimAxes[current_axis].Run();
TrimAxes[current_axis]=FGTrimAxis(fdmex,&fgic,tUdot,tGamma);
} else {
cout << " Sorry, " << TrimAxes[current_axis].GetStateName()
<< " doesn't appear to be trimmable" << endl;
//total_its=k;
trim_failed=true; //force the trim to fail
} //gamma_fallback
}
} //solution check
} //for loop
} //all-but-one check
N++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else { // The trim has failed
total_its=N;
// Restore the aircraft parameters to their initial values
fgic = *fdmex->GetIC();
FCS->SetDeCmd(elevator0);
FCS->SetDaCmd(aileron0);
FCS->SetDrCmd(rudder0);
FCS->SetPitchTrimCmd(PitchTrim0);
for (unsigned int i=0; i < throttle0.size(); i++)
FCS->SetThrottleCmd(i, throttle0[i]);
fdmex->Initialize(&fgic);
fdmex->Run();
// If WOW is true we must make sure there are no gears into the ground.
if (fdmex->GetGroundReactions()->GetWOW())
trimOnGround();
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
fdmex->ResumeIntegration();
fdmex->SetTrimStatus(false);
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
return !trim_failed;
}
int spline_hermite_val ( int ndata, double tdata[], double c[], double tval,
double *sval, double *spval )
//****************************************************************************80
//
// Purpose:
//
// SPLINE_HERMITE_VAL evaluates a piecewise cubic Hermite interpolant.
//
// Discussion:
//
// SPLINE_HERMITE_SET must be called first, to set up the
// spline data from the raw function and derivative data.
//
// In the interval (TDATA(I), TDATA(I+1)), the interpolating
// Hermite polynomial is given by
//
// SVAL(TVAL) = C(1,I)
// + ( TVAL - TDATA(I) ) * ( C(2,I)
// + ( TVAL - TDATA(I) ) * ( C(3,I)
// + ( TVAL - TDATA(I) ) * C(4,I) ) )
//
// and
//
// SVAL'(TVAL) = C(2,I)
// + ( TVAL - TDATA(I) ) * ( 2 * C(3,I)
// + ( TVAL - TDATA(I) ) * 3 * C(4,I) )
//
// This is algorithm PCUBIC from Conte and deBoor.
//
// Modified:
//
// 24 February 2004
//
// Author:
//
// John Burkardt
//
// Reference:
//
// Samuel Conte, Carl deBoor,
// Elementary Numerical Analysis,
// Second Edition,
// McGraw Hill, 1972,
// ISBN: 07-012446-4.
//
// Parameters:
//
// Input, int NDATA, the number of data points.
// NDATA is assumed to be at least 2.
//
// Input, double TDATA[NDATA], the abscissas of the data points.
// The entries of TDATA are assumed to be strictly increasing.
//
// Input, double C[4*NDATA], the coefficient data computed by
// SPLINE_HERMITE_SET.
//
// Input, double TVAL, the point where the interpolant is to
// be evaluated.
//
// Output, double *SVAL, *SPVAL, the value of the interpolant
// and its derivative at TVAL.
//
/** TKS mods Feb 2008
add a return value: 0 failure, 1 success
use interpolating interval search; fail if arg out of range
don't compute derivative if spval is null
declare local vars register
**/
{
register double dt;
register int left;
left = findInterval( ndata, tdata, tval );
if( left < 0 ) return 0;
//
// Evaluate the cubic polynomial.
//
dt = tval - tdata[left];
*sval = c[0+(left)*4]
+ dt * ( c[1+(left)*4]
+ dt * ( c[2+(left)*4]
+ dt * c[3+(left)*4] ) );
if( spval )
*spval = c[1+(left)*4]
+ dt * ( 2.0 * c[2+(left)*4]
+ dt * 3.0 * c[3+(left)*4] );
return 1;
}
