static void* cycleWander(void *maxPtr) {
int maxX = *((int *) maxPtr);
int index = randUnif(1, 87);
int sleepFor = 0;
while (1) {
index = randNextCell(index);
pthread_mutex_lock(&mutex);
printFolly(maxX);
printCell(index, maxX);
sleepFor = randUnif(0, 4);
move(0, 0);
pthread_mutex_unlock(&mutex);
if (sleepFor == 0) {
usleep(250000);
} else if (sleepFor == 1) {
usleep(450000);
} else if (sleepFor == 2) {
usleep(500000);
} else if (sleepFor == 3) {
usleep(550000);
} else {
usleep(1250000);
}
}
return NULL;
}
void PartFilter::resample()
{
double invNbSamp = 1./mModels.size();
Eigen::VectorXf cdf(mModels.size());
cdf[0]=mCurrentWeights[0];
for (int i=1 ; i<mModels.size() ; i++)
{
cdf[i]=cdf[i-1]+mCurrentWeights[i];
}
int i=0;
double u = randUnif(invNbSamp);
for (int j=0 ; j<mModels.size() ; j++)
{
while(u>cdf[i])
{
i++;
}
for (int k=0 ; k<mOrientationVec[i].size() ; k++)
{
(*mOrientationVec[j][k])=(*mOrientationVec[i][k]);
(*mOffsetVec[j][k])=(*mOffsetVec[i][k]);
}
mCurrentWeights[j]=invNbSamp;
u=u+invNbSamp;
}
}
double Filter::randn(double sigma)
{
/*int U1int = rand()%10001;
int U2int = rand()%10001;
float U1 = (float)U1int / 10001.;
float U2 = (float)U2int / 10001.;*/
double U1, U2, X, Y;
do
{
U1 = randUnif();
U2 = randUnif();
X = sqrt(-2*log(U1))*cos(2*3.14*U2);
Y = sqrt(-2*log(U1))*sin(2*3.14*U2);
X *= sigma;
Y *= sigma;
}
while(X != X);
return X;
}
