3dpcp/.svn/pristine/55/55c32b784ae23c7e76eb9fb6d0d3d9f6caceeddb.svn-base
2012-09-16 14:33:11 +02:00

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