3dpcp/.svn/pristine/99/9950464c192a22a6ce4d9902792fff4c19c388bc.svn-base

260 lines
7.1 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/**
* @file plane3d.cc
*
* @auhtor Remus Claudiu Dumitru <r.dumitru@jacobs-university.de>
* @date 16 Feb 2012
*
*/
//==============================================================================
// Includes
//==============================================================================
#include "model/plane3d.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>
using namespace Eigen;
#include <math.h>
#include <iostream>
#include <stdexcept>
using namespace std;
model::Plane3d::Plane3d() {
// nothing to do
}
model::Plane3d::Plane3d(const Point3d& pt, const Vector3d& normal) {
this->pt = pt;
this->normal = normal;
// normals must be normalized
this->normal.normalize();
}
model::Plane3d::Plane3d(const Point3d& pt, const Vector3d& normal,
const vector<Point3d>& hull)
{
this->pt = pt;
this->normal = normal;
this->hull = hull;
// normals must be normalized
this->normal.normalize();
}
model::Plane3d::Plane3d(const Plane3d& other) {
this->pt = other.pt;
this->normal = other.normal;
this->hull = other.hull;
}
model::Plane3d::~Plane3d() {
// nothing to do
}
model::Point3d model::Plane3d::intersect(const Plane3d& second, const Plane3d& third) {
// XXX http://mathworld.wolfram.com/Plane-PlaneIntersection.html
Point3d result;
Plane3d first = *this;
MatrixXd mat(3, 3);
mat << first.normal.x, second.normal.x, third.normal.x,
first.normal.y, second.normal.y, third.normal.y,
first.normal.z, second.normal.z, third.normal.z;
Vector3d term[3];
term[0] = second.normal.crossProduct(third.normal) *
first.normal.dotProduct(Vector3d(first.pt.x, first.pt.y, first.pt.z));
term[1] = third.normal.crossProduct(first.normal) *
second.normal.dotProduct(Vector3d(second.pt.x, second.pt.y, second.pt.z));
term[2] = first.normal.crossProduct(second.normal) *
third.normal.dotProduct(Vector3d(third.pt.x, third.pt.y, third.pt.z));
Vector3d tempResult = (term[0] + term[1] + term[2]) * (1.0 / mat.determinant());
result.x = tempResult.x;
result.y = tempResult.y;
result.z = tempResult.z;
return result;
}
model::Plane3d& model::Plane3d::operator=(const Plane3d& other) {
this->pt = other.pt;
this->normal = other.normal;
this->hull = other.hull;
return *this;
}
bool model::Plane3d::isVertical() {
this->normal.normalize();
return (fabs(this->normal.y) < _EPSILON_VERTICAL);
}
bool model::Plane3d::isHorizontal() {
this->normal.normalize();
return ((1 - fabs(this->normal.y / this->normal.length())) < _EPSILON_HORIZONTAL);
}
bool model::Plane3d::isSamePlane(model::Plane3d other) {
// just to be safe
this->normal.normalize();
other.normal.normalize();
double dist = this->pt.distance(other);
double norm = this->normal.crossProduct(other.normal).length();
// doing fabs of norm just to be safe, though it is always positive
if (fabs(dist) < _EPSILON_DISTANCE && fabs(norm) < _EPSILON_NORMAL_CROSS_PRODUCT) {
return true;
}
return false;
}
model::Vector3d model::Plane3d::computeAverageNormal() {
if(this->hull.size() < 3) {
throw runtime_error("hull contains less than 3 points, cannot compute normal");
}
// start from zero
Vector3d normal(0.0, 0.0, 0.0);
for (vector<Point3d>::iterator it = this->hull.begin() + 1; it != this->hull.end() - 1; ++it) {
Point3d& left = *(it-1);
Point3d& middle = *it;
Point3d& right = *(it+1);
normal += Vector3d(middle, left).crossProduct(Vector3d(middle, right));
}
// treat the boundary cases
Point3d& first = this->hull[0];
Point3d& second = this->hull[1];
Point3d& secondLast = this->hull[this->hull.size() - 2];
Point3d& last = this->hull[this->hull.size() - 1];
normal += Vector3d(first, last).crossProduct(Vector3d(first, second));
normal += Vector3d(last, secondLast).crossProduct(Vector3d(last, first));
normal.normalize();
return normal;
}
vector<vector<model::Point3d> > model::Plane3d::getDiscretePoints(const double& dist) const {
if (this->hull.size() < 3) {
throw runtime_error("cannot retrieve discrete points on plane with hull containing less than 3 points");
}
// make a deep copy of the current plane
Plane3d copy = *this;
// this will eventually contain the result
vector<vector<model::Point3d> > result;
// the origin of the current system, take the first point in the hull
Point3d origin = copy.hull[0];
Point3d zero(0.0, 0.0, 0.0);
// define the rotation around Z and X axis here
Rotation3d rotZ(0.0, 0.0, atan2(copy.normal.x, copy.normal.y));
copy.normal.rotate(rotZ);
Rotation3d rotX(-atan2(copy.normal.z, copy.normal.y), 0.0, 0.0);
// translate all points to origin
for (vector<Point3d>::iterator it = copy.hull.begin(); it != copy.hull.end(); ++it) {
// translate
*it -= origin;
// rotate around Z
it->rotate(zero, rotZ);
// rotate around X
it->rotate(zero, rotX);
}
// figure out how to rotate around Y
Vector3d firstSegment;
Point3d first = copy.hull.front();
Point3d second = copy.hull[1];
Point3d third = copy.hull[2];
if (first.distance(second) > second.distance(third)) {
firstSegment = Vector3d(first, second);
} else {
firstSegment = Vector3d(second, third);
}
// define rotation around Y axis after we have moved the entire hull into the XZ plane
copy.normal.rotate(rotX);
Rotation3d rotY(0.0, atan2(firstSegment.z, firstSegment.x), 0.0);
for (vector<Point3d>::iterator it = copy.hull.begin(); it != copy.hull.end(); ++it) {
// rotate around Y
it->rotate(zero, rotY);
}
// we need to compute the maximum and minimum offsets
double minX = copy.hull[0].x;
double maxX = copy.hull[0].x;
double minZ = copy.hull[0].z;
double maxZ = copy.hull[0].z;
// we need to start from the second point
for (vector<Point3d>::iterator it = copy.hull.begin() + 1;
it != copy.hull.end(); ++it)
{
if (it->x < minX) {
minX = it->x;
}
if (it->x > maxX) {
maxX = it->x;
}
if (it->z < minZ) {
minZ = it->z;
}
if (it->z > maxZ) {
maxZ = it->z;
}
}
// how many steps on each direction?
int stepsX = round((maxX - minX) / dist) + 2;
int stepsZ = round((maxZ - minZ) / dist) + 2;
// resize the result vector
result.resize(stepsZ);
for (int j = 0; j < stepsZ; ++j) {
for (int i = 0; i < stepsX; ++i) {
Point3d toPush(minX + dist * (i - 0.5), 0.0, maxZ - dist * (j - 0.5));
// rotate back around Y
toPush.rotate(zero, rotY.getInverse());
// rotate back around X
toPush.rotate(zero, rotX.getInverse());;
// rotate back around Z
toPush.rotate(zero, rotZ.getInverse());
//translate back to previous system
toPush += origin;
result[j].push_back(toPush);
}
}
return result;
}