void RainingProcess(SearchSpace *s, int *flow) { int i, j; double tmp, rand, dist; for (i = 1; i < s->nsr + 1; i++) { rand = GenerateUniformRandomNumber(0, 1); dist = EuclideanDistance(s->a[0]->x, s->a[i]->x, s->n); /* It obtains the euclidean distance for further use */ if ((dist < s->dmax) || (rand < 0.1)) { DestroyAgent(&(s->a[i]), _WCA_); s->a[i] = GenerateNewAgent(s, _WCA_); } } for (i = s->nsr + 1; i < flow[0]; i++) { rand = GenerateUniformRandomNumber(0, 1); dist = EuclideanDistance(s->a[0]->x, s->a[i]->x, s->n); /* It obtains the euclidean distance for further use */ if ((dist < s->dmax) || (rand < 0.1)) { DestroyAgent(&(s->a[i]), _WCA_); s->a[i] = GenerateNewAgent(s, _WCA_); } } }
cost AnyAngleAlgorithm::SmoothPath(const std::vector<xyLocCont> & path, std::vector<xyLocCont> &smoothed_path) { smoothed_path.clear(); if (path.empty()) return INFINITE_COST; // Add the start. smoothed_path.push_back(path[0]); // Go over all locations on the original path, in order. for (unsigned int i = 1; i < path.size(); i++) { if (!LineOfSight(smoothed_path.back(), path[i])) // If there is no line of sight to the last location that is added to the smoothed path.. { smoothed_path.push_back(path[i - 1]); // ..add the i-1st location on the original path to the smoothed path. } } // Add the goal. if (fabs(smoothed_path.back().x - path.back().x) > EPSILON || fabs(smoothed_path.back().y - path.back().y) > EPSILON) smoothed_path.push_back(path.back()); // Compute path cost. cost c = 0; for (unsigned int i = 1; i < smoothed_path.size(); i++) c += EuclideanDistance(smoothed_path[i-1], smoothed_path[i]); return c; }
cost AnyAngleAlgorithm::EvaluatePath(const std::vector<CornerLoc> & path, const bool validate_path) { int num_los_checks_so_far = num_los_checks_; // Do not include the line-of-sight check for path validation in statistics. bool valid_path = true; if (validate_path) { if (path.size() == 0) { valid_path = false; } else { // First and last locations on the path should be start and goal if (path[0].x != from_.x || path[0].y != from_.y) valid_path = false; if (path.back().x != to_.x || path.back().y != to_.y) valid_path = false; } } // Compute the path cost and validate the path. cost c = 0; for (unsigned int i = 0; i + 1 < path.size(); i++) { if (validate_path && !LineOfSight(path[i], path[i + 1])) valid_path = false; c += EuclideanDistance(path[i], path[i + 1]); } num_los_checks_ = num_los_checks_so_far; // Do not include the line-of-sight check for path validation in statistics. // Count the number of heading changes num_heading_changes_ = 0; num_freespace_heading_changes_ = 0; num_taut_corner_heading_changes_ = 0; num_non_taut_corner_heading_changes_ = 0; for (unsigned int i = 1; i + 1 < path.size(); i++) { if (!CoLinear(path[i - 1].x, path[i - 1].y, path[i].x, path[i].y, path[i + 1].x, path[i + 1].y)) { num_heading_changes_++; // If the turning point is not at a convex corner of an obstacle if (!IsConvexCorner(path[i])) num_freespace_heading_changes_++; // If the turn at the convex corner of an obstacle produces a taut path else if (IsTautCornerTurn(path[i - 1].x, path[i - 1].y, path[i].x, path[i].y, path[i + 1].x, path[i + 1].y)) num_taut_corner_heading_changes_++; // If the turn at the convex corner of an obstacle does not produce a taut path else num_non_taut_corner_heading_changes_++; } } if (!valid_path) return INFINITE_COST; else return c; }
double EuclidDistHeuristic::getMetricGoalDistance(double x, double y, double z) { auto& goal_pose = planningSpace()->goal().pose; return EuclideanDistance( x, y, z, goal_pose.translation()[0], goal_pose.translation()[1], goal_pose.translation()[2]); }
int Utility::NearestNeighborhoodPoint(const Point pointSet[], const int pointSetSize, const Point homePoint) { int Neighborhood = 0; double currentShortestDistance; if(pointSet[0] != homePoint) currentShortestDistance = EuclideanDistance(pointSet[0], homePoint); else currentShortestDistance = EuclideanDistance(pointSet[1], homePoint); for(int i = 1; i < pointSetSize; i++) { if(pointSet[i] == homePoint) continue; if(EuclideanDistance(pointSet[i], homePoint) < currentShortestDistance) { Neighborhood = i; currentShortestDistance = EuclideanDistance(pointSet[i], homePoint); } } return Neighborhood; }
void Entity::moveTowards(Position newPos, float speed) { double dist = EuclideanDistance(position, newPos); float dx = newPos.x - position.x; float dy = newPos.y - position.y; dist > 0 ? dx /= dist : dx; dist > 0 ? dy /= dist : dy; dx *= speed; dy *= speed; position.x += dx; position.y += dy; }
void ArclengthTable::BuildTable() { indices[0] = 0; parametricEntries[0] = 0.0f; arclengths[0] = 0.0f; float parametricStep = 1.0 / points->size(); float distanceToNextPoint; int i; for (i = 1; i < points->size(); i++) { indices[i] = i; parametricEntries[i] = i * parametricStep; distanceToNextPoint = EuclideanDistance(GetPoint(i - 1), GetPoint(i)); arclengths[i] = arclengths[i - 1] + distanceToNextPoint; } Normalize(arclengths); }
float AStar::HeuristicDistance(Vector StartPos, Vector EndPos) { // Seems like HEURISTIC_EUCLIDEAN == HEURISTIC_DISTANCE if(Heuristic == HEURISTIC_EUCLIDEAN) { return EuclideanDistance(StartPos, EndPos); } else if(Heuristic == HEURISTIC_DISTANCE) { return (StartPos - EndPos).Length(); } else if(Heuristic == HEURISTIC_CUSTOM) { //gLua->PushReference(HeuristicRef); //GMOD_PushVector(StartPos); //GMOD_PushVector(EndPos); //gLua->Call(2, 1); //return gLua->GetNumber(1); } return ManhattanDistance(StartPos, EndPos); }
/* It clusters the agents and returns a pointer with the best agent's ID per cluster. Parameters: s: search space best_ideas: pointer to the ids of the best ideas per cluster (k-sized array) ideas_per_cluster: pointer to the ids of the ideas per cluster (k x (Y_i)+1-sized array) where Y_i stands for the number of ideas in cluster i. Notice we have one more column (the first one) that stores the number of ideas that belongs to cluster i. */ void k_means(SearchSpace *s, int *best_ideas, int ***ideas_per_cluster) { int *nearest = NULL, *ctr = NULL, i, j, r; char *is_chosen = NULL, OK; double **center = NULL, old_error, error = DBL_MAX, min_distance, distance; double **center_mean = NULL, *best_fitness_cluster = NULL; if ((!s) || (!best_ideas) || (!ideas_per_cluster)) { fprintf(stderr, "\nSearch space and/or input arrays not allocated @k_means.\n"); exit(-1); } best_fitness_cluster = (double *)malloc(s->k * sizeof(double)); nearest = (int *)malloc(s->m * sizeof(int)); ctr = (int *)calloc(s->k, sizeof(int)); center = (double **)malloc(s->k * sizeof(double *)); center_mean = (double **)malloc(s->k * sizeof(double *)); is_chosen = (char *)calloc(s->m, sizeof(char)); /* initializing k centers with samples choosen at random */ for (i = 0; i < s->k; i++) { center[i] = (double *)malloc(s->n * sizeof(double)); center_mean[i] = (double *)calloc(s->n, sizeof(double)); OK = j = 0; do { j++; r = (int)GenerateUniformRandomNumber(0, s->m); if (!is_chosen[r]) { is_chosen[r] = 1; OK = 1; } } while ((!OK) && (j <= s->m)); if (j > s->m) { fprintf(stderr, "\nProblems initializing the k centers @k_means. Probably k is too large.\n"); exit(-1); } for (j = 0; j < s->n; j++) center[i][j] = s->a[r]->x[j]; } /* main algorithm */ do { old_error = error; error = 0; for (i = 0; i < s->k; i++) { ctr[i] = 0; for (j = 0; j < s->n; j++) center_mean[i][j] = 0; } /* for each idea, it finds the nearest center */ for (i = 0; i < s->m; i++) { min_distance = DBL_MAX; for (j = 0; j < s->k; j++) { distance = EuclideanDistance(s->a[i]->x, center[j], s->n); if (distance < min_distance) { nearest[i] = j; min_distance = distance; } } /* after finding the nearest center, we accumulate the idea's coordinates at this center */ for (j = 0; j < s->n; j++) center_mean[nearest[i]][j] += s->a[i]->x[j]; ctr[nearest[i]]++; error += min_distance; } error /= s->m; /* updating the centers */ for (i = 0; i < s->k; i++) { for (j = 0; j < s->n; j++) { if (ctr[i]) center[i][j] = center_mean[i][j] / ctr[i]; else center[i][j] = center_mean[i][j]; } } } while (fabs(error - old_error) > 1e-5); /* identifying the best idea (smallest fitness) per cluster */ for (i = 0; i < s->k; i++) { best_fitness_cluster[i] = DBL_MAX; /* it allocates an array to store the ids of the ideas of cluster i. The first position stores the number of ideas clustered in cluster i. */ (*ideas_per_cluster)[i] = (int *)calloc((ctr[i] + 1), sizeof(int)); } for (i = 0; i < s->m; i++) { (*ideas_per_cluster)[nearest[i]][ctr[nearest[i]]] = i; ctr[nearest[i]]--; if (s->a[i]->fit < best_fitness_cluster[nearest[i]]) { best_fitness_cluster[nearest[i]] = s->a[i]->fit; best_ideas[nearest[i]] = i; } } /*************************************************************/ for (i = 0; i < s->k; i++) { free(center[i]); free(center_mean[i]); } free(center); free(center_mean); free(is_chosen); free(nearest); free(ctr); free(best_fitness_cluster); }
bool IsSamePoint(const CXPoint& A, const CXPoint& B) { return EuclideanDistance(A, B) < eDistance; }
//-------------------------------------------------------------- void testApp::update(){ for(int i=0;i<availableResources.size();i++) { //pParticle->addAttractionForce(availableResources[i]->getPos(), 500,100.9); } for(int i=0;i<repulsors.size();i++) { pParticle->addRepulsionForce(repulsors[i]->getPos(), 50,10.9); } if(found) { float dist = 1000000.0f; int n = 0; for(int i=0;i<availableResources.size();i++) { float current_dist = EuclideanDistance(availableResources[i]->getPos(), originResource->getPos()); if(dist>current_dist) { dist = current_dist; n = i; } } targetResource = availableResources[n]; availableResources.erase(availableResources.begin()+n); found = false; } //pParticle->addAttractionForce(availableResources[i]->getPos(), 500,100.9); //pParticle->addDampingForce(); //pParticle->addAttractionForce(ofPoint(200,200), 500,1.10); //pParticle->update(); path.setup(originResource->getPos().x, originResource->getPos().y, pParticle->pos.x,pParticle->pos.y ,ofColor(100, 100, 100, 25),6, 70, 0.04); path.parse(); pParticle->particleToPoint(targetResource->getPos()); float dist = EuclideanDistance(targetResource->getPos(), pParticle->pos); if(dist<0.1) { originResource = targetResource; found = true; if(availableResources.empty()) { for(int i=0;i<resources.size();i++) { if (resources[i]!=originResource) { availableResources.push_back(resources[i]); } } } } // for(int i=0;i<availableResources.size();i++) // { // float dist = EuclideanDistance(availableResources[i]->getPos(), pParticle->pos); // if (dist<0.2) { // availableResources.erase(availableResources.begin()+i); // originResource = availableResources[i]; // // } // } }
/* It executes the Black Hole Algorithm for function minimization Parameters: s: search space Evaluate: pointer to the function used to evaluate agents arg: list of additional arguments */ void runBHA(SearchSpace *s, prtFun Evaluate, ...) { va_list arg, argtmp; int t, i, j; double fitValue, sum, rand, dist, radius; Agent *tmp = NULL; va_start(arg, Evaluate); va_copy(argtmp, arg); if (!s) { fprintf(stderr, "\nSearch space not allocated @runBHA.\n"); exit(-1); } EvaluateSearchSpace(s, _BHA_, Evaluate, arg); /* Initial evaluation of the search space */ for (t = 1; t <= s->iterations; t++) { fprintf(stderr, "\nRunning iteration %d/%d ... ", t, s->iterations); sum = 0; /* Changing the position of each star according to Equation 3 */ for (i = 0; i < s->m; i++) { va_copy(arg, argtmp); rand = GenerateUniformRandomNumber(0, 1); for (j = 0; j < s->n; j++) s->a[i]->x[j] += rand * (s->g[j] - s->a[i]->x[j]); CheckAgentLimits(s, s->a[i]); s->a[i]->fit = Evaluate(s->a[i], arg); /* It executes the fitness function for agent i */ tmp = CopyAgent(s->a[i], _BHA_, _NOTENSOR_); if (s->a[i]->fit < s->gfit) { fitValue = s->gfit; s->gfit = s->a[i]->fit; s->a[i]->fit = fitValue; for (j = 0; j < s->n; j++) { tmp->x[j] = s->g[j]; s->g[j] = s->a[i]->x[j]; s->a[i]->x[j] = tmp->x[j]; } } DestroyAgent(&tmp, _BHA_); sum = sum + s->a[i]->fit; } va_copy(arg, argtmp); /* Event Horizon and evaluating the solutions */ radius = s->gfit / sum; for (i = 0; i < s->m; i++) { dist = EuclideanDistance(s->g, s->a[i]->x, s->n); /* It obtains the euclidean distance */ if (dist < radius) { DestroyAgent(&(s->a[i]), _BHA_); s->a[i] = GenerateNewAgent(s, _BHA_); } } EvaluateSearchSpace(s, _BHA_, Evaluate, arg); fprintf(stderr, "OK (minimum fitness value %lf)", s->gfit); } va_end(arg); }