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