int DeterministicGameState::getClosestGhostDistance(pacman_msgs::PacmanAction action)
{
geometry_msgs::Pose new_pose = getPacmanPose();
int new_x = new_pose.position.x;
int new_y = new_pose.position.y;
std::map< std::pair<int, int>, int > distances = getDistances(new_x, new_y);
int min_dist = util::INFINITE;
for(std::vector< geometry_msgs:: Pose >::reverse_iterator it = ghosts_poses_.rbegin();
it != ghosts_poses_.rend() ; ++it)
{
int dist = distances[std::make_pair(it->position.x, it->position.y)];
if (dist < min_dist) {
min_dist = dist;
}
}
return min_dist;// / ( (float) height_ * width_);
}
// TODO: dividing by map width*height
float DeterministicGameState::getClosestFoodDistance(pacman_msgs::PacmanAction action)
{
geometry_msgs::Pose new_pose = getPacmanPose();
int new_x = new_pose.position.x;
int new_y = new_pose.position.y;
std::map< std::pair<int, int>, int > distances = getDistances(new_x, new_y);
int min_dist = util::INFINITE;
for (int i = 0 ; i < height_ ; i++) {
for (int j = 0 ; j < width_ ; j++) {
if(map_[i][j] == FOOD) {
int dist = distances[std::make_pair(j, i)];
if (dist < min_dist) {
min_dist = dist;
}
}
}
}
return min_dist;// / ( (float) height_ * width_);
}
void calculateUstatistic( IPDF * pdf, IDataSet * data, PhaseSpaceBoundary * phase, TH1D * distances)
{
double pdfValue = 0.;
double distance = 0.;
double smallest_distance = 0., sd = 0.;
double U = 0;
DataPoint * event_i = 0;
DataPoint * event_j = 0;
DataPoint * closest = 0;
bool firstEvent = true;
size_t dimension = (pdf->GetPrototypeDataPoint()).size();
if ( dimension == 7 ) dimension = 4;
dimension = 2;
cout << "number of dimensions: " << dimension << endl;
unsigned int nD = (unsigned)data->GetDataNumber();
int count = 0;
for (unsigned int i = 0; i < nD; i++) {
if ( !(i % 100) ) cout << i << endl;
event_i = data->GetDataPoint( (int)i );
pdfValue = pdf->Evaluate( event_i )/pdf->Integral( event_i, phase );
firstEvent = true;
for (unsigned int j = 0; j < nD; j++) {
if ( j == i ) continue;
event_j = data->GetDataPoint( (int)j );
distance = getDistance( event_i, event_j );
if ( firstEvent ) {
smallest_distance = distance;
firstEvent = false;
}
if (distance < smallest_distance) {
smallest_distance = distance;
closest = event_j;
}
}
sd = smallest_distance;
vector<double> vectorOfDistances = getDistances( event_i, closest );
PhaseSpaceBoundary * tempPhase = new PhaseSpaceBoundary( phase->GetAllNames() );
copyPhaseSpaceBoundary( tempPhase, phase );
updatePhaseSpaceBoundary( event_i, tempPhase, phase, vectorOfDistances );
if ( vectorOfDistances[0] > vectorOfDistances[1] ) count++;
//double distToCosThetaBoundary = 1. - fabs(event_i->GetObservable( "cosTheta" )->GetValue()) ;
//double distToCosPsiBoundary = 1. - fabs(event_i->GetObservable( "cosPsi" )->GetValue()) ;
//double distToTimeBoundary = event_i->GetObservable( "time" )->GetValue() - 0.3;
//double distToMassBoundary = min(fabs(event_i->GetObservable( "mass" )->GetValue() - 5200.),fabs(event_i->GetObservable( "mass" )->GetValue() - 5550.)) ;
//(void) distToTimeBoundary; (void) distToMassBoundary; //Unused
double f1 = 1.;
double f2 = 1.;
//if( sd > distToCosThetaBoundary ) f1 = 1. - acos(distToCosThetaBoundary/sd)/TMath::Pi() ;
//if( sd > distToCosPsiBoundary ) f2 = 1. - acos(distToCosPsiBoundary/sd)/TMath::Pi() ;
//if( sd > distToTimeBoundary ) f1 = 1. - acos(distToTimeBoundary/sd)/TMath::Pi() ;
//if( sd > distToMassBoundary ) f2 = 1. - acos(distToMassBoundary/sd)/TMath::Pi() ;
if ( dimension == 1 ) U = exp(-1.*nD*2. * sd * pdfValue * f1 );
if ( dimension == 2 ) U = exp(-1.*nD * TMath::Pi() * pow(sd, 2) * pdfValue * f1 * f2 );
if ( dimension == 3 ) U = exp(-1.*nD*4./3. * TMath::Pi() * pow(sd, 3) * pdfValue );
if ( dimension == 4 ) U = exp(-1.*nD*1./2. *pow( TMath::Pi(), 2) * pow(sd, 4) * pdfValue * f1 * f2 );
if ( dimension == 5 ) U = exp(-1.*nD*8./15. *pow( TMath::Pi(), 2) * pow(sd, 5) * pdfValue );
if ( dimension == 6 ) U = exp(-1.*nD*1./6. *pow( TMath::Pi(), 3) * pow(sd, 6) * pdfValue );
if ( dimension == 7 ) U = exp(-1.*nD*16./105.*pow( TMath::Pi(), 3) * pow(sd, 7) * pdfValue );
/*if ( U < 1.e-02 ) {
cout << "pdfValue " << pdfValue << " smallest " << sd << " U " << U << " time " << event_i->GetObservable( "time" )->GetValue()
<< " mass " << event_i->GetObservable( "mass" )->GetValue()
<< " cosTheta " << event_i->GetObservable( "cosTheta" )->GetValue()
<< " phi " << event_i->GetObservable( "phi" )->GetValue()
<< " cosPsi " << event_i->GetObservable( "cosPsi" )->GetValue() << endl;
}*/
distances->Fill( U );
}
cout << "count " << count << endl;
}
