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