bool performAction(char code){
printf("\tPerforming action %c on %d,%d %c\n",code,x,y,orientation);
if (code == 'L' || code == 'R'){
orientation = getNewOrientation(code == 'L');
printf("\t\tChanging orientation to %c\n", orientation);
return true;
} else if (code == 'F') {
int oldX = x;
int oldY = y;
move();
if (validPosition(x,y)){
printf("\t\tMoving to %d, %d\n", x,y);
world[x][y] = rNum;
return true;
} else if (existsHint(oldX, oldY)){
printf("\t\tIgnoring order to move to %d,%d\n", x,y);
int x = oldX;
int y = oldY;
return true;
} else {
printf("\t\tRobot lost in position %d,%d\n", x,y);
return false;
}
} else {
world[x][y] = rNum;
printf("\t\tStay in %d, %d\n", x,y);
return true;
}
}
std::string SingleParticle2dx::DataStructures::Particle::getDataString()
{
std::string result = SingleParticle2dx::Utilities::StringFunctions::TtoString(getParticleNumber());
result += "\t" + getGlobalParticleInformation().getDataString();
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getUseForReconstruction());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getLastAngularChange());
result += "\t" + getParticleShift().getDataString();
result += "\t" + getInitialOrientation().getDataString();
result += "\t" + getOldOrientation().getDataString();
result += "\t" + getNewOrientation().getDataString();
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getSimMeasure());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getQuality());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getConsistency());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getWeight());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getContainerNumber());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(getIsCMParticle());
result += "\t" + SingleParticle2dx::Utilities::StringFunctions::TtoString(calculateDensity());
result += "\t" + getClassInformation().getDataString();
return result;
}
void SingleParticle2dx::DataStructures::Particle::calculateConsistency()
{
size_type n = 6;
std::vector<Eigen::VectorXf> points;
std::vector<Particle*> neighbors = getNeighbors();
for (size_type i=0; i<static_cast<size_type>(neighbors.size()); i++ )
{
Eigen::VectorXf new_vec(n);
new_vec[0] = neighbors[i]->getNewOrientation().getTLTAXIS();
new_vec[1] = neighbors[i]->getNewOrientation().getTLTANG();
new_vec[2] = neighbors[i]->getNewOrientation().getTAXA();
new_vec[3] = neighbors[i]->getParticleShift().getShiftX();
new_vec[4] = neighbors[i]->getParticleShift().getShiftY();
new_vec[5] = neighbors[i]->getSimMeasure();
points.push_back(new_vec);
}
Eigen::VectorXf mean(n);
for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
{
mean += points[i];
}
mean /= static_cast<value_type>(points.size());
// std::cout << ": mean = " << mean[0] << " " << mean[1] << " " << mean[2] << " " << mean[3] << " " << mean[4] << " " << mean[5] << std::endl;
Eigen::MatrixXf covMat = Eigen::MatrixXf::Zero(n,n);
// std::cout << mean << std::endl;
for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
{
Eigen::VectorXf diff = (points[i]-mean).conjugate();
covMat += diff * diff.adjoint();
}
// std::cout << ":det = " << covMat.determinant() << std::endl;
value_type det = covMat.determinant();
if ( !std::isfinite(det) )
{
setConsistency(0);
return;
}
if ( det < 0.001 )
{
setConsistency(0);
return;
}
//SingleParticle2dx::Utilities::UtilityFunctions::reqularizeMatrix(covMat);
// std::cout << covMat << std::endl;
Eigen::VectorXf vec(n);
vec[0] = getNewOrientation().getTLTAXIS();
vec[1] = getNewOrientation().getTLTANG();
vec[2] = getNewOrientation().getTAXA();
vec[3] = getParticleShift().getShiftX();
vec[4] = getParticleShift().getShiftY();
vec[5] = getSimMeasure();
value_type result = -0.5 * log(covMat.determinant());
// #pragma omp critical (det_output)
//{
//std::cout << ":det = " << det << std::endl;
//}
if ( covMat.determinant() < 0.000001 )
{
setConsistency(0);
return;
}
// std::cout << ":first term: " << result << std::endl;
Eigen::VectorXf tmp1 = (covMat.inverse()) * (vec-mean);
value_type tmp2 = (vec-mean).dot(tmp1);
result -= 0.5 * tmp2;
// std::cout << ":second term: " << -0.5 * tmp2 << std::endl;
if ( boost::math::isnan(result) || boost::math::isnan(-result) )
{
std::cout << "::reset CONS realy done now" << std::endl;
result = 0;
}
setConsistency(result);
}
