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; }