static double planeDist(const Plane_3 &p0, const Plane_3 &p1)
{
double a = p0.a() - p1.a();
double b = p0.b() - p1.b();
double c = p0.c() - p1.c();
double d = p0.d() - p1.d();
return sqrt(a * a + b * b + c * c + d * d);
}
static Plane_3 centerizePlane(const Plane_3 &p, const Point_3 &C, double sign)
{
Plane_3 pp(p.a(), p.b(), p.c(), p.d() + p.a() * C.x() + p.b() * C.y()
+ p.c() * C.z());
double a = pp.a();
double b = pp.b();
double c = pp.c();
double d = pp.d();
if (sign * signedDist(pp, C) < 0.)
{
a = -a;
b = -b;
c = -c;
d = -d;
}
pp = Plane_3(a, b, c, d);
return pp;
}
Point_3 calculateMove(const Vertex_handle &vertex,
const Vector_3 &tangient)
{
DEBUG_START;
Plane_3 plane = dual(vertex->point());
Vector_3 u(plane.a(), plane.b(), plane.c());
if (plane.d() > 0.)
u = -u;
double value = tangient * u;
Plane_3 planeNew(u.x(), u.y(), u.z(), -value);
Point_3 point = dual(planeNew);
DEBUG_END;
return point;
}
static std::pair<bool, double> calculateAlpha(const Vector_3 &xOld,
const Vector_3 &xNew,
const Plane_3 &planeOuter)
{
DEBUG_START;
std::cout << "Calculating alpha for plane " << planeOuter << std::endl;
Plane_3 plane = planeOuter;
Vector_3 u(plane.a(), plane.b(), plane.c());
double value = -plane.d();
if (value < 0.)
{
u = -u;
value = -value;
}
double length = sqrt(u.squared_length());
u = u / length;
std::cout << " u = " << u << std::endl;
value = value / length;
std::cout << " h initial = " << value << std::endl;
double productOld = xOld * u;
std::cout << " h current = " << productOld << std::endl;
double productNew = xNew * u;
std::cout << " h new = " << productNew << std::endl;
double productDifference = productNew - productOld;
std::cout << " product difference: " << productDifference
<< std::endl;
if (productDifference < 0.)
{
std::cout << "Stop strange step processing" << std::endl;
DEBUG_END;
return std::make_pair(false, 0.);
}
double alpha = (productNew - value) / (productNew - productOld);
if (alpha > 1.)
{
std::cout << "Truncating alpha: " << alpha << " -> 1."
<< std::endl;
alpha = 1.;
}
DEBUG_END;
return std::make_pair(true, alpha);
}
static Plane_3 normalizePlane(const Plane_3 &p)
{
double a = p.a();
double b = p.b();
double c = p.c();
double d = p.d();
if (d > 0.)
{
a = -a;
b = -b;
c = -c;
d = -d;
}
double l = sqrt(a * a + b * b + c * c);
ASSERT(l > 1e-15);
a /= l;
b /= l;
c /= l;
d /= l;
return Plane_3(a, b, c, d);
}
/* FIXME: Copied from Polyhedron_io.cpp with slight modifications. */
static std::shared_ptr<Polyhedron> convertWithAssociation(Polyhedron_3 p,
const Point_3 &C, const std::vector<Plane_3> &initPlanes)
{
/* Check for non-emptiness. */
ASSERT(p.size_of_vertices());
ASSERT(p.size_of_facets());
int numVertices = p.size_of_vertices();
int numFacets = p.size_of_facets();
/* Allocate memory for arrays. */
Vector3d *vertices = new Vector3d[numVertices];
Facet *facets = new Facet[numFacets];
/* Transform vertexes. */
int iVertex = 0;
for (auto vertex = p.vertices_begin(); vertex != p.vertices_end(); ++vertex)
{
Point_3 point = C + vertex->point();
vertices[iVertex++] = Vector3d(point.x(), point.y(), point.z());
}
/*
* Transform facets.
* This algorithm is based on example kindly provided at CGAL online user
* manual. See example Polyhedron/polyhedron_prog_off.cpp
*/
int iFacet = 0;
auto plane = p.planes_begin();
auto facet = p.facets_begin();
/* Iterate through the std::lists of planes and facets. */
do
{
int id = p.indexPlanes_[iFacet];
facets[id].id = id;
/* Transform current plane. */
Plane_3 pi = centerizePlane(*plane,
Point_3(-C.x(), -C.y(), -C.z()),
signedDist(initPlanes[id], C));
facets[id].plane = Plane(Vector3d(pi.a(), pi.b(), pi.c()),
pi.d());
/*
* Iterate through the std::list of halfedges incident to the curent CGAL
* facet.
*/
auto halfedge = facet->facet_begin();
/* Facets in polyhedral surfaces are at least triangles. */
CGAL_assertion(CGAL::circulator_size(halfedge) >= 3);
facets[id].numVertices = CGAL::circulator_size(halfedge);
facets[id].indVertices =
new int[3 * facets[id].numVertices + 1];
/*
* TODO: It's too unsafe architecture if we do such things as setting
* the size of internal array outside the API functions. Moreover, it
* can cause us to write memory leaks.
* indFacets and numFacets should no be public members.
*/
int iFacetVertex = 0;
do
{
facets[id].indVertices[iFacetVertex++] =
std::distance(p.vertices_begin(), halfedge->vertex());
} while (++halfedge != facet->facet_begin());
/* Add cycling vertex to avoid assertion during printing. */
facets[id].indVertices[facets[id].numVertices] =
facets[id].indVertices[0];
ASSERT(facets[id].correctPlane());
/* Increment the ID of facet. */
++iFacet;
} while (++plane != p.planes_end() && ++facet != p.facets_end());
return std::make_shared<Polyhedron>(numVertices, numFacets, vertices,
facets);
}
static double signedDist(const Plane_3 &p, const Point_3 &C)
{
return p.a() * C.x() + p.b() * C.y() + p.c() * C.z() + p.d();
}
