369 lines
9.9 KiB
Text
369 lines
9.9 KiB
Text
|
/**
|
||
|
|
* @file graphicsAlg.cc
|
||
|
|
*
|
||
|
|
* @auhtor Remus Claudiu Dumitru <r.dumitru@jacobs-university.de>
|
||
|
|
* @date 18 Feb 2012
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
//==============================================================================
|
||
|
|
// Includes
|
||
|
|
//==============================================================================
|
||
|
|
#include "model/graphicsAlg.h"
|
||
|
|
|
||
|
|
#include "model/scene.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include <algorithm>
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
|
||
|
|
//==============================================================================
|
||
|
|
// Global Variables
|
||
|
|
//==============================================================================
|
||
|
|
extern bool quiet;
|
||
|
|
|
||
|
|
template <class InputIterator, class OutputIterator>
|
||
|
|
void model::GraphicsAlg::alpha_edges(InputIterator begin, InputIterator end,
|
||
|
|
const Type& Alpha,
|
||
|
|
bool mode,
|
||
|
|
OutputIterator out)
|
||
|
|
{
|
||
|
|
Alpha_shape_2 shape(begin, end);
|
||
|
|
|
||
|
|
if (mode) {
|
||
|
|
shape.set_mode(Alpha_shape_2::GENERAL);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
shape.set_mode(Alpha_shape_2::REGULARIZED);
|
||
|
|
}
|
||
|
|
|
||
|
|
shape.set_alpha(Alpha);
|
||
|
|
|
||
|
|
for(Alpha_shape_edges_iterator it = shape.alpha_shape_edges_begin();
|
||
|
|
it != shape.alpha_shape_edges_end(); ++it)
|
||
|
|
{
|
||
|
|
*out++ = shape.segment(*it);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// to be used to sort the pairs in the clockwiseSort method
|
||
|
|
bool compare(pair<double, unsigned int> left, pair<double, unsigned int> right) {
|
||
|
|
// we want to order clockwise, so we should compare the other way around
|
||
|
|
return left.first > right.first;
|
||
|
|
}
|
||
|
|
|
||
|
|
void model::GraphicsAlg::clockwiseSort(vector<Plane3d>& planes) {
|
||
|
|
if (planes.size() <= 2) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// to be returned at the end
|
||
|
|
vector<Plane3d> result;
|
||
|
|
|
||
|
|
// first compute the center of gravity of the given planes
|
||
|
|
Point3d cog;
|
||
|
|
for (unsigned int i = 0; i < planes.size(); ++i) {
|
||
|
|
cog += planes[i].pt;
|
||
|
|
}
|
||
|
|
cog /= planes.size();
|
||
|
|
|
||
|
|
// keep the angle of each plane wrt to the center of gravity
|
||
|
|
// also keep and index with the position of the plane in the original vector
|
||
|
|
vector<pair<double, unsigned int> > angles;
|
||
|
|
|
||
|
|
// compute each angle and add it to the angles vector alongside the index
|
||
|
|
for (unsigned int i = 0; i < planes.size(); ++i) {
|
||
|
|
double angle = atan2(planes[i].pt.z - cog.z, planes[i].pt.x - cog.x);
|
||
|
|
angles.push_back(make_pair(angle, i));
|
||
|
|
}
|
||
|
|
|
||
|
|
// use stl sort
|
||
|
|
sort(angles.begin(), angles.end(), compare);
|
||
|
|
|
||
|
|
// construct the result
|
||
|
|
for (unsigned int i = 0; i < angles.size(); ++i) {
|
||
|
|
result.push_back(planes[angles[i].second]);
|
||
|
|
}
|
||
|
|
|
||
|
|
// copy result into planes
|
||
|
|
planes = result;
|
||
|
|
}
|
||
|
|
|
||
|
|
vector<model::Plane3d> model::GraphicsAlg::getConcaveHull(std::vector<model::Plane3d> planes) {
|
||
|
|
if (planes.size() <= 3) {
|
||
|
|
return planes;
|
||
|
|
}
|
||
|
|
|
||
|
|
const double eps = 0.1;
|
||
|
|
vector<Plane3d> result;
|
||
|
|
vector<Plane3d> verticalPlanes;
|
||
|
|
|
||
|
|
// CGAL datatypes
|
||
|
|
list<Point> points;
|
||
|
|
vector<Segment> segments;
|
||
|
|
|
||
|
|
// keep vertical planes
|
||
|
|
for (vector<Plane3d>::iterator it = planes.begin();
|
||
|
|
it < planes.end(); ++it)
|
||
|
|
{
|
||
|
|
if (it->isVertical()) {
|
||
|
|
verticalPlanes.push_back(*it);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// free a bit of memory
|
||
|
|
planes.clear();
|
||
|
|
|
||
|
|
// collect all the remaining planes as CGAL points
|
||
|
|
for (vector<Plane3d>::iterator it = verticalPlanes.begin();
|
||
|
|
it < verticalPlanes.end(); ++it)
|
||
|
|
{
|
||
|
|
Point toAdd(it->pt.x, it->pt.z);
|
||
|
|
points.push_back(toAdd);
|
||
|
|
}
|
||
|
|
|
||
|
|
// XXX compute the concave hull, aka. the alpha edges
|
||
|
|
alpha_edges(points.begin(), points.end(),
|
||
|
|
Type(250000), Alpha_shape_2::GENERAL,
|
||
|
|
std::back_inserter(segments));
|
||
|
|
|
||
|
|
if (!quiet) cout << "** " << segments.size() << " concave hull segments found" << endl;
|
||
|
|
|
||
|
|
// counts how many planes are rematched to make sure we find them again
|
||
|
|
unsigned int counter = 0;
|
||
|
|
|
||
|
|
// XXX naive retrieval of planes
|
||
|
|
Point3d center;
|
||
|
|
vector<Point> possiblePoints;
|
||
|
|
for (vector<Segment>::iterator it = segments.begin();
|
||
|
|
it < segments.end(); ++it)
|
||
|
|
{
|
||
|
|
Point source = it->source();
|
||
|
|
Point target = it->target();
|
||
|
|
|
||
|
|
center.x += source.x();
|
||
|
|
center.z += source.y();
|
||
|
|
center.x += target.x();
|
||
|
|
center.z += target.y();
|
||
|
|
|
||
|
|
possiblePoints.push_back(source);
|
||
|
|
possiblePoints.push_back(target);
|
||
|
|
}
|
||
|
|
center /= possiblePoints.size();
|
||
|
|
|
||
|
|
// match the points found using alpha shapes, back to their planes
|
||
|
|
for (vector<Point>::iterator it = possiblePoints.begin(); it < possiblePoints.end(); ++it) {
|
||
|
|
for (vector<Plane3d>::iterator jt = verticalPlanes.begin(); jt < verticalPlanes.end(); ++jt) {
|
||
|
|
// second coordinate is on the z axis in the left handed coord system
|
||
|
|
if ((it->x() >= jt->pt.x - eps) && (it->x() <= jt->pt.x + eps) &&
|
||
|
|
(it->y() >= jt->pt.z - eps) && (it->y() <= jt->pt.z + eps))
|
||
|
|
{
|
||
|
|
result.push_back(*jt);
|
||
|
|
verticalPlanes.erase(jt);
|
||
|
|
counter++;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// eliminate duplicate planes
|
||
|
|
result = Scene::getSignificantPlanes(result);
|
||
|
|
|
||
|
|
// sort them clockwise, it is in place
|
||
|
|
clockwiseSort(result);
|
||
|
|
|
||
|
|
// another threshold for deciding whether to keep a plane or not
|
||
|
|
double eps2 = 0.2;
|
||
|
|
|
||
|
|
// discard internal walls that are not part of the external walls of the room
|
||
|
|
for (vector<Plane3d>::iterator it = result.begin() + 1;
|
||
|
|
it < result.end(); ++it)
|
||
|
|
{
|
||
|
|
vector<Plane3d>::iterator prev = it - 1;
|
||
|
|
vector<Plane3d>::iterator curr = it;
|
||
|
|
|
||
|
|
// normalize normals just to be sure
|
||
|
|
prev->normal.normalize();
|
||
|
|
curr->normal.normalize();
|
||
|
|
|
||
|
|
if ((1.0 - fabs(prev->normal.dotProduct(curr->normal))) < eps2) {
|
||
|
|
if (fabs(center.distance(*prev)) < fabs(center.distance(*curr))) {
|
||
|
|
if (!quiet) cout << "** Discarding previous internal plane centered at "
|
||
|
|
<< prev->pt.x << " " << prev->pt.y << " " << prev->pt.z
|
||
|
|
<< endl;
|
||
|
|
result.erase(prev);
|
||
|
|
} else {
|
||
|
|
if (!quiet) cout << "** Discarding current internal plane centered at "
|
||
|
|
<< curr->pt.x << " " << curr->pt.y << " " << curr->pt.z
|
||
|
|
<< endl;
|
||
|
|
result.erase(curr);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!quiet) cout << "** Added " << result.size() << " elements to concave hull result" << endl;
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
void model::GraphicsAlg::getDiscreteLine(Point3d src, Point3d dest, double precision, const double& extraDist,
|
||
|
|
vector<Point3d>& line)
|
||
|
|
{
|
||
|
|
// clear the result just in case it contains something
|
||
|
|
line.clear();
|
||
|
|
|
||
|
|
// add the extra distance
|
||
|
|
double len = src.distance(dest);
|
||
|
|
double temp = (len + extraDist) / len;
|
||
|
|
dest.x = src.x + (dest.x - src.x) * temp;
|
||
|
|
dest.y = src.y + (dest.y - src.y) * temp;
|
||
|
|
dest.z = src.z + (dest.z - src.z) * temp;
|
||
|
|
|
||
|
|
// round up the values
|
||
|
|
precision = round(precision);
|
||
|
|
|
||
|
|
src.x = round(src.x);
|
||
|
|
src.y = round(src.y);
|
||
|
|
src.z = round(src.z);
|
||
|
|
|
||
|
|
dest.x = round(dest.x);
|
||
|
|
dest.y = round(dest.y);
|
||
|
|
dest.z = round(dest.z);
|
||
|
|
|
||
|
|
// adjust according to precision
|
||
|
|
const double coef = pow(10.0, precision);
|
||
|
|
src *= coef;
|
||
|
|
dest *= coef;
|
||
|
|
|
||
|
|
// Point3d diff = dest - src;
|
||
|
|
// diff.x = fabs(diff.x);
|
||
|
|
// diff.y = fabs(diff.y);
|
||
|
|
// diff.z = fabs(diff.z);
|
||
|
|
// diff += Point3d(1.0, 1.0, 1.0);
|
||
|
|
// double temp[] = {diff.x, diff.y, diff.z};
|
||
|
|
//
|
||
|
|
// double d = *max_element(temp, temp + 3);
|
||
|
|
|
||
|
|
double x1 = src.x;
|
||
|
|
double y1 = src.y;
|
||
|
|
double z1 = src.z;
|
||
|
|
|
||
|
|
double x2 = dest.x;
|
||
|
|
double y2 = dest.y;
|
||
|
|
double z2 = dest.z;
|
||
|
|
|
||
|
|
double dx = x2 - x1;
|
||
|
|
double dy = y2 - y1;
|
||
|
|
double dz = z2 - z1;
|
||
|
|
|
||
|
|
double ax = 2 * fabs(dx);
|
||
|
|
double ay = 2 * fabs(dy);
|
||
|
|
double az = 2 * fabs(dz);
|
||
|
|
|
||
|
|
double sx = dx > 0 ? +1.0 : -1.0;
|
||
|
|
double sy = dy > 0 ? +1.0 : -1.0;
|
||
|
|
double sz = dz > 0 ? +1.0 : -1.0;
|
||
|
|
|
||
|
|
double x = x1;
|
||
|
|
double y = y1;
|
||
|
|
double z = z1;
|
||
|
|
|
||
|
|
if (ax >= max(ay, az)) {
|
||
|
|
// x dominant
|
||
|
|
double yd = ay - ax / 2.0;
|
||
|
|
double zd = az - ax / 2.0;
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
line.push_back(Point3d(x, y, z));
|
||
|
|
|
||
|
|
if (x == x2) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along y
|
||
|
|
if (yd >= 0) {
|
||
|
|
y += sy;
|
||
|
|
yd -= ax;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along z
|
||
|
|
if (zd >= 0) {
|
||
|
|
z += sz;
|
||
|
|
zd -= ax;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along x
|
||
|
|
x += sx;
|
||
|
|
yd += ay;
|
||
|
|
zd += az;
|
||
|
|
}
|
||
|
|
} else if (ay >= max(ax, az)) {
|
||
|
|
// y dominant
|
||
|
|
double xd = ax - ay / 2.0;
|
||
|
|
double zd = az - ay / 2.0;
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
line.push_back(Point3d(x, y, z));
|
||
|
|
|
||
|
|
if (y == y2) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along x
|
||
|
|
if (xd >= 0) {
|
||
|
|
x += sx;
|
||
|
|
xd -= ay;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along z
|
||
|
|
if (zd >= 0) {
|
||
|
|
z += sz;
|
||
|
|
zd -= ay;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along y
|
||
|
|
y += sy;
|
||
|
|
xd += ax;
|
||
|
|
zd += az;
|
||
|
|
}
|
||
|
|
} else if (az >= max(ax, ay)) {
|
||
|
|
// z dominant
|
||
|
|
double xd = ax - az / 2.0;
|
||
|
|
double yd = ay - az / 2.0;
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
line.push_back(Point3d(x, y, z));
|
||
|
|
|
||
|
|
if (z == z2) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along x
|
||
|
|
if (xd >= 0) {
|
||
|
|
x += sx;
|
||
|
|
xd -= az;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along y
|
||
|
|
if (yd >= 0) {
|
||
|
|
y += sy;
|
||
|
|
yd -= az;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move along x
|
||
|
|
z += sz;
|
||
|
|
xd += ax;
|
||
|
|
yd += ay;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
throw logic_error("invalid branch taken while computing discrete line");
|
||
|
|
}
|
||
|
|
|
||
|
|
for (vector<Point3d>::iterator it = line.begin(); it != line.end(); ++it) {
|
||
|
|
it->x /= coef;
|
||
|
|
it->y /= coef;
|
||
|
|
it->z /= coef;
|
||
|
|
}
|
||
|
|
}
|