WindowGroupControl WindowGroup::windowGroupControl() const
{
boost::optional<double> largestArea;
boost::optional<Point3d> centroid;
boost::optional<Vector3d> outwardNormal;
for (const auto & windowPolygon : m_windowPolygons){
boost::optional<double> area = getArea(windowPolygon);
if (area){
if (!largestArea || (*area > *largestArea)){
boost::optional<Point3d> tmpCentroid = getCentroid(windowPolygon);
boost::optional<Vector3d> tmpOutwardNormal = getOutwardNormal(windowPolygon);
if (tmpCentroid && tmpOutwardNormal){
largestArea = area;
centroid = tmpCentroid;
outwardNormal = tmpOutwardNormal;
}
}
}
}
WindowGroupControl result;
result.largestArea = largestArea;
result.centroid = centroid;
result.outwardNormal = outwardNormal;
return result;
}
/// transforms face coordinates to regular system, face normal will be z'
/// will try to align y' with z, but if that fails will align y' with y
/// face origin will be minimum point in x', y' and z'=0
/// will return identity transformation if cannot compute plane for vertices
Transformation Transformation::alignFace(const std::vector<Point3d>& vertices)
{
OptionalVector3d zPrime = getOutwardNormal(vertices);
if (!zPrime){
LOG(Error, "Cannot compute outward normal for vertices");
return Transformation();
}
// align z' with outward normal
Transformation align = alignZPrime(*zPrime);
Point3dVector alignedVertices = align.inverse()*vertices;
// compute translation to minimum in aligned system
double minX = alignedVertices[0].x();
double minY = alignedVertices[0].y();
double minZ = alignedVertices[0].z();
for (const Point3d& vertex : alignedVertices){
minX = min(minX, vertex.x());
minY = min(minY, vertex.y());
minZ = min(minZ, vertex.z());
}
Transformation translate = translation(Vector3d(minX, minY, minZ));
return align*translate;
}
std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
{
std::vector<std::vector<Point3d> > result;
// check input
if (vertices.size () < 3){
return result;
}
boost::optional<Vector3d> normal = getOutwardNormal(vertices);
if (!normal || normal->z() > -0.999){
return result;
}
for (const auto& hole : holes){
normal = getOutwardNormal(hole);
if (!normal || normal->z() > -0.999){
return result;
}
}
std::vector<Point3d> allPoints;
// PolyPartition does not support holes which intersect the polygon or share an edge
// if any hole is not fully contained we will use boost to remove all the holes
bool polyPartitionHoles = true;
for (const std::vector<Point3d>& hole : holes){
if (!within(hole, vertices, tol)){
// PolyPartition can't handle this
polyPartitionHoles = false;
break;
}
}
if (!polyPartitionHoles){
// use boost to do all the intersections
std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
std::vector<std::vector<Point3d> > noHoles;
for (const std::vector<Point3d>& face : allFaces){
std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
result.insert(result.end(), temp.begin(), temp.end());
}
return result;
}
// convert input to vector of TPPLPoly
std::list<TPPLPoly> polys;
TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
outerPoly.Init(vertices.size());
outerPoly.SetHole(false);
unsigned n = vertices.size();
for(unsigned i = 0; i < n; ++i){
// should all have zero z coordinate now
double z = vertices[n-i-1].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
outerPoly[i].x = point.x();
outerPoly[i].y = point.y();
}
outerPoly.SetOrientation(TPPL_CCW);
polys.push_back(outerPoly);
for (const std::vector<Point3d>& holeVertices : holes){
if (holeVertices.size () < 3){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
continue;
}
TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
innerPoly.Init(holeVertices.size());
innerPoly.SetHole(true);
//std::cout << "inner :";
for(unsigned i = 0; i < holeVertices.size(); ++i){
// should all have zero z coordinate now
double z = holeVertices[i].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
innerPoly[i].x = point.x();
innerPoly[i].y = point.y();
}
innerPoly.SetOrientation(TPPL_CW);
polys.push_back(innerPoly);
}
// do partitioning
TPPLPartition pp;
std::list<TPPLPoly> resultPolys;
int test = pp.Triangulate_EC(&polys,&resultPolys);
if (test == 0){
test = pp.Triangulate_MONO(&polys, &resultPolys);
}
if (test == 0){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
return result;
}
// convert back to vertices
std::list<TPPLPoly>::iterator it, itend;
//std::cout << "Start" << std::endl;
for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
it->SetOrientation(TPPL_CW);
std::vector<Point3d> triangle;
for (long i = 0; i < it->GetNumPoints(); ++i){
TPPLPoint point = it->GetPoint(i);
triangle.push_back(Point3d(point.x, point.y, 0));
}
//std::cout << triangle << std::endl;
result.push_back(triangle);
}
//std::cout << "End" << std::endl;
return result;
}
Plane::Plane(const std::vector<Point3d>& points)
: m_a(0.0), m_b(0.0), m_c(0.0), m_d(0.0)
{
unsigned N = points.size();
if (N < 3){
LOG_AND_THROW("Cannot compute plane with fewer than three points");
}else if (N == 3){
// duplicates code in point and normal ctor, points[1] is the point and (a x b) is the normal
Point3d point = points[1];
Vector3d a = points[1]-points[0];
Vector3d b = points[2]-points[1];
Vector3d thisNormal = a.cross(b);
if (!thisNormal.normalize()){
LOG_AND_THROW("Cannot initialize plane because normal is undefined");
}
m_a = thisNormal.x();
m_b = thisNormal.y();
m_c = thisNormal.z();
m_d = -thisNormal.x()*point.x() - thisNormal.y()*point.y() - thisNormal.z()*point.z();
}else{
bool foundSolution = false;
double tol = 1e-8; // 0.0001 was too big for the determinant tolerance, 1e-12 was too small
double maxDet = tol;
// solve the equation ax+by+cz+d=0 in a few different ways, keep the best one
{
// Ax = b, x = [a/c; b/c; d/c]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].x() - points[0].x();
A(i,1) = points[i].y() - points[0].y();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].z() - points[0].z());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double a_c = x[0];
double b_c = x[1];
double d_c = x[2];
// a = a_c*c
// b = b_c*c
// d = d_c*c
// a^2 + b^2 + c^2 = 1
// c^2*(a_c^2 + b_c^2 + 1) = 1
m_c = 1.0/sqrt(a_c*a_c + b_c*b_c + 1.0);
m_a = a_c*m_c;
m_b = b_c*m_c;
m_d = d_c*m_c - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
{
// Ax = b, x = [a/b; c/b; d/b]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].x() - points[0].x();
A(i,1) = points[i].z() - points[0].z();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].y() - points[0].y());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double a_b = x[0];
double c_b = x[1];
double d_b = x[2];
// a = a_b*b
// c = c_b*b
// d = d_b*b
// a^2 + b^2 + c^2 = 1
// b^2*(a_b^2 + c_b^2 + 1) = 1
m_b = 1.0/sqrt(a_b*a_b + c_b*c_b + 1.0);
m_a = a_b*m_b;
m_c = c_b*m_b;
m_d = d_b*m_b - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
{
// Ax = b, x = [b/a; c/a; d/a]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].y() - points[0].y();
A(i,1) = points[i].z() - points[0].z();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].x() - points[0].x());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double b_a = x[0];
double c_a = x[1];
double d_a = x[2];
// b = b_a*a
// c = c_a*a
// d = d_a*a
// a^2 + b^2 + c^2 = 1
// a^2*(b_a^2 + c_a^2 + 1) = 1
m_a = 1.0/sqrt(b_a*b_a + c_a*c_a + 1.0);
m_b = b_a*m_a;
m_c = c_a*m_a;
m_d = d_a*m_a - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
if (!foundSolution){
//std::stringstream ss;
//ss << std::fixed << std::setprecision(20) << points << std::endl;
//std::string s = ss.str();
LOG_AND_THROW("Cannot compute plane for points " << points);
}
// get outward normal from vertices, plane outward normal should match sense of vertices
// this corresponds to the other solution to the sqrt
boost::optional<Vector3d> outwardNormal = getOutwardNormal(points);
if (outwardNormal){
double dot = m_a*outwardNormal.get().x() + m_b*outwardNormal.get().y() + m_c*outwardNormal.get().z();
if (dot < 0){
m_a = -m_a;
m_b = -m_b;
m_c = -m_c;
m_d = -m_d;
}
}
// test that normal has length 1
double length = m_a*m_a + m_b*m_b + m_c*m_c;
tol = 0.0001;
OS_ASSERT(fabs(1.0-length) <= tol);
}
}
