// Set all the adjacent "1"s to "0"
void setToZero(vector<vector<char>>& grid, int i, int j) {
grid[i][j] = '0';
if(i > 0 && grid[i-1][j] == '1')
setToZero(grid, i-1, j);
if(i < grid.size() - 1 && grid[i+1][j] == '1')
setToZero(grid, i+1, j);
if(j > 0 && grid[i][j-1] == '1')
setToZero(grid, i, j-1);
if(j < grid[0].size() - 1 && grid[i][j+1] == '1')
setToZero(grid, i, j+1);
}
void Polynomial::doubleProduct(const Field & f,const Monomial & x,
const Polynomial & poly,const Monomial & y) {
if(&poly==this) errorc(__LINE__);
#ifdef CHECK_FOR_ZERO_COEFFICIENTS
GBStream << "MXS:doubleProduct:poly:" << poly << '\n';
GBStream << "MXS:doubleProduct:aTerm:" << aTerm << '\n';
GBStream << "MXS:doubleProduct:bTerm:" << bTerm << '\n';
#endif
setToZero();
if(!f.zero()) {
const int sz = poly.numberOfTerms();
PolynomialIterator w = poly.begin();
Term t;
for(int i=1;i<=sz;++i,++w) {
t = *w;
t.Coefficient() *= f;
Monomial & m = t.MonomialPart();
Monomial result(x);
result *= m;
result *= y;
m = result;
addTermToEnd(t);
}
}
};
int main()
{
double x [100];
double y [50];
int i;
srand(time(NULL));
setToZero(x,100);
/*fillArray(x, 10);*/
fillRandom(&x[0],100);
printArray(x,100);
printf("The largest is %6.2f\n",largest(x,100));
printf("Total is %6.2f\n",sumArray(x,100));
printf("Ave is %6.2f\n",aveArray(x,100));
printf("\n@@@@@@@@@@Sorted@@@@@@@@@@@@@@@@@@\n");
doSort(x,100);
printArray(x,100);
/*
fillRandom(y,50);
printArray(y,50);
*/
return 0;
}
bool EyeTrackerCalibration::estimateParameters(
const std::vector<cv::Point2d> & eyeData,
const std::vector<cv::Point2d> & calPointData)
{
Q_ASSERT(eyeData.size() == calPointData.size());
const size_t dataCounter = eyeData.size();
qDebug() << "Screen points:";
for(size_t i = 0; i < dataCounter; i++)
qDebug() << calPointData[i].x << " " << calPointData[i].y;
qDebug() << "Pupil positions:";
for(size_t i = 0; i < dataCounter; i++)
qDebug() << eyeData[i].x << " " << eyeData[i].y;
if(dataCounter <= 0)
{
setToZero();
return false;
}
// Calculate calibration using two methods.
// Active gaze estimation method can be selected later.
estimateParametersMethod1(eyeData, calPointData);
estimateParametersMethod2(eyeData, calPointData);
return true;
}
inline void Polynomial::operator = (const Term & RightSide) {
setToZero();
if(!RightSide.CoefficientPart().zero())
{
addTermToEnd(RightSide);
}
// The list is now properly ordered.
};
void Polynomial::setToOne() {
setToZero();
operator =(Term::s_TERM_ONE);
if(AdmissibleOrder::s_getCurrentP()==0) {
GBStream << "Invalid order pointer.\n";
errorc(__LINE__);
}
};
void Polynomial::operator +=(const Polynomial & p) {
//GBStream << *this << "+=" << p << '\n';
if(!p.zero()) {
if(zero()) {
operator =(p);
} else {
Polynomial temp(*this);
setToZero();
const int sz1 = temp.numberOfTerms();
const int sz2 = p.numberOfTerms();
PolynomialIterator w1 = temp.begin();
PolynomialIterator w2 = p.begin();
int i1=1;
int i2=1;
Term t1 = * w1;
Term t2 = * w2;
while(i1<=sz1 && i2<=sz2) {
int cmp = compareTwoMonomials(t1.MonomialPart(),t2.MonomialPart());
//GBStream << "Add: cmp is " << cmp << '\n';
if(cmp==0) {
t1.Coefficient() += t2.CoefficientPart();
if(!t1.CoefficientPart().zero()) {
//GBStream << "Adding element" << TERM(c,t1.MonomialPart()) << '\n';
addTermToEnd(t1);
}
++w1; ++i1;
++w2; ++i2;
if(i1<=sz1 && i2<=sz2) {
t1 = * w1;
t2 = * w2;
}
} else if(cmp<0) {
//GBStream << "Adding element" << t2 << '\n';
addTermToEnd(t2);
++w2; ++i2;
if(i2<=sz2) t2 = * w2;
} else // if(cmp>0)
{
//GBStream << "Adding element" << t1 << '\n';
addTermToEnd(t1);
++w1; ++i1;
if(i1<=sz1) t1 = * w1;
}
}
for(;i1<=sz1;++i1,++w1) {
//GBStream << "Adding element" << *w1 << '\n';
addTermToEnd(*w1);
}
for(;i2<=sz2;++i2,++w2) {
//GBStream << "Adding element" << *w2 << '\n';
addTermToEnd(*w2);
}
}
}
};
void Polynomial::setWithList(const list<Term> & x) {
setToZero();
typedef list<Term>::const_iterator LI;
LI w(x.begin()), e(x.end());
Polynomial p;
while(w!=e) {
p = *w;
operator +=(p);
++w;
};
};
void initialize(int INinitPoint)
{
zeroMotors();
//--SET VALUES--//
PIDLineFollow.kp = 0.2;
PIDLineFollow.ki = 0.00;
PIDLineFollow.kd = 0.00;
PIDDriveL.kp = 0.5;
PIDDriveL.ki = 0.00;
PIDDriveL.kd = 0.00;
PIDDriveR.kp = 0.5;
PIDDriveR.ki = 0.00;
PIDDriveR.kd = 0.00;
PIDGyro1.kp = 1.8; //1 wheel
PIDGyro1.ki = 0.00;
PIDGyro1.kd = 0.02;
PIDGyro2.kp = 1.2; //2 wheels
PIDGyro2.ki = 0.00;
PIDGyro2.kd = 0.00;
slewConstants[D_RIGHT]= AUTO_DRV_SLEW; //AUTONOMOUS
slewConstants[D_LEFT] = AUTO_DRV_SLEW;
//--RESET TIMERS--//
ClearTimer(T1); //Current Autonomous Step
ClearTimer(T2); //PID wait
ClearTimer(T3); //Entire Autonomous Time
ClearTimer(T4); //unassigned
//--MISCELLANEOUS--//
writeDebugStreamLine("================");
setToZero(senEncL);
setToZero(senEncR);
senAddToAbsGyro=INinitPoint;
}
void autoReset(int INcurrentStep)
{
zeroMotors();
if (INcurrentStep == 0) //Runs at start of Autonomous
{
ClearTimer(T1);
ClearTimer(T3);
autoTimer=0;
autoClockRunning = true;
autoStep = 0;
setToZero(senEncL);
setToZero(senEncR);
}
else //Runs at end of Autonomous
{
writeDebugStreamLine("----------------");
writeDebugStreamLine("Time: %.1f",((float)autoTimer/1000));
writeDebugStreamLine("----------------");
autoClockRunning = false;
setToZero(senEncL);
setToZero(senEncR);
}
}
void Polynomial::setWithList(const list<Term> & x) {
setToZero();
typedef list<Term>::const_iterator LI;
LI w(x.begin()), e(x.end());
Polynomial p;
while(w!=e) {
p = *w;
operator +=(p);
#ifdef POLYNOMIAL_USE_LIST
putAllInList();
#endif
++w;
};
};
void Polynomial::operator *= (const Field & x) {
if(!x.zero() && !zero()) {
Polynomial temp(*this);
Term aterm(temp.tip());
aterm.Coefficient() *= x;
Copy<Term> copyterm(aterm);
temp.Removetip();
setToZero();
PolynomialRep5 * q = new PolynomialRep5(copyterm,x,temp);
addIntoSet(q);
};
#ifdef POLYNOMIAL_USE_LIST
putAllInList();
#endif
};
int numIslands(vector<vector<char>>& grid) {
if(grid.empty()) return 0;
int result = 0;
for (int i = 0; i < grid.size(); i++)
{
for (int j = 0; j < grid[0].size(); j++)
{
if(grid[i][j] == '1')
{
result++;
setToZero(grid, i, j);
}
}
}
return result;
}
BigInteger::BigInteger( const char * s)
{
if (debugOn ) cout << " DEBUG: char * argument Constructor used to create BigInteger at " << this << endl;
int len = strlen(s);
setToZero();
if ( len > NUMDIGITS )
{
overflow = true;
return;
}
overflow = false;
for ( int i = NUMDIGITS - 1, j = len - 1; j >= 0; j--,i--)
{
digits[i] = s[j] - '0';
}
return;
}
void Polynomial::doubleProduct(const Monomial & x,
const Polynomial & poly,const Monomial & y) {
if(&poly==this) DBG();
setToZero();
if(!poly.zero()) {
Polynomial temp(poly);
Term aterm(x);
aterm *= temp.tip();
aterm.MonomialPart() *= y;
Copy<Term> copyterm(aterm);
temp.Removetip();
PolynomialRep4 * p = new PolynomialRep4(copyterm,x,temp,y);
d_set.insert(p);
#ifdef POLYNOMIAL_USE_LIST
putAllInList();
#endif
};
};
void BigReal::trimHead() {
m_expo--;
Digit *p;
for(p = m_first->next; p && (p->n == 0); p = p->next, m_expo--);
if(p == NULL) { // all digits were 0 => *this = zero
setToZero();
} else {
deleteDigits(m_first, p->prev);
(m_first = p)->prev = NULL;
if(m_last->n == 0) {
for(m_low++, p = m_last->prev; p->n == 0; p = p->prev, m_low++);
deleteDigits(p->next, m_last);
(m_last = p)->next = NULL;
}
}
}
int Application::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QObject::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: update((*reinterpret_cast< QString(*)>(_a[1])),(*reinterpret_cast< int(*)>(_a[2]))); break;
case 1: setToZero(); break;
case 2: onExit(); break;
case 3: Tick(); break;
case 4: Screen(); break;
case 5: Hide(); break;
default: ;
}
_id -= 6;
}
return _id;
}
void Polynomial::doubleProduct(const Monomial & x,
const Polynomial & poly,const Monomial & y) {
if(&poly==this) errorc(__LINE__);
#ifdef CHECK_FOR_ZERO_COEFFICIENTS
GBStream << "MXS:doubleProduct:poly:" << poly << '\n';
GBStream << "MXS:doubleProduct:aTerm:" << aTerm << '\n';
GBStream << "MXS:doubleProduct:bTerm:" << bTerm << '\n';
#endif
setToZero();
const int sz = poly.numberOfTerms();
PolynomialIterator w = poly.begin();
Term result;
for(int i=1;i<=sz;++i,++w) {
result.assign(x);
result *= (*w);
result *= y;
addTermToEnd(result);
}
};
/* allocate an object */
void *ggggc_malloc(struct GGGGC_Descriptor *descriptor)
{
/* FILLME */
ggc_size_t *ret = NULL;
//allocate a pool if no pool
if(!ggggc_fromPoolList){
ggggc_fromPoolList = newPool(1);
ggggc_fromPoolList->generation = 1;
totalYoungSpace += ggggc_fromPoolList->end - ggggc_fromPoolList->start;
ggggc_toPoolList = newPool(1);
ggggc_toPoolList->generation = 3;
totalYoungSpace += ggggc_toPoolList->end - ggggc_toPoolList->start;
ggggc_fromPoolTail = ggggc_fromPoolList;
ggggc_toPoolTail = ggggc_toPoolList;
}
if(!ggggc_curToPool)
ggggc_curToPool = ggggc_toPoolList;
while (ggggc_curToPool){
if (ggggc_curToPool->end - ggggc_curToPool->free >= descriptor->size){
//allocate using free space
ret = (ggc_size_t*)ggggc_curToPool->free;
ggggc_curToPool->free += descriptor->size;
setToZero(ret, descriptor->size);
((struct GGGGC_Header*)ret)->descriptor__ptr = descriptor;
//totalFilled += allocationSizeRounded;
return ret;
}
ggggc_curToPool = ggggc_curToPool -> next;
}
ggggc_collect();
//the descriptor used for allocate sis moved after collect
if(isForwarded((ggc_size_t *)descriptor))
descriptor = (struct GGGGC_Descriptor *)(getForwardingAddress((ggc_size_t *)descriptor));
return ggggc_malloc(descriptor);
//return GC_MALLOC(descriptor->size * sizeof(void*));
}
void BigReal::load(ByteInputStream &s) {
int b = s.getByte();
if(b == EOF) {
throwBigRealException(_T("Unexpected enf of stream"));
}
if(b & ZERO_FLAG) {
setToZero();
} else {
clearDigits();
m_negative = (b & NEGATIVE_FLAG) ? true : false;
s.getBytesForced((BYTE*)&m_expo, sizeof(m_expo));
size_t length;
s.getBytesForced((BYTE*)&length, sizeof(length));
for(size_t i = 0; i < length; i++) {
BRDigitType d;
s.getBytesForced((BYTE*)(&d), sizeof(d));
assert(d < BIGREALBASE);
appendDigit(d);
}
m_low = m_expo - length + 1;
}
}
void allocate(int nb, int np, bool initToZero)
{ F_GB.resize(nb); f_GP.resize(np);
if (initToZero) setToZero(); else setToNaN(); }
vec3::vec3()
{
setToZero();
}
void
EBCoarToFineRedist::
define(const EBLevelGrid& a_eblgFine,
const EBLevelGrid& a_eblgCoar,
const int& a_nref,
const int& a_nvar,
const int& a_redistRad)
{
CH_TIME("EBCoarToFineRedist::define");
//from here we can assume the redistRad == 1
m_isDefined = true;
m_nComp = a_nvar;
m_refRat = a_nref;
m_domainCoar = a_eblgCoar.getDomain().domainBox();
m_gridsFine = a_eblgFine.getDBL();
m_gridsCoar = a_eblgCoar.getDBL();
m_ebislFine = a_eblgFine.getEBISL();
m_ebislCoar = a_eblgCoar.getEBISL();
m_redistRad = a_redistRad;
//created the coarsened fine layout
m_gridsCedFine = DisjointBoxLayout();
coarsen(m_gridsCedFine, m_gridsFine, m_refRat);
const EBIndexSpace* const ebisPtr = a_eblgFine.getEBIS();
CH_assert(ebisPtr->isDefined());
int nghost = 3*m_redistRad;
ebisPtr->fillEBISLayout(m_ebislCedFine, m_gridsCedFine,
m_domainCoar, nghost);
m_ebislCedFine.setMaxRefinementRatio(m_refRat, ebisPtr);
//define the intvectsets over which the objects live
m_setsCoar.define(m_gridsCoar);
m_setsCedFine.define(m_gridsCedFine);
//the buffers for the fine level are really simple.
//grow the coarsened fine grid by the redist radius
//and subtract off the coarsened fine grids.
//intersect with the irregular cells.
//this way get irregular cells range of this grid on coarse
//grid.
IntVectSet coveringIVS;
{
CH_TIME("coarsened_fine_sets");
coveringIVS = a_eblgFine.getCoveringIVS();
coveringIVS.coarsen(m_refRat);
for (DataIterator dit = m_gridsCedFine.dataIterator();
dit.ok(); ++dit)
{
Box grownBox = grow(m_gridsCedFine.get(dit()), m_redistRad);
grownBox &= m_domainCoar;
m_setsCedFine[dit()] = m_ebislCedFine[dit()].getIrregIVS(grownBox);
m_setsCedFine[dit()] -= coveringIVS;
}
}
//the coarse buffers are more complicated.
//Need to intersect irregular cells on this grid
// with entire coarse/fine interface of width redist radius
{
CH_TIME("coarse_sets");
IntVectSet cfInterface = coveringIVS;
cfInterface.grow(m_redistRad);
cfInterface &= m_domainCoar;
cfInterface -= coveringIVS;
for (DataIterator dit = m_gridsCoar.dataIterator();
dit.ok(); ++dit)
{
Box coarBox = m_gridsCoar.get(dit());
m_setsCoar[dit()] = m_ebislCoar[dit()].getIrregIVS(coarBox);
m_setsCoar[dit()] &= cfInterface;
}
}
defineDataHolders();
setToZero();
}
void
EBCoarToFineRedist::
define(const DisjointBoxLayout& a_dblFine,
const DisjointBoxLayout& a_dblCoar,
const EBISLayout& a_ebislFine,
const EBISLayout& a_ebislCoar,
const Box& a_domainCoar,
const int& a_nref,
const int& a_nvar,
int a_redistRad,
const EBIndexSpace* ebisPtr)
{
CH_TIME("EBCoarToFineRedist::define");
m_isDefined = true;
m_nComp = a_nvar;
m_refRat = a_nref;
m_domainCoar = a_domainCoar;
m_gridsFine = a_dblFine;
m_gridsCoar = a_dblCoar;
m_ebislFine = a_ebislFine;
m_ebislCoar = a_ebislCoar;
m_redistRad = a_redistRad;
//created the coarsened fine layout
m_gridsCedFine = DisjointBoxLayout();
coarsen(m_gridsCedFine, m_gridsFine, m_refRat);
CH_assert(ebisPtr->isDefined());
int nghost = 3*m_redistRad;
ebisPtr->fillEBISLayout(m_ebislCedFine, m_gridsCedFine,
m_domainCoar, nghost);
m_ebislCedFine.setMaxRefinementRatio(m_refRat, ebisPtr);
//define the intvectsets over which the objects live
m_setsCoar.define(m_gridsCoar);
m_setsCedFine.define(m_gridsCedFine);
//the buffers for the fine level are really simple.
//grow the coarsened fine grid by the redist radius
//and subtract off the coarsened fine grids.
//intersect with the irregular cells.
//this way get irregular cells range of this grid on coarse
//grid.
{
CH_TIME("coarsened_fine_sets");
for (DataIterator dit = m_gridsCedFine.dataIterator();
dit.ok(); ++dit)
{
Box grownBox = grow(m_gridsCedFine.get(dit()), m_redistRad);
grownBox &= m_domainCoar;
IntVectSet localIVS = m_ebislCedFine[dit()].getIrregIVS(grownBox);
//subtract off all boxes on fine level so we get stuff at the
//coarse-fine interface that will redistribute to this grid
for (LayoutIterator lit = m_gridsCedFine.layoutIterator();
lit.ok(); ++lit)
{
localIVS -= m_gridsCedFine.get(lit());
}
m_setsCedFine[dit()] = localIVS;
}
}
//the coarse buffers are more complicated.
//Need to intersect irregular cells on this grid
// with entire coarse/fine interface of width redist radius
{
CH_TIME("coarse_sets");
for (DataIterator dit = m_gridsCoar.dataIterator();
dit.ok(); ++dit)
{
Box coarBox = m_gridsCoar.get(dit());
//make the complement set the whole
IntVectSet cfIntComp(coarBox);
//subtract the CF Interface from each coarsened fine box
//from the complement
for (LayoutIterator lit1 = m_gridsCedFine.layoutIterator();
lit1.ok(); ++lit1)
{
Box grownBox = grow(m_gridsCedFine.get(lit1()), m_redistRad);
grownBox &= m_domainCoar;
IntVectSet cedCFIVS(grownBox);
//subtract off all boxes on fine level so we get stuff at the
//coarse-fine interface that will redistribute to this grid
for (LayoutIterator lit2 = m_gridsCedFine.layoutIterator();
lit2.ok(); ++lit2)
{
cedCFIVS -= m_gridsCedFine.get(lit2());
}
cfIntComp -=cedCFIVS;
}
//coarse fine interface is whole box - complement
IntVectSet cfInterface(coarBox);
cfInterface -= cfIntComp;
IntVectSet localIVS = m_ebislCoar[dit()].getIrregIVS(coarBox);
//intersect with fat coarse-fine interface
localIVS &= cfInterface;
m_setsCoar[dit()] = localIVS;
}
}
//define the registers and all that
defineDataHolders();
//initialize the buffers to zero
setToZero();
}
void
EBFineToCoarRedist::
define(const EBLevelGrid& a_eblgFine,
const EBLevelGrid& a_eblgCoar,
const int& a_nref,
const int& a_nvar,
const int& a_redistRad)
{
CH_TIME("EBFineToCoarRedist::define_with_eblevelgrids");
m_isDefined = true;
m_nComp = a_nvar;
m_refRat = a_nref;
m_redistRad = a_redistRad;
m_domainCoar= a_eblgCoar.getDomain().domainBox();
m_gridsFine = a_eblgFine.getDBL();
m_gridsCoar = a_eblgCoar.getDBL();
m_ebislFine = a_eblgFine.getEBISL();
m_ebislCoar = a_eblgCoar.getEBISL();
//created the coarsened fine layout
m_gridsRefCoar = DisjointBoxLayout();
refine(m_gridsRefCoar, m_gridsCoar, m_refRat);
const EBIndexSpace* ebisPtr = a_eblgFine.getEBIS();
CH_assert(ebisPtr->isDefined());
int nghost = 3*m_redistRad;
Box domainFine = refine(m_domainCoar, m_refRat);
ebisPtr->fillEBISLayout(m_ebislRefCoar, m_gridsRefCoar,
domainFine, nghost);
m_ebislRefCoar.setMaxCoarseningRatio(m_refRat,ebisPtr);
//define the intvectsets over which the objects live
m_setsFine.define(m_gridsFine);
m_setsRefCoar.define(m_gridsCoar);
//make sets
//global set consists of redistrad on the fine side of the coarse-fine
//interface
//the fine set is that within one fine box.
//the refcoar set is that set within one refined coarse box.
IntVectSet fineShell;
{
CH_TIME("make_fine_sets");
IntVectSet fineInterior = a_eblgFine.getCoveringIVS();
EBArith::shrinkIVS(fineInterior, m_redistRad);
fineShell = a_eblgFine.getCoveringIVS();
fineShell -= fineInterior;
for (DataIterator dit = m_gridsFine.dataIterator(); dit.ok(); ++dit)
{
const Box& fineBox = m_gridsFine.get(dit());
m_setsFine[dit()] = m_ebislFine[dit()].getIrregIVS(fineBox);
m_setsFine[dit()] &= fineShell;
}
}
{
CH_TIME("make_coar_sets");
for (DataIterator dit = m_gridsCoar.dataIterator(); dit.ok(); ++dit)
{
Box grownBox = grow(m_gridsRefCoar.get(dit()), m_redistRad);
grownBox &= domainFine;
m_setsRefCoar[dit()] = m_ebislRefCoar[dit()].getIrregIVS(grownBox);
m_setsRefCoar[dit()] &= fineShell;
}
}
defineDataHolders();
setToZero();
}
//extern "C"{
SEXP sampler(
/*prior params*/
double *a1, double *a2, /* prior for tau2*/
double *b1, double *b2, /* prior for sigma2 */
double *alphaW, double *betaW, /* prior for w */
double *v0, /* gamma */
double *varKsi, /*vector length qKsiUpdate!!*/
/*model dimensions*/
int *q, /*length of ksi*/
int *qKsiUpdate, /*length of updated ksi*/
int *p, /*length alpha*/
int *pPen, /*length penalized alpha/ tau2 / gamma*/
int *n, /* no. of obs.*/
int *d, /*vector (length p): group sizes*/
/*parameter vectors*/
double *beta,
double *alpha,
double *ksi,
double *tau2,
double *gamma,
double *sigma2,
double *w,
/* (precomputed) constants */
double *y,
double *X,
double *G,
double *scale,
double *offset,
/*info about updateBlocks*/
int *blocksAlpha,
int *indA1Alpha,
int *indA2Alpha,
int *blocksKsi,
int *indA1Ksi,
int *indA2Ksi,
/*MCMC parameters*/
int *pcts,
int *burnin,
int *thin,
int *totalLength,
int *verbose,
double *ksiDF,
int *scaleMode,
double *modeSwitching,
int *family,
double *acceptKsi,
double *acceptAlpha,
/*return matrices*/
double *betaMatR,
double *alphaMatR,
double *ksiMatR,
double *gammaMatR,
double *probV1MatR,
double *tau2MatR,
double *sigma2MatR,
double *wMatR,
double *likMatR,
double *logPostMatR
)
{
// ############################################### //
// ######## unwrap/initialize args ############### //
// ############################################### //
int pIncluded=0, i=0, j=0, startPen = *p-*pPen, qKsiNoUpdate = *q - *qKsiUpdate,
save = 0, keep = *burnin, nrv =1, info=0,
nSamp=(*totalLength-*burnin)/(*thin), oneInt = 1, zeroInt = 0;
double *p1 =Calloc(*pPen, double);
double infV = 100000, oneV = 1.0, zeroV = 0.0, minusOneV =-1.0;
double *one=&oneV, *zero=&zeroV, *minusOne=&minusOneV, *inf=&infV, acceptance=0;
double invSigma2 = 1 / *sigma2, sqrtInvSigma2 = R_pow(invSigma2, 0.5);
double *penAlphaSq, *alphaLong, *varAlpha, *priorMeanAlpha, *modeAlpha, *offsetAlpha;;
penAlphaSq = Calloc(*pPen, double);
for(int i=*p-*pPen; i<*p; i++) penAlphaSq[i- *p + *pPen] = R_pow(alpha[i], 2.0);
alphaLong = Calloc(*q, double);
F77_CALL(dgemm)("N","N", q, &oneInt, p, one, G, q, alpha, p, zero, alphaLong, q);
varAlpha = Calloc(*p, double);
for(int i=0; i<startPen; i++) varAlpha[i] = *inf; /*unpenalized*/
for(int i=startPen; i<*p; i++) varAlpha[i] = tau2[i-startPen]*gamma[i-startPen]; /*penalized*/
priorMeanAlpha = Calloc(*p, double);
setToZero(priorMeanAlpha, *p);
modeAlpha = Calloc(*p, double);
F77_CALL(dcopy)(p, alpha, &oneInt, modeAlpha, &oneInt);
offsetAlpha = Calloc(*n, double);
F77_CALL(dcopy)(n, offset, &oneInt, offsetAlpha, &oneInt);
double *ksiUpdate, *priorMeanKsi, *modeKsi, *offsetKsi;
int safeQKsiUpdate = imax2(1, *qKsiUpdate);
//ksiUpdate contains the last qKsiUpdate elements in ksi
ksiUpdate = Calloc(safeQKsiUpdate, double);
F77_CALL(dcopy)(&safeQKsiUpdate, &ksi[*q-safeQKsiUpdate], &oneInt, ksiUpdate, &oneInt);
priorMeanKsi = Calloc(safeQKsiUpdate, double);
setToZero(priorMeanKsi, safeQKsiUpdate);
for(int i=0; i<*qKsiUpdate; i++) priorMeanKsi[i] = 1.0;
modeKsi = Calloc(safeQKsiUpdate, double);
setToZero(modeKsi, safeQKsiUpdate);
for(int i=0; i<*qKsiUpdate; i++) modeKsi[i] = ksi[i+qKsiNoUpdate];
// offsetKsi = offset + X_d=1*alpha : use lin.predictor of grps with ksi==1 as offset
offsetKsi = Calloc(*n, double);
F77_CALL(dcopy)(n, offset, &oneInt, offsetKsi, &oneInt);
if(qKsiNoUpdate < *q){
if(qKsiNoUpdate > 0){
F77_CALL(dgemm)("N","N", n, &oneInt, &qKsiNoUpdate, one, X, n, alpha, &qKsiNoUpdate, one, offsetKsi, n);
}
}
double *eta, *resid, rss, *XAlpha, *XKsiUpdate, *etaOffset;
eta = Calloc(*n, double);
F77_CALL(dgemm)("N","N", n, &oneInt, q, one, X, n, beta, q, zero, eta, n);
resid = Calloc(*n, double);
rss = 0;
for(int i=0; i<*n; i++) {
resid[i] = y[i]-eta[i] - offset[i];
rss += R_pow(resid[i], 2.0);
}
XAlpha = Calloc(*p * (*n), double);
updateXAlpha(XAlpha, X, G, ksi, q, qKsiUpdate, p, n);
XKsiUpdate = Calloc( *n * safeQKsiUpdate, double);
setToZero(XKsiUpdate, *n * safeQKsiUpdate);
if(qKsiNoUpdate < *q){
updateXKsi(XKsiUpdate, X, alphaLong, q, &qKsiNoUpdate, n);
}
etaOffset = Calloc(*n, double);
for(int i=0; i<*n; i++) etaOffset[i] = eta[i]+offset[i];
// ############################################################ //
// ######## set up blocks for blockwise updates ############### //
// ############################################################ //
XBlockQR *AlphaBlocks = Calloc(*blocksAlpha, XBlockQR);
XBlockQR *KsiBlocks = Calloc(*blocksKsi, XBlockQR);
for(int i=0; i < *blocksAlpha; i++){
(AlphaBlocks[i]).indA1 = indA1Alpha[i];
(AlphaBlocks[i]).indA2 = indA2Alpha[i];
(AlphaBlocks[i]).qA = (AlphaBlocks[i]).indA2 - (AlphaBlocks[i]).indA1 + 1;
(AlphaBlocks[i]).qI = *p - (AlphaBlocks[i]).qA;
(AlphaBlocks[i]).qraux = Calloc((AlphaBlocks[i]).qA, double);
setToZero((AlphaBlocks[i]).qraux, (AlphaBlocks[i]).qA);
(AlphaBlocks[i]).work = Calloc((AlphaBlocks[i]).qA, double);
setToZero((AlphaBlocks[i]).work, (AlphaBlocks[i]).qA);
(AlphaBlocks[i]).pivots = Calloc((AlphaBlocks[i]).qA, int);
for(int j=0; j < (AlphaBlocks[i]).qA; j++) (AlphaBlocks[i]).pivots[j] = 0;
(AlphaBlocks[i]).coefI = Calloc((AlphaBlocks[i]).qI, double);
setToZero((AlphaBlocks[i]).coefI, (AlphaBlocks[i]).qI);
(AlphaBlocks[i]).Xa = Calloc(((AlphaBlocks[i]).qA + *n) * (AlphaBlocks[i]).qA, double);
setToZero((AlphaBlocks[i]).Xa, ((AlphaBlocks[i]).qA + *n) * (AlphaBlocks[i]).qA);
(AlphaBlocks[i]).Xi = Calloc(*n * (AlphaBlocks[i]).qI, double);
setToZero((AlphaBlocks[i]).Xi, *n * (AlphaBlocks[i]).qI );
(AlphaBlocks[i]).ya = Calloc(((AlphaBlocks[i]).qA + *n), double);
F77_CALL(dcopy)(n, y, &nrv, (AlphaBlocks[i]).ya, &nrv);
setToZero((AlphaBlocks[i]).ya + *n, (AlphaBlocks[i]).qA);
(AlphaBlocks[i]).m = Calloc((AlphaBlocks[i]).qA, double);
setToZero((AlphaBlocks[i]).m, (AlphaBlocks[i]).qA);
(AlphaBlocks[i]).err = Calloc((AlphaBlocks[i]).qA, double);
setToZero((AlphaBlocks[i]).err, (AlphaBlocks[i]).qA);
}
initializeBlocksQR(AlphaBlocks, XAlpha, *n, *blocksAlpha, *p, varAlpha, scale);
if(*qKsiUpdate > 0){
for(int i=0; i < *blocksKsi; i++){
(KsiBlocks[i]).indA1 = indA1Ksi[i];
(KsiBlocks[i]).indA2 = indA2Ksi[i];
(KsiBlocks[i]).qA = (KsiBlocks[i]).indA2 - (KsiBlocks[i]).indA1 + 1;
(KsiBlocks[i]).qI = *qKsiUpdate - (KsiBlocks[i]).qA;
(KsiBlocks[i]).qraux = Calloc((KsiBlocks[i]).qA, double);
setToZero((KsiBlocks[i]).qraux, (KsiBlocks[i]).qA);
(KsiBlocks[i]).work = Calloc((KsiBlocks[i]).qA, double);
setToZero((KsiBlocks[i]).work, (KsiBlocks[i]).qA);
(KsiBlocks[i]).pivots = Calloc((KsiBlocks[i]).qA, int);
for(int j=0; j < (KsiBlocks[i]).qA; j++) (KsiBlocks[i]).pivots[j] = 0;
(KsiBlocks[i]).coefI = Calloc((KsiBlocks[i]).qI, double);
setToZero((KsiBlocks[i]).coefI, (KsiBlocks[i]).qI);
(KsiBlocks[i]).Xa = Calloc(((KsiBlocks[i]).qA + *n) * (KsiBlocks[i]).qA, double);
setToZero((KsiBlocks[i]).Xa, ((KsiBlocks[i]).qA + *n) * (KsiBlocks[i]).qA);
(KsiBlocks[i]).Xi = Calloc(*n * (KsiBlocks[i]).qI, double);
setToZero((KsiBlocks[i]).Xi, *n * (KsiBlocks[i]).qI );
(KsiBlocks[i]).ya = Calloc(((KsiBlocks[i]).qA + *n), double);
F77_CALL(dcopy)(n, y, &nrv, (KsiBlocks[i]).ya, &nrv);
setToZero((KsiBlocks[i]).ya + *n, (KsiBlocks[i]).qA);
(KsiBlocks[i]).m = Calloc((KsiBlocks[i]).qA, double);
setToZero((KsiBlocks[i]).m, (KsiBlocks[i]).qA);
(KsiBlocks[i]).err = Calloc((KsiBlocks[i]).qA, double);
setToZero((KsiBlocks[i]).err, (KsiBlocks[i]).qA);
}
initializeBlocksQR(KsiBlocks, XKsiUpdate, *n, *blocksKsi, *qKsiUpdate, varKsi, scale);
}
// ############################################### //
// ######## start sampling ############### //
// ############################################### //
#ifdef Win32
R_FlushConsole();
#endif
/* sampling */
GetRNGstate();
for(i = 0; i < *totalLength; i++)
{
debugMsg("\n###########################################\n\n");
//update alpha
{
//update varAlpha
for(j=startPen; j<*p; j++) varAlpha[j] = tau2[j-startPen] * gamma[j-startPen];
//update alpha
updateCoefQR(y, XAlpha, AlphaBlocks,
*blocksAlpha,
alpha,
varAlpha, *p,
scale,
*n, nrv, oneInt, info, *minusOne, *zero, *one, 1, priorMeanAlpha,
*family, modeAlpha, eta, acceptAlpha, offsetAlpha, *modeSwitching, zeroInt);
}
//update ksi
if(qKsiNoUpdate < *q){
//update alphaLong = G %*% alpha
F77_CALL(dgemm)("N","N", q, &oneInt, p, one, G, q, alpha, p, zero, alphaLong, q);
//update design for ksi
updateXKsi(XKsiUpdate, X, alphaLong, q, &qKsiNoUpdate, n);
//update offsetKsi
if(qKsiNoUpdate > 0){
F77_CALL(dcopy)(n, offset, &oneInt, offsetKsi, &oneInt);
F77_CALL(dgemm)("N","N", n, &oneInt, &qKsiNoUpdate, one, X, n, alpha, &qKsiNoUpdate, one, offsetKsi, n);
}
for(j = 0; j < *qKsiUpdate; j++){
priorMeanKsi[j] = sign( 1/(1 + exp(-2*ksiUpdate[j]/varKsi[j])) - runif(0,1) );
}
if(*ksiDF>0){
updateVarKsi(ksiUpdate, varKsi, ksiDF, priorMeanKsi, qKsiNoUpdate, *q);
}
updateCoefQR(y, XKsiUpdate, KsiBlocks,
*blocksKsi,
ksiUpdate, varKsi, *qKsiUpdate,
scale,
*n, nrv, oneInt, info, *minusOne, *zero, *one, 1, priorMeanKsi,
*family, modeKsi, eta, acceptKsi, offsetKsi, *modeSwitching, *scaleMode);
//write back to ksi
F77_CALL(dcopy)(qKsiUpdate, ksiUpdate, &oneInt, &ksi[*q-*qKsiUpdate], &oneInt);
//rescale ksi, alpha & put back in ksiUpdate
if(*scaleMode > 0){
rescaleKsiAlpha(ksi, alpha, varKsi, tau2, G, d, *p, *q, qKsiNoUpdate, *pPen, *scaleMode, modeAlpha, modeKsi, *family);
F77_CALL(dcopy)(qKsiUpdate, &ksi[*q-*qKsiUpdate], &oneInt, ksiUpdate, &oneInt);
}
//update XAlpha
updateXAlpha(XAlpha, X, G, ksi, q, qKsiUpdate, p, n);
//update alphaLong = G %*% alpha
F77_CALL(dgemm)("N","N", q, &oneInt, p, one, G, q, alpha, p, zero, alphaLong, q);
} else {
F77_CALL(dcopy)(q, alpha, &oneInt, alphaLong, &oneInt);
}
for(int i = *p-*pPen; i < *p; i++) penAlphaSq[i - *p + *pPen] = R_pow(alpha[i], 2.0);
updateTau(penAlphaSq, gamma, tau2, *a1, *a2, *pPen);
updateP1Gamma(penAlphaSq, tau2, p1, gamma, *v0, *w, *pPen);
pIncluded = 0;
for(j=0; j<*p - startPen; j++) pIncluded += (gamma[j] == 1.0);
*w = rbeta( *alphaW + pIncluded, *betaW + *p - pIncluded );
// update beta
for(j = 0; j < *q; j++){
beta[j] = alphaLong[j]*ksi[j];
}
//update eta, eta+offset
F77_CALL(dgemm)("N", "N", n, &oneInt, q, one, X, n, beta, q, zero, eta, n);
for(int i=0; i<*n; i++) etaOffset[i] = eta[i] + offset[i];
//update sigma_eps
if(*family == 0){
//resid = y - eta - offset
F77_CALL(dcopy)(n, y, &nrv, resid, &nrv); //resid <- y
F77_CALL(daxpy)(n, minusOne, etaOffset, &nrv, resid, &nrv); //resid <- resid - eta - offset
//rss = resid'resid
rss = F77_CALL(ddot)(n, resid, &oneInt, resid, &oneInt);
//update sigma2
invSigma2 = rgamma(*n/2 + *b1, 1/(rss/2 + *b2));
sqrtInvSigma2 = R_pow(invSigma2, 0.5);
scale[0] = sqrtInvSigma2;
*sigma2 = 1 / invSigma2;
}
if(i >= *burnin){
/* report progress */
if(*verbose){
for(j=0; j<9; j++){
if(i == pcts[j]){
Rprintf(".");
#ifdef Win32
R_FlushConsole();
#endif
break;
}
}
}
/* save samples*/
if(i == keep){
for(j = 0; j < *q; j++){
(betaMatR)[save + j*nSamp] = beta[j];
(ksiMatR)[save + j*nSamp] = ksi[j];
}
for(j=0; j < *p; j++){
(alphaMatR)[save + j*nSamp] = alpha[j];
}
for(j=0; j < *pPen; j++){
(tau2MatR)[save + j*nSamp] = tau2[j];
(gammaMatR)[save + j*nSamp] = gamma[j];
(probV1MatR)[save + j*nSamp] = p1[j];
}
(wMatR)[save] = *w;
(sigma2MatR)[save] = *sigma2;
likMatR[save] = logLik(y, etaOffset, *family, scale, *n);
(logPostMatR)[save] = updateLogPost(y, alpha, varAlpha,
ksi, varKsi, scale, *b1, *b2, gamma, *w, *alphaW, *betaW,
tau2, *a1, *a2, *n, *q, *p, *pPen, pIncluded, qKsiNoUpdate, priorMeanKsi, *family, likMatR[save]);
keep = keep + *thin;
save ++;
R_CheckUserInterrupt();
}
} else {
if(*verbose){
if(i == (*burnin-1)){
Rprintf("b");
#ifdef Win32
R_FlushConsole();
#endif
}
}
}
} /* end for i*/
PutRNGstate();
if(*verbose) Rprintf(".");
if(*family > 0) {
acceptance = 0.0;
for(j=0; j<*blocksAlpha; j++) acceptance += acceptAlpha[j];
acceptance = 0.0;
if(qKsiNoUpdate < *q){
for(j=0; j<*blocksKsi; j++) acceptance += acceptKsi[j];
}
}
Free(etaOffset); Free(XKsiUpdate); Free(XAlpha); Free(resid); Free(eta);
Free(offsetKsi); Free(modeKsi); Free(priorMeanKsi); Free(ksiUpdate);
Free(offsetAlpha);
Free(modeAlpha);
Free(priorMeanAlpha);
Free(varAlpha);
Free(alphaLong);
Free(penAlphaSq);
freeXBlockQR(AlphaBlocks, *blocksAlpha);
if(qKsiNoUpdate < *q) freeXBlockQR(KsiBlocks, *blocksKsi);
Free(p1);
return(R_NilValue);
}/* end sampler ()*/
void *ggggc_mallocOld(struct GGGGC_Descriptor *descriptor){
struct GGGGC_FreeObj *prevFreeObj = NULL, *curFreeObj = NULL, *splitFreeObj = NULL;
ggc_size_t *ret = NULL, curFreeObjSize = 0, *curFreeObjEnd = NULL, allocationSizeRounded = 0;
allocationSizeRounded = roundToTwo(descriptor->size);
/*
if((totalOldSpace < 3*totalOldFilled) && totalFilled > totalSurvivors *2){
ggggc_collect();
}
*/
//allocate a pool if no pool
if(!ggggc_oldPoolList){
ggggc_oldPoolList = newPool(1);
ggggc_oldPoolList->generation = 2;
totalOldSpace += ggggc_oldPoolList->end - ggggc_oldPoolList->start;
ggggc_curOldPool = ggggc_oldPoolList;
}
while (ggggc_curOldPool){
curFreeObj = ggggc_curOldPool -> freelist;
prevFreeObj = NULL;
while (curFreeObj){
curFreeObjEnd = GetfreeObjEnd(curFreeObj);
curFreeObjSize = curFreeObjEnd - (ggc_size_t*) curFreeObj;
if(curFreeObjSize >= allocationSizeRounded){
//free obj has enought space to allocate
ret = (ggc_size_t*) curFreeObj;
if(curFreeObjSize >= allocationSizeRounded + 2){
//Split
splitFreeObj = (struct GGGGC_FreeObj *)((ggc_size_t *) curFreeObj + allocationSizeRounded);
splitFreeObj->end_bitSneaky = curFreeObj->end_bitSneaky;
splitFreeObj->next = curFreeObj->next;
if(prevFreeObj)
prevFreeObj->next = splitFreeObj;
else if(curFreeObj == ggggc_curOldPool->freelist)
ggggc_curOldPool->freelist = splitFreeObj;
}
else{
if(prevFreeObj)
prevFreeObj->next = curFreeObj->next;
else if(curFreeObj == ggggc_curOldPool->freelist)
ggggc_curOldPool->freelist = curFreeObj->next;
}
setToZero(ret, allocationSizeRounded);
((struct GGGGC_Header*)ret)->descriptor__ptr = descriptor;
//totalFilled += allocationSizeRounded;
return ret;
}
prevFreeObj = curFreeObj;
curFreeObj = curFreeObj -> next;
}
if (ggggc_curOldPool->end - ggggc_curOldPool->free >= allocationSizeRounded){
//allocate using free space
ret = (ggc_size_t*)ggggc_curOldPool->free;
ggggc_curOldPool->free += allocationSizeRounded;
setToZero(ret, allocationSizeRounded);
((struct GGGGC_Header*)ret)->descriptor__ptr = descriptor;
//totalFilled += allocationSizeRounded;
return ret;
}
ggggc_curOldPool = ggggc_curOldPool -> next;
}
return NULL;
}
void Polynomial::operator -=(const Polynomial & p) {
if(!p.zero()) {
#if 0
if(zero()) {
InternalContainerType::iterator w = _terms.begin();
const int sz = numberOfTerms();
for(int i=1;i<=sz;++i,++w) {
(*w).Coefficient() = -1;
}
} else {
#endif
Polynomial temp(*this);
setToZero();
const int sz1 = temp.numberOfTerms();
const int sz2 = p.numberOfTerms();
PolynomialIterator w1 = temp.begin();
PolynomialIterator w2 = p.begin();
int i1=1;
int i2=1;
#if 1
if(sz1>0) {
#endif
Term t1 = * w1;
Term t2 = * w2;
while(i1<=sz1 && i2<=sz2) {
int cmp = compareTwoMonomials(t1.MonomialPart(),t2.MonomialPart());
//GBStream << "Subtract: cmp is " << cmp << '\n';
if(cmp==0) {
t1.Coefficient() -= t2.CoefficientPart();
if(!t1.Coefficient().zero()) {
//GBStream << "-=: Adding element" << TERM(c,t1.MonomialPart()) << '\n';
addTermToEnd(t1);
}
++w1; ++i1;
++w2; ++i2;
if(i1<=sz1 && i2<=sz2) {
t1 = * w1;
t2 = * w2;
}
} else if(cmp<0) {
//GBStream << "-=Adding element" << t2 << '\n';
addTermToEnd(-t2);
++w2; ++i2;
if(i2<=sz2) t2 = * w2;
} else // if(cmp>0)
{
//GBStream << "-=Adding element" << t1 << '\n';
addTermToEnd(t1);
++w1; ++i1;
if(i1<=sz1) t1 = * w1;
}
}
#if 1
}
#endif
for(;i1<=sz1;++i1,++w1) {
//GBStream << "-=Adding element" << *w1 << '\n';
addTermToEnd(*w1);
}
for(;i2<=sz2;++i2,++w2) {
//GBStream << "-=Adding element" << -*w2 << '\n';
addTermToEnd(-(*w2));
}
#if 0
}
#endif
}
};
BigInteger::BigInteger()
{
setToZero();
if (debugOn ) cout << " DEBUG: Default Constructor used to create BigInteger at " << this << endl;
}
EyeTrackerCalibration::EyeTrackerCalibration()
{
setToZero();
}
