int main()
{
int holes, boo, i, escape;
point gopher, dog, hole;
boo = scanf("%d ", &holes);
while (boo != EOF)
{
scanf("%lf %lf %lf %lf\n", &gopher.x, &gopher.y, &dog.x, &dog.y);
escape = 0;
for (i = 0; i < holes; i++)
{
scanf("%lf %lf", &hole.x, &hole.y);
if (escape == 1) continue;
if (pDistance(dog, hole) / 2.0 >= pDistance(gopher, hole))
{
escape = 1;
printf("The gopher can escape through the hole at (%.3f,%.3f).\n", hole.x, hole.y);
}
}
if (escape == 0)
{
printf("The gopher cannot escape.\n");
}
boo = scanf("\n%d", &holes);
}
return 0;
}
void pclGetOutliers(PCLPointCloud::Ptr inputCloud, PCLPointCloud::Ptr virtualCloud, PCLPointCloud::Ptr outliers, double threshold)
{
std::vector<int> pIdxSearch(1);
std::vector<float> pDistance(1);
outliers->points.clear();
pcl::KdTreeFLANN<pcl::PointXYZ> kd;
kd.setInputCloud(virtualCloud);
for (uint i=0; i<inputCloud->points.size(); i++)
{
if (kd.nearestKSearch(inputCloud->points[i], 1, pIdxSearch, pDistance) > 0)
{
if (pDistance[0] > threshold)
{
outliers->points.push_back(inputCloud->points[i]);
}
}
}
outliers->width = outliers->points.size();
outliers->height = 1;
outliers->is_dense = false;
return;
}
Error pInverseSquareDistance(Particle *p, Particle *q, unsigned int numDimensions, double *invSqrDistance)
{
pDistance(p, q, numDimensions, invSqrDistance);
if (*invSqrDistance == 0.0)
{
return DIVIDE_BY_ZERO_ERROR;
}
*invSqrDistance = 1.0 / (*invSqrDistance * *invSqrDistance);
return NO_ERROR;
}
void CloudParticle::computeWeight(const CloudPFInputData &data)
{
std::vector<int> pIdxSearch(1);
std::vector<float> pDistance(1);
double sum = 0;
particleError = 0;
for (uint i=0; i<data.cloud_moves->points.size(); i++)
{
const QVec p_rc = transformation*QVec::vec4(data.cloud_moves->points[i].x,
data.cloud_moves->points[i].y,
data.cloud_moves->points[i].z, 1);
const pcl::PointXYZ p_pcl(p_rc(0), p_rc(1), p_rc(2));
if (data.kdtree->nearestKSearch(p_pcl, 1, pIdxSearch, pDistance) > 0)
{
const float d = pDistance[0];
// particleError += d*d;
if (d > DISTANCE_THRESHOLD)
particleError+=1;
if (d < PARTICLE_OPTIMIZATION_MAX)
{
if (d > PARTICLE_OPTIMIZATION_MIN)
{
if (DISTANCE_FUNCTION == 0) /// BINARY
{
if (d > DISTANCE_THRESHOLD)
sum+=1;
}
else if (DISTANCE_FUNCTION == 1) /// Exponential
{
sum+= 1./(pow(10, (d/100.))+0.00001);
}
else if (DISTANCE_FUNCTION == 2) /// Simple
{
sum+= 1./(d+0.00001);
}
else if (DISTANCE_FUNCTION == 3) /// Potential
{
sum+= 1./((d*d)+0.00001);
}
}
}
}
}
this->weight = sum;
// sum /= data.cloud_moves->points.size();
// this->weight = 1./(sum+0.00000000000000000001);
}
void Car::setSpeeds(const Band &b)
{
float sym = (currentPoint.y > lastPoint.y) ? -1.0f : +1.0f;
float d = pDistance(currentPoint, lastPoint);
current_speed = (((((d / b.getConfine().getPixelsPerMeter(currentPoint.y)) +
(d / b.getConfine().getPixelsPerMeter(lastPoint.y))) / 2) * fps) * sym) * 3.6f; // set to Km/h
if (!first_time)
{
if (current_speed > max_speed)
max_speed = current_speed;
if (current_speed < min_speed)
min_speed = current_speed;
}
else
first_time = 0;
}
