double n_2(set& s) {
double min = euclidean_distance(s[0], s[1]);
for (std::size_t i = 0; i < s.size() - 1; ++i)
for (std::size_t j = i + 1; j < s.size(); ++j)
min = std::min(min, euclidean_distance(s[i], s[j]));
return min;
}
void target_based_agent::update_ghost_locations(vector<loc> current_ghost_locations) {
if (current_ghost_locations.size() < ghost_locations.size()) {
set<loc> updated_ghost_locations;
for (int i = 0; i < current_ghost_locations.size(); ++i) {
set<loc>::iterator nearest_prev_loc = ghost_locations.begin();
double least_distance = euclidean_distance(*nearest_prev_loc, current_ghost_locations[i]);
for (set<loc>::iterator it = ghost_locations.begin(); it != ghost_locations.end(); ++it) {
double dist = euclidean_distance(*it, current_ghost_locations[i]);
if (dist < least_distance) {
least_distance = dist;
nearest_prev_loc = it;
}
}
loc ghost_loc = make_pair((*nearest_prev_loc).first, (*nearest_prev_loc).second);
updated_ghost_locations.insert(current_ghost_locations[i]);
if (ghost_locations.find(ghost_loc) == ghost_locations.end()) {
cout << "not found" << endl;
exit(0);
}
ghost_locations.erase(ghost_loc);
}
ghost_locations.insert(updated_ghost_locations.begin(), updated_ghost_locations.end());
}
else {
ghost_locations.clear();
ghost_locations.insert(current_ghost_locations.begin(), current_ghost_locations.end());
}
}
bool better_to_kick() {
if(!enemy || !enemy->known() || !ball || !ball->known() || !us || !us->known())
return false;
float enemy_to_ball = euclidean_distance(enemy->x, enemy->y, ball->x, ball->y);
if(enemy_to_ball > scale(50))
return false;
if(euclidean_distance(us->x, us->y, ball->x, ball->y) > scale(20))
return false;
float error = 0.03f;
float dir = 0;
if(old_us) {
double difference = normalize_rotation(us->rotation - old_us->rotation);
difference = std::min(difference, 2 * pi - difference);
error += difference / 2;
dir = us->rotation - old_us->rotation;
}
int good = 0;
float a = us->rotation - error + dir;
float b = us->rotation + error + dir;
init_enemy_segments();
for(int i = 0; i < 50; ++i) {
float f = a + (b - a) * i / 49;
if(f < 0.2 || f > pi - 0.2)
continue;
if((us->rotation > 0.2 && us->rotation < pi - 0.2) && (enemy_to_ball <= scale(30) || !enemy_in_the_way(f)))
++good;
}
if(good >= 2)
return true;
return false;
}
double corner_cost(loc amidar_location) {
double cost = 0;
cost += CORNER_COST / pow(euclidean_distance(amidar_location, make_pair(15, 13)), 2);
cost += CORNER_COST / pow(euclidean_distance(amidar_location, make_pair(15, 143)), 2);
cost += CORNER_COST / pow(euclidean_distance(amidar_location, make_pair(167, 13)), 2);
cost += CORNER_COST / pow(euclidean_distance(amidar_location, make_pair(167, 143)), 2);
return cost;
}
double strip_min(set& s) {
std::sort(s.begin(), s.end(), [](point a, point b) {
return a.second < b.second;
});
double min = euclidean_distance(s[0], s[1]);
for (std::size_t i = 0; i < s.size() - 1; ++i)
for (std::size_t j = i + 1; j < s.size() && (s[j].second - s[i].second) < min; ++j)
min = std::min(min, euclidean_distance(s[i], s[j]));
return min;
}
//given the feature-space histogram for an image, get the category by finding nearest centroid
int LocalDescriptorAndBagOfFeature::get_category(const Histogram &feature_vector, const std::vector<std::vector<double>> &category_centroids){
int closest_index = 0;
double closest_distance = euclidean_distance(category_centroids[0], feature_vector);
for(int i = 1; i < category_centroids.size(); i++){
double distance = euclidean_distance(category_centroids[i], feature_vector);
if(distance < closest_distance){
closest_index = i;
closest_distance = distance;
}
}
return closest_index;
}
std::vector<DataPoint> initial_centroids(std::vector<DataPoint> points, int k) {
// Build a sample of points
std::random_shuffle(points.begin(), points.end());
int sample_size =
(points.size() * 0.20) > k ? (points.size() * 0.20) : points.size();
std::vector<DataPoint>::const_iterator first = points.begin();
std::vector<DataPoint>::const_iterator last = points.begin() + sample_size;
std::vector<DataPoint> sample(first, last);
std::vector<DataPoint> centroids;
// Pick a random centroid first
int index = std::rand() % sample.size();
DataPoint centroid = sample[index];
sample.erase(sample.begin() + index);
centroids.push_back(centroid);
// Build list of centroids by always choosing the point
// farthest apart from the current centroid
while (centroids.size() < k) {
std::vector<DataPoint>::iterator max = sample.begin();
double maxDistance = euclidean_distance(centroid, *max);
for (std::vector<DataPoint>::iterator it = sample.begin() + 1;
it != sample.end(); ++it) {
double distance = euclidean_distance(centroid, *it);
if (distance > maxDistance) {
max = it;
maxDistance = distance;
}
}
centroid = *max;
centroids.push_back(centroid);
sample.erase(max);
}
for (std::vector<DataPoint>::iterator it = centroids.begin();
it != centroids.end(); ++it) {
int index = std::distance(centroids.begin(), it);
it->k = index + 1;
}
if (verbose) {
std::cout << "Initial Centroids " << std::endl;
print_data_points(centroids);
}
return centroids;
}
typename enable_if<
typename gtl_and_3<
y_pt_man_dist,
typename is_point_concept<
typename geometry_concept<PointType1>::type
>::type,
typename is_point_concept<
typename geometry_concept<PointType2>::type
>::type
>::type,
typename point_difference_type<PointType1>::type>::type
manhattan_distance(const PointType1& point1, const PointType2& point2) {
return euclidean_distance(point1, point2, HORIZONTAL) +
euclidean_distance(point1, point2, VERTICAL);
}
void move_projectiles(GameState* state) {
register int i, j;
for (i = 0; i < state->towers_length; i++) {
for (j = 0; j < state->towers[i]->tower.ammo; j++) {
Projectile* proj = &state->towers[i]->tower.projectiles[j];
int from_x = proj->world_x;
int from_y = proj->world_y;
if (proj->target != NULL && proj->live) {
int to_x = proj->target->world_x;
int to_y = proj->target->world_y;
float alpha, a, b, c;
calc_direction(from_x, from_y, to_x, to_y, &proj->direction_x, &proj->direction_y);
a = fabs(proj->target->world_x - proj->world_x);
b = fabs(proj->target->world_y - proj->world_y);
c = euclidean_distance(proj->target->world_x, proj->target->world_y, proj->world_x, proj->world_y);
alpha = find_alpha(a, b, c);
proj->world_x += proj->speed * proj->direction_x;
proj->world_y += proj->speed * proj->direction_y;
proj->angle = find_render_angle(alpha, proj->direction_x, proj->direction_y);
} else{
//verwijder stilstaande projectielen
proj->live = 0;
}
}
}
}
void finding_neighbour_sensor_nodes(int n)
{
double distance;
int i, j, k;
for(i=0; i<n; i++)
{
sensor_node[i].neighbour_size=0;
int phy_neighbours_array[100000];
for(j=0; j<n; j++)
{
if(i!=j)
{
distance = euclidean_distance(sensor_node[i].x, sensor_node[i].y, sensor_node[j].x, sensor_node[j].y);
if(distance<25.00)
{
phy_neighbours_array[sensor_node[i].neighbour_size] = j;
sensor_node[i].neighbour_size++;
}
}
}
//physical_neighbour_size = physical_neighbour_size + sensor_node[i].phynbrsize;
sensor_node[i].phynbr =(int*)malloc(sensor_node[i].neighbour_size*sizeof(int));
for(k=0; k<sensor_node[i].neighbour_size; k++)
{
*(sensor_node[i].phynbr+k) = phy_neighbours_array[k];
}
}
}
double xmeans::splitting_criterion(const std::vector<std::vector<unsigned int> * > * const analysed_clusters, const std::vector<std::vector<double> > * const analysed_centers) const {
std::vector<double> scores(analysed_centers->size(), 0.0);
double dimension = (*analysed_centers)[0].size();
double sigma = 0.0;
unsigned int K = analysed_centers->size();
unsigned int N = 0;
for (unsigned int index_cluster = 0; index_cluster < analysed_clusters->size(); index_cluster++) {
for (std::vector<unsigned int>::const_iterator index_object = (*analysed_clusters)[index_cluster]->begin(); index_object != (*analysed_clusters)[index_cluster]->end(); index_object++) {
sigma += euclidean_distance( &(*dataset)[*index_object], &(*analysed_centers)[index_cluster] );
}
N += (*analysed_clusters)[index_cluster]->size();
}
sigma /= (double) (N - K);
/* splitting criterion */
for (unsigned int index_cluster = 0; index_cluster < analysed_centers->size(); index_cluster++) {
double n = (double) (*analysed_clusters)[index_cluster]->size();
scores[index_cluster] = n * std::log(n) - n * std::log(N) - n * std::log(2.0 * pi()) / 2.0 - n * dimension * std::log(sigma) / 2.0 - (n - K) / 2.0;
}
double score = 0.0;
for (std::vector<double>::iterator cluster_score = scores.begin(); cluster_score != scores.end(); cluster_score++) {
score += (*cluster_score);
}
return score;
}
void generate_glass(struct universe *world, dubfloat_t radius, dubfloat_t U_threshold, dubfloat_t epsilon, dubfloat_t G){
/* loop variables */
int ii;
int jj;
/* distance computation variables */
dubfloat_t d;
/* pointers to state vectors */
dubfloat_t *r_in;
dubfloat_t *m_in;
/* total gravitational potential energy */
dubfloat_t U;
dubfloat_t rad;
dubfloat_t theta;
r_in=world->r;
m_in=world->m;
/* initial displacement */
r_in[3*0+0]=0.0;
r_in[3*0+1]=0.0;
r_in[3*0+2]=0.0;
for(ii=1;ii<world->num;ii++){
//printf("Generating random displacement for particle %d...\n", ii);
do{
/* generate random displacement */
rad=rejection_sampling();
theta=2.0*PI*(dubfloat_t)rand()/RAND_MAX;
r_in[3*ii+0]=rad*cos(theta);
r_in[3*ii+1]=rad*sin(theta);
r_in[3*ii+2]=50.0*boxmuller()*0.1;
//r_in[3*ii+0]=radius*boxmuller();
//r_in[3*ii+1]=radius*boxmuller();
//r_in[3*ii+2]=radius*boxmuller()*0.1;
//do{
//r_in[3*ii+0]=radius*(2.0*((dubfloat_t)rand()/RAND_MAX)-1.0);
//r_in[3*ii+1]=radius*(2.0*((dubfloat_t)rand()/RAND_MAX)-1.0);
//r_in[3*ii+2]=0.000001*radius*(2.0*((dubfloat_t)rand()/RAND_MAX)-1.0);
//}while(sqrt(r_in[3*ii+0]*r_in[3*ii+0]+
// r_in[3*ii+1]*r_in[3*ii+1]+
// r_in[3*ii+2]*r_in[3*ii+2])>radius);
/* calculate total gravitational energy for new displacement */
/* unit is AU^3/(M_solar*yr^2)*M_solar^2/AU */
U=0.0;
for(jj=0;jj<ii;jj++){
d=euclidean_distance(&r_in[ii*3],&r_in[jj*3],3);
d=sqrt(d*d+epsilon*epsilon);
U-=G*m_in[ii]*m_in[jj]*d;
}
//}while(U<U_threshold);
}while(0);
}
}
void move_enemies(GameState* state) {
register int i;
for (i = 0; i < state->enemies_length; i++) {
Enemy* en = &state->enemies[i].enemy;
if (en->alive) {
int to_x = convert_tile2world_x(en->path.nodes[en->path.current_node_index].tile_x);
int to_y = convert_tile2world_y(en->path.nodes[en->path.current_node_index].tile_y);
//@euclidean_distance: laat een kleine speling toe
if (euclidean_distance(en->world_x, en->world_y, to_x, to_y) < 2 && en->path.current_node_index > 0) {
//enemy heeft tile bereikt; verzet zijn doel naar de volgende node
en->path.current_node_index--;
}
calc_direction(en->world_x, en->world_y, to_x, to_y, &en->direction_x, &en->direction_y);
en->angle = find_render_angle(0, en->direction_x, en->direction_y);
en->world_x += en->speed * en->direction_x;
en->world_y += en->speed * en->direction_y;
if (en->world_x == state->world.castle->castle.world_x && en->world_y == state->world.castle->castle.world_y) {
//enemy heeft kasteel bereikt, trek levenspunten af en despawn enemy
state->world.castle->castle.health -= en->damage;
en->alive = 0;
if (state->world.castle->castle.health <= 0) {
//in geval dat health onder 0 zou staan
state->world.castle->castle.health = 0;
state->game_over = 1;
}
}
}
}
}
double xmeans::update_centers(std::vector<std::vector<unsigned int> *> * analysed_clusters, std::vector<std::vector<double> > * analysed_centers) {
double maximum_change = 0;
/* for each cluster */
for (unsigned int index_cluster = 0; index_cluster < analysed_clusters->size(); index_cluster++) {
std::vector<long double> total((*analysed_centers)[index_cluster].size(), 0);
/* for each object in cluster */
for (std::vector<unsigned int>::const_iterator object_index_iterator = (*analysed_clusters)[index_cluster]->begin(); object_index_iterator < (*analysed_clusters)[index_cluster]->end(); object_index_iterator++) {
/* for each dimension */
for (unsigned int dimension = 0; dimension < total.size(); dimension++) {
total[dimension] += (*dataset)[*object_index_iterator][dimension];
}
}
/* average for each dimension */
for (std::vector<long double>::iterator dimension_iterator = total.begin(); dimension_iterator != total.end(); dimension_iterator++) {
*dimension_iterator = *dimension_iterator / (*analysed_clusters)[index_cluster]->size();
}
#ifdef FAST_SOLUTION
double distance = euclidean_distance_sqrt( &(*analysed_centers)[index_cluster], (std::vector<double> *) &total );
#else
double distance = euclidean_distance( &(*analysed_centers)[index_cluster], (std::vector<double> *) &total );
#endif
if (distance > maximum_change) {
maximum_change = distance;
}
std::copy(total.begin(), total.end(), (*analysed_centers)[index_cluster].begin());
}
return maximum_change;
}
double evaluate(const Graph *g)
{
double result = 0.0;
double exclusiveRadius = 1.0;
for (Graph::vertices_const_iterator i = g->vertices.begin(); i != g->vertices.end(); ++i)
{
for (Graph::vertices_const_iterator j = i; j != g->vertices.end(); ++j)
{
double dist = euclidean_distance(i->second, j->second);
//std::cout << "Dist(" << i->second.number << "," << j->second.number << "): " << dist << std::endl;
// If vertices connected - they should be at specified distance
if (g->mtx[i->second.number][j->second.number])
result += fabs(3.0 - dist);
// Otherwise, they should be at another distance
else
result += fabs(6.0 - dist);
// Extra penalty for collisions
if (dist < 2 * exclusiveRadius)
result += 5 * (2 * exclusiveRadius - dist);
}
}
return result;
}
void kmeans::update_clusters(void) {
/* clear content of clusters. */
for (std::vector<std::vector<unsigned int> *>::iterator iter = clusters->begin(); iter != clusters->end(); iter++) {
(*iter)->clear();
}
/* fill clusters again in line with centers. */
for (unsigned int index_object = 0; index_object < dataset->size(); index_object++) {
double minimum_distance = std::numeric_limits<double>::max();
unsigned int suitable_index_cluster = 0;
for (unsigned int index_cluster = 0; index_cluster < clusters->size(); index_cluster++) {
#ifdef FAST_SOLUTION
double distance = euclidean_distance_sqrt( &(*centers)[index_cluster], &(*dataset)[index_object] );
#else
double distance = euclidean_distance( &(*centers)[index_cluster], &(*dataset)[index_object] );
#endif
if (distance < minimum_distance) {
minimum_distance = distance;
suitable_index_cluster = index_cluster;
}
}
(*clusters)[suitable_index_cluster]->push_back(index_object);
}
/* if there is clusters that are not able to capture objects */
for (size_t index_cluster = clusters->size() - 1; index_cluster != (size_t) -1; index_cluster--) {
if ((*clusters)[index_cluster]->empty()) {
clusters->erase(clusters->begin() + index_cluster);
}
}
}
int main(int argc, char *argv[])
{
/*Test if we can open a file to be parsed.*/
FILE *ip;
if (argc != 2)
{
printf("usage: ./02 <filename>\n");
exit(EXIT_FAILURE);
}
if (( ip = fopen(argv[1], "r")) == NULL)
{
printf("%s can't be opened.\n", argv[1]);
exit(EXIT_FAILURE);
}
FILE *op = fopen("./out.csv", "w");
/*Initialize a point and create a line buffer
* typedef struct point {long resn; float x; float y; float z; bool located; } point;
*/
char line[LINE_MAX];
point current;
point previous;
char field_one[6];
char field_three[4];
float measurement;
int count = 0;
while(fgets(line, LINE_MAX, ip) != (char *)0){
/*Get field values. Parse marker represents the a pointer to a index in the line buffer.*/
strncpy(field_one, line, 6);
char *parse_marker = &line[13];
strncpy(field_three, parse_marker, 3);
/*Confirm that field one is either a ATOM or a HETATM and field three is a CA.*/
if((strcmp(field_one, "ATOM ") == 0 \
|| strcmp(field_one, "HETATM") == 0 ) \
&& strncmp(field_three, "CA", 2) == 0){
count++;
/*If the first point is unset, the initialize it.*/
point current = { strtol(&line[23], &parse_marker, 10),
strtof(parse_marker, &parse_marker),
strtof(parse_marker, &parse_marker),
strtof(parse_marker, &parse_marker)};
/*Compute Euclidean distance between each point.*/
if (count % 2 == 0 ){
fprintf(op ,"%d, %d, %f\n", previous.resn, current.resn, euclidean_distance(current, previous) );
}
previous = current;
}
}
fclose(ip);
fclose(op);
return 0;
}
bool possible_goal(float rotation) {
const static Line line(Point(pitch_width, 0), Point(pitch_width, pitch_height));
Ray ray(Point(ball->x, ball->y), Vector(sin(rotation), cos(rotation)));
CGAL::Object result = intersection(line, ray);
Point p;
if(CGAL::assign(p, result)) {
if(p.y() < min_goal_y - ball_radius || p.y() > max_goal_y + ball_radius)
return false;
if(euclidean_distance(pitch_width, min_goal_y, p.x(), p.y()) < ball_radius)
return false;
if(euclidean_distance(pitch_width, max_goal_y, p.x(), p.y()) < ball_radius)
return false;
return true;
}
return false;
}
bool MapStepCostEstimator::getCost(const State& left_foot, const State& right_foot, const State& swing_foot, double& cost, double& cost_multiplier, double& risk, double& risk_multiplier) const
{
cost = 0.0;
cost_multiplier = 1.0;
risk = 0.0;
risk_multiplier = 1.0;
const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
double foot_dist = euclidean_distance(stand_foot.getX(), stand_foot.getY(), swing_foot.getX(), swing_foot.getY());
const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
double dist = 0.5*euclidean_distance(swing_foot_before.getX(), swing_foot_before.getY(), swing_foot.getX(), swing_foot.getY());
if (dist > 0.3 || foot_dist > 0.45)
{
risk = 1.0;
}
else
{
StepCostKey key(left_foot, right_foot, swing_foot, cell_size, angle_bin_size);
boost::unordered_map<StepCostKey, std::pair<double, double> >::const_iterator iter = cost_map.find(key);
if (iter == cost_map.end())
{
risk = 1.0;
}
else
{
risk = iter->second.second <= 0.25 ? abs(iter->second.first) : 1.0;
}
}
//std::vector<double> state;
//key.getState(state);
//ROS_INFO("%f %f %f | %f %f %f", swing_foot_before.getX(), swing_foot_before.getY(), swing_foot_before.getYaw(), swing_foot.getX(), swing_foot.getY(), swing_foot.getYaw());
//ROS_INFO("%f %f %f | %f %f %f", state[0], state[1], state[2], state[3], state[4], state[5]);
//ROS_INFO("------");
//if (risk_cost < 1.0 && state[5] > 0.0)
// ROS_INFO("Cost: %f + %f + %f Yaw: %f", dist, risk_cost, default_step_cost, state[5]);
cost = dist + 2*risk*risk;
}
void fill_euclidean_distances(float **matrix, int num_items,
const item_t items[])
{
for (int i = 0; i < num_items; ++i)
for (int j = i; j < num_items; ++j) {
matrix[i][j] =
euclidean_distance(&(items[i].coord),
&(items[j].coord));
matrix[j][i] = matrix[i][j];
}
}
void newproc( data_struct *data_in, data_struct *clusters ){
double sumOfDist;
double tmp_dist = 0;
int tmp_index = 0, i, j, k, count;
double min_dist = 0;
MPI_Request req;
double * tempCentroids;
tempCentroids = (double*)malloc( ( ( clusters->leading_dim + 1 )*clusters->secondary_dim + 1 )*sizeof(double));
MPI_Status status;
MPI_Recv( data_in->dataset, data_in->leading_dim * data_in->secondary_dim, MPI_DOUBLE, 0, 1, MPI_COMM_WORLD, &status );
int rank;
MPI_Comm_rank( MPI_COMM_WORLD, &rank );
while( 1 ){
tmp_dist = 0;
tmp_index = 0;
min_dist = 0;
MPI_Recv( clusters->dataset, clusters->leading_dim * clusters->secondary_dim, MPI_DOUBLE, 0, 2, MPI_COMM_WORLD, &status );
MPI_Get_count( &status, MPI_DOUBLE, &count );
if( count == 1 ){
break;
}
for( i = 0; i < clusters->secondary_dim; ++i ){
for( j = 0; j < clusters->leading_dim + 1; ++j ){
tempCentroids[ i * ( clusters->leading_dim + 1 ) + j ] = 0;
}
}
tempCentroids[ clusters->secondary_dim * ( clusters->leading_dim + 1 ) ] = 0;
for( i = 0; i<data_in->secondary_dim; i++){ //run through points
tmp_dist = 0;
tmp_index = 0;
min_dist = FLT_MAX;
//find nearest center
for(k=0; k<clusters->secondary_dim; k++){ //run through centroids
tmp_dist = euclidean_distance(data_in->dataset+i*data_in->leading_dim, clusters->dataset+k*clusters->leading_dim, data_in->leading_dim);
if(tmp_dist<min_dist){
min_dist = tmp_dist;
tmp_index = k; //num of centroid
}
}
data_in->members[i] = tmp_index; //num of centroid
tempCentroids[ ( clusters->leading_dim + 1 ) * clusters->secondary_dim ] += min_dist; //last field
tempCentroids[ tmp_index * ( clusters->leading_dim + 1 ) + clusters->leading_dim ]++; //last of every entry
for(j=0; j< clusters->leading_dim; j++){
tempCentroids[tmp_index * ( clusters->leading_dim + 1 ) + j] += data_in->dataset[i * data_in->leading_dim + j];
}
}
MPI_Isend( tempCentroids, ( clusters->leading_dim + 1 ) * clusters->secondary_dim + 1, MPI_DOUBLE, 0, 3, MPI_COMM_WORLD, &req );
}
MPI_Send( data_in->members, data_in->secondary_dim, MPI_UNSIGNED, 0, 4, MPI_COMM_WORLD );
}
typename enable_if<
typename gtl_and_3<
y_pt_eds,
typename is_point_concept<
typename geometry_concept<PointType1>::type
>::type,
typename is_point_concept<
typename geometry_concept<PointType2>::type
>::type
>::type,
typename point_difference_type<PointType1>::type>::type
distance_squared(const PointType1& point1, const PointType2& point2) {
typename point_difference_type<PointType1>::type dx =
euclidean_distance(point1, point2, HORIZONTAL);
typename point_difference_type<PointType1>::type dy =
euclidean_distance(point1, point2, VERTICAL);
dx *= dx;
dy *= dy;
return dx + dy;
}
// ----------------------------------------------------------------------
void
LocalizationNeighborhood::
reassign_twins( double twin_measure )
throw()
{
// main task of this cycle is to unset all twins in the neighborhood.
// additionally it is checked, whether the neighbor is a twin of the
// source node or not.
for ( NeighborhoodIterator
it = begin_neighborhood_w();
it != end_neighborhood_w();
++it )
if ( euclidean_distance(
source().est_position(), it->second->node().est_position() )
<= twin_measure )
it->second->set_twin( true );
else
it->second->set_twin( false );
// this cycle sorts out all neighbors that are twins to each other, so
// that after this check there is only one neighbor on one position.
for ( NeighborhoodIterator
it1 = begin_neighborhood_w();
it1 != end_neighborhood_w();
++it1 )
for ( NeighborhoodIterator
it2 = it1;
it2 != end_neighborhood_w();
++it2 )
{
if ( it1 == it2 )
continue;
if ( !it1->second->has_pos() || !it2->second->has_pos() )
continue;
if ( euclidean_distance( it1->second->pos(), it2->second->pos() )
<= twin_measure )
it2->second->set_twin( true );
}
}
void pdist_euclidean(const double *X, double *dm, int m, int n) {
int i, j;
const double *u, *v;
double *it = dm;
for (i = 0; i < m; i++) {
for (j = i + 1; j < m; j++, it++) {
u = X + (n * i);
v = X + (n * j);
*it = euclidean_distance(u, v, n);
}
}
}
void cdist_euclidean(const double *XA,
const double *XB, double *dm, int mA, int mB, int n) {
int i, j;
const double *u, *v;
double *it = dm;
for (i = 0; i < mA; i++) {
for (j = 0; j < mB; j++, it++) {
u = XA + (n * i);
v = XB + (n * j);
*it = euclidean_distance(u, v, n);
}
}
}
double ghost_cost(loc ghost_location, loc amidar_loc) {
if (ghost_location.first < 20 || ghost_location.first > 160) {
// cout << "here1" << endl;
return GHOST_COST / pow(manhattan_distance(ghost_location, amidar_loc), 2);
}
else if (ghost_location.second < 20 || ghost_location.second > 138) {
// cout << "here2" << endl;
// cout << ghost_location.first << ", " << ghost_location.second << endl;
// cout << amidar_loc.first << ", " << amidar_loc.second << endl;
return GHOST_COST / pow(manhattan_distance(ghost_location, amidar_loc), 2);
}
else return GHOST_COST / pow(euclidean_distance(ghost_location, amidar_loc), 2);
}
double euclidean_distance_lab(CvScalar p_1, CvScalar p_2)
{
// convert to Lab for better perceptual distance
IplImage * convert = cvCreateImage( cvSize(2,1), 8, 3);
cvSet2D(convert,0,0,p_1);
cvSet2D(convert,0,1,p_2);
cvCvtColor(convert,convert,CV_BGR2Lab);
p_1 = cvGet2D(convert,0,0);
p_2 = cvGet2D(convert,0,1);
cvReleaseImage(&convert);
return euclidean_distance(p_1, p_2);
}
void kmeans_process(data_struct *data_in, data_struct *clusters, double *newCentroids, double* SumOfDist, double *sse) {
int i, j, k;
double tmp_dist = 0;
int tmp_index = 0;
double min_dist = 0;
double *dataset = data_in->dataset;
double *centroids = clusters->dataset;
unsigned int *Index = data_in->members;
unsigned int *cluster_size = clusters->members;
//SumOfDist[0] = 0;
for(i=0; i<clusters->secondary_dim; i++) {
cluster_size[i] = 0;
}
for(i=0; i<data_in->secondary_dim; i++) {
tmp_dist = 0;
tmp_index = 0;
min_dist = FLT_MAX;
/*find nearest center*/
for(k=0; k<clusters->secondary_dim; k++) {
tmp_dist = euclidean_distance(dataset+i*data_in->leading_dim, centroids+k*clusters->leading_dim, data_in->leading_dim);
if(tmp_dist<min_dist) {
min_dist = tmp_dist;
tmp_index = k;
}
}
Index[i] = tmp_index;
SumOfDist[0] += min_dist;
sse[0] += min_dist*min_dist;
cluster_size[tmp_index]++;
for(j=0; j<data_in->leading_dim; j++) {
newCentroids[tmp_index * clusters->leading_dim + j] += dataset[i * data_in->leading_dim + j];
}
}
/*update cluster centers*/
for(k=0; k<clusters->secondary_dim; k++) {
for(j=0; j<data_in->leading_dim; j++) {
centroids[k * clusters->leading_dim + j] = newCentroids[k * clusters->leading_dim + j];
}
}
}
// ----------------------------------------------------------------------
void
SimulationTaskLocalizationEvaluation::Results::
collect_information( SimulationController& sc, const SimulationTaskLocalizationEvaluation& stle )
throw()
{
LocalizationObserver observer;
if ( sc.world().communication_model().exists_communication_upper_bound() )
comm_range = sc.world().communication_model().communication_upper_bound();
set_seed( sc, stle );
for( World::node_iterator
it = sc.world_w().begin_nodes_w();
it != sc.world_w().end_nodes_w();
++it )
{
Node& v = *it;
LocalizationProcessor* lp =
v.get_processor_of_type_w<LocalizationProcessor>();
if ( lp == NULL || lp->is_anchor() ) continue;
observer.set_owner( *lp );
if ( v.has_est_position() )
{
++has_pos_cnt;
double distance = euclidean_distance( v.real_position(), v.est_position() );
stat_abs_position += distance;
}
else
{
++no_pos_cnt;
}
stat_known_anchors += observer.anchor_cnt();
stat_valid_known_anchors += observer.valid_anchor_cnt();
observer.fill_stat_rel_anchor_distance(
stat_real_err_anchor_dist,
stat_comm_err_anchor_dist );
observer.fill_stat_rel_neighbor_distance(
stat_real_neighbor_dist,
stat_comm_neighbor_dist );
stat_neighbor_cnt += (double)observer.neighbor_cnt();
}// for all nodes
}
std::pair<Point, Point> closest_pair_of_points(InputIterator begin,
InputIterator end) {
if (std::distance(begin, end) < 2) return std::pair<Point, Point>();
std::vector<Point> points(begin, end);
std::sort(points.begin(), points.end());
std::vector<Point>::const_iterator tail = points.begin();
std::vector<Point>::const_iterator head = points.begin() + 1;
double minimum_distance = euclidean_distance(*tail, *head);
std::pair<Point, Point> best_pair = std::make_pair(*tail, *head);
std::set<Point, VerticalPointComparator> candidates(tail, head);
// Add the current head to the candidate set and select new head.
candidates.insert(*head++);
while (head != points.end()) {
// Remove candidates that are too far horizontally.
while ((head->x - tail->x) > minimum_distance) {
candidates.erase(*tail++);
}
// Select all candidates that are not too far vertically.
Point minimum_y(head->x, head->y - minimum_distance);
Point maximum_y(head->x, head->y + minimum_distance);
auto candidates_begin = candidates.lower_bound(minimum_y);
auto candidates_end = candidates.upper_bound(maximum_y);
// Compute all distances.
while (candidates_begin != candidates_end) {
double distance = euclidean_distance(*candidates_begin, *head);
if (distance < minimum_distance) {
minimum_distance = distance;
best_pair = std::make_pair(*candidates_begin, *head);
}
candidates_begin++;
}
// Add the current head to the candidate set and select new head.
candidates.insert(*head++);
}
return best_pair;
}
