/**
* Merge point cloud in the internal model
* with the sensor to world transformation,
* and slide, save, load tiles.
*
* @param cloud: point cloud in the sensor frame
* @param transformation: sensor to world transformation
*/
void atlaas::merge(points& cloud, const matrix& transformation,
const covmat& covariance, bool dump) {
if (cloud.size() < 1)
return; // pcl writeBinaryCompressed crash with empty cloud
if (dump) {
save_inc(cloud, transformation);
if (reprocess_in_progress)
return;
}
sensor_xy = matrix_to_point(transformation);
// slide map while needed
do_slide();
// use dynamic merge
// clear the dynamic map (zeros)
cell_info_t zeros{}; // value-initialization w/empty initializer
std::fill(dyninter.begin(), dyninter.end(), zeros);
// transform the cloud from sensor to custom frame
transform(cloud, transformation);
// merge the point-cloud
rasterize(cloud, dyninter);
// merge the dynamic atlaas with internal data
merge();
}
void convex_hull(points &a, points &res) {
res.resize(2 * a.size() + 10);
sort(a.begin(), a.end(), cmpxy);
a.erase(unique(a.begin(), a.end()), a.end());
int m = 0;
for (int i = 0; i < (int)a.size(); i ++) {
while(m > 1 && dcmp(det(res[m - 1] - res[m - 2], a[i] - res[m - 2])) <= 0) --m;
res[m++] = a[i];
}
int k = m;
for (int i = (int)a.size() - 2; i >= 0; i --) {
while(m > k && dcmp(det(res[m - 1] - res[m - 2], a[i] - res[m - 2])) <= 0) --m;
res[m++] = a[i];
}
res.resize(m);
//if (a.size() > 1) res.resize(m - 1);
}
void cut(points &p, point b, point a, points &res) {
res.clear();
int n = p.size();
for (int i = 0; i < n; i ++) {
point c = p[i];
point d = p[next(i)];
if (dcmp(det(b - a, c - a)) >= 0) res.push_back(c);
if (dcmp(det(b - a, c - d)) != 0) {
point cp = interpoint(line(a, b), line(c, d));
if (onseg(cp, c, d)) res.push_back(cp);
}
}
}
points convexHull (points a)
{
points up,dw;
sort (a.begin(), a.end());
for (int i=0;i<a.size();i++)
{
while (up.size()>=2 && ccw(*++up.rbegin(),*up.rbegin(),a[i])>=0) up.pop_back();
while (dw.size()>=2 && ccw(*++dw.rbegin(),*dw.rbegin(),a[i])<=0) dw.pop_back();
up.emplace_back(a[i]);
dw.emplace_back(a[i]);
}
up.insert (up.end(), ++dw.rbegin(), --dw.rend());
return up;
}
int main(int argc, char *argv[])
{
if (argc != 4)
{
std::cout << usage << std::endl;
return -1;
}
std::string line;
std::ifstream input(argv[1], std::ifstream::in);
std::getline(input, line);
std::istringstream seiss(line);
double sx, sy, ex, ey;
seiss >> sx >> sy >> ex >> ey;
point start(sx, sy), end(ex, ey);
std::getline(input, line);
std::istringstream niss(line);
int count = 0;
niss >> count;
for (int i = 0; i < count; i++)
{
std::getline(input, line);
std::istringstream iss(line);
double x, y;
iss >> x >> y;
polygon_points.push_back(point(x, y));
}
int vertices_count = polygon_points.size();
int triangles_count = polygon_points.size() - 2;
double (*vertices)[2] = new double[vertices_count + 1][2];
int (*triangles)[3] = new int[triangles_count][3];
for (int i = 1; i <= polygon_points.size(); i++)
{
vertices[i][0] = polygon_points[i - 1].x;
vertices[i][1] = polygon_points[i - 1].y;
}
int ncontours = 1;
int cntr[1] = {polygon_points.size()};
triangulate_polygon(ncontours, cntr, vertices, triangles);
for (int i = 0; i < triangles_count; i++)
{
triangles[i][0]--;
triangles[i][1]--;
triangles[i][2]--;
}
std::ofstream output(argv[2], std::ofstream::out);
int start_tri = locate_point(triangles, triangles_count, start);
int end_tri = locate_point(triangles, triangles_count, end);
bool distance = false;
if (strcmp(argv[3], "path") == 0)
distance = false;
else if (strcmp(argv[3], "distance") == 0)
distance = true;
if (start_tri == end_tri)
{
if (distance)
output << (int)ceil(dist(start, end)) << std::endl;
else
{
output << "s" << std::endl;
output << "e" << std::endl;
}
}
else
{
std::vector<int> triangles_list = dfs(triangles, triangles_count, start_tri, end_tri);
std::vector<diagnal> d_list = diagnals_list(triangles, triangles_list);
path shortest_path = simple_stupid_funnel(d_list, start, end);
if (distance)
{
double total_dist = 0;
for (int i = 0; i < shortest_path.size(); i++)
{
point last;
point current = polygon_points[shortest_path[i]];
if (i == 0)
last = start;
else
last = polygon_points[shortest_path[i - 1]];
total_dist += dist(last, current);
}
if (shortest_path.size() > 0)
total_dist += dist(polygon_points[shortest_path[shortest_path.size() - 1]], end);
else
total_dist += dist(start, end);
output << (int)ceil(total_dist) << std::endl;
}
else
{
output << "s" << std::endl;
for (int i = 0; i < shortest_path.size(); i++)
output << shortest_path[i] << std::endl;
output << "e" << std::endl;
}
}
delete[] vertices;
delete[] triangles;
return 0;
}
double area(points &p) {
double z = 0;
for (int i = 0; i < (int)p.size() - 1; i ++)
z += det(p[i] - p[0], p[i + 1] - p[0]);
return fabs(z) / 2;
}