//compute the cross point
Point_3 compute_cross_point(Plane_3 plane, Point_3 start, Point_3 end)
{
Vector_3 normal = plane.orthogonal_vector();
Vector_3 line_direction = end - start;
Point_3 p= plane.point();
double t;
double a = (start.x() - p.x()) * normal.x() + (start.y() - p.y()) * normal.y() + (start.z() - p.z()) * normal.z();
double b = line_direction.x() * normal.x() + line_direction.y() * normal.y() + line_direction.z() * normal.z();
assert(b != 0);
t = -a / b;
return start + t * line_direction;
}
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 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);
}
bool on_positive_side(const Plane_3& P, const Point_3& p)
{
return P.has_on_positive_side(p);
// typedef CGAL::Cartesian<double> K;
// typedef CGAL::Plane_3<K> Plane_3;
// Plane_3 CP(P.A(), P.B(), P.C(), P.D());
// return CP.has_on_positive_side(to_cgal(p));
}
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);
}
/**
* Finds the exit point of upslope_path on the facet left of h.
*
* upslope_path must intersect the boundary of the facet in 2 points or a
* segment. One of these points must be start_point. If the intersection is a
* segment, returns the endpoint that is not start_point. Otherwise returns the
* other intersection point. Updates h so it is the halfedge where the
* intersection is found.
*/
Point_2 find_exit(Halfedge_handle& h, const Ray_2& upslope_path,
const Point_2& start_point)
{
Point_2 exit;
Plane_3 plane = h->facet()->plane();
typedef Facet::Halfedge_around_facet_circulator Circulator;
Circulator current = h->facet()->facet_begin();
Circulator end = h->facet()->facet_begin();
do {
Point_2 source = plane.to_2d(current->vertex()->point());
Point_2 target = plane.to_2d(current->opposite()->vertex()->point());
Segment_2 seg = Segment_2(source, target);
// Example pulled from http://tinyurl.com/intersect-doc
CGAL::Object intersect = CGAL::intersection(upslope_path, seg);
// Return for a point intersection
if (const CGAL::Point_2<Kernel> *ipoint =
CGAL::object_cast<CGAL::Point_2<Kernel> >(&intersect)) {
if (*ipoint != start_point) {
h = current;
return *ipoint;
}
}
// Return the opposite point of the segment for a segment intersection.
else if (const CGAL::Segment_2<Kernel> *iseg =
CGAL::object_cast<CGAL::Segment_2<Kernel> >(&intersect)) {
h = current;
if (iseg->source() == start_point)
return iseg->target();
return iseg->source();
}
} while (++current != end);
cout << "Failed to find an intersection point." << endl;
cout << "Start: " << start_point << endl;
cout << "Upslope path: " << upslope_path << endl;
print_facet(*h->facet());
std::abort();
return exit;
}
void CAT_facet::gl_draw(const Plane_3& pln) const
{
for (std::list<Point_3>::const_iterator it = cat_vtx.begin(); it != cat_vtx.end(); ++it)
if (pln.has_on_negative_side(*it))
return;
glColor4f(0.2f, 0.8f, 1.0f, 1.0f);
glBegin(GL_POLYGON);
for (std::list<Point_3>::const_iterator it = cat_vtx.begin(); it != cat_vtx.end(); ++it)
glVertex3d(it->x(), it->y(), it->z());
glEnd();
glColor4f(0.1f, 0.1f, 0.1f, 1.0f);
glBegin(GL_LINE_LOOP);
for (std::list<Point_3>::const_iterator it = cat_vtx.begin(); it != cat_vtx.end(); ++it)
glVertex3d(it->x(), it->y(), it->z());
glEnd();
}
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;
}
/**
* Determines whether a plane is flat.
*/
bool is_flat_plane(const Plane_3& h)
{
return h.a() == 0.0 && h.b() == 0.0;
}
// compute one cross point between 1-ring of c_vh and cutPlane
bool vertexTo(Vertex_handle &c_vh, Halfedge_handle &c_hh, Vertex_handle center,
Halfedge_vertex_circulator &optimal_start_spoke,
const Plane_3 &cutPlane, Polyhedron* mesh, std::list<Point_3> &cross_points,
int nthTarget)
{
bool result(false);
Halfedge_vertex_circulator hc;
if ( c_vh == center )
hc = optimal_start_spoke;
else
hc = c_vh->vertex_begin();
Halfedge_vertex_circulator end = hc;
CGAL_For_all(hc,end)
{
Vertex_handle lvh = hc->opposite()->vertex();
Vertex_handle rvh = hc->next()->vertex();
Point_3 lp = lvh->point();
Point_3 rp = rvh->point();
int il = cutPlane.oriented_side(lp);
int ir = cutPlane.oriented_side(rp);
int tmp = il*ir;
if ( tmp<0)//异侧
{
Halfedge_handle lrhh = hc->next()->next();
if (lrhh->tag()==nthTarget)//在找这次的target的过程中已经用过了
{
continue;
}
else
{
lrhh->tag(nthTarget);
lrhh->opposite()->tag(nthTarget);
}
if ( c_vh == center )
{
optimal_start_spoke = hc;// ++optimal_start_spoke;
}
Point_3 cp = compute_cross_point(cutPlane,lp,rp);
cross_points.push_back(cp);
c_hh = hc->next()->next()->opposite();
c_vh = 0;
result = true;
break;
}
else if(tmp>0)//同侧
{
continue;
}
if (ir)//lp is on the cut plane
{
if (lvh->tag()==nthTarget)//在找这次的target的过程中已经用过了
{
continue;
}
else
{
lvh->tag(nthTarget);
}
if ( c_vh == center)
{
optimal_start_spoke = hc;// ++optimal_start_spoke;
}
c_vh = lvh;
}
else
{
if (rvh->tag()==nthTarget)//在找这次的target的过程中已经用过了
{
continue;
}
else
{
rvh->tag(nthTarget);
}
if ( c_vh == center)
{
optimal_start_spoke = hc; ++optimal_start_spoke; //++optimal_start_spoke;
}
c_vh = rvh;
}
cross_points.push_back(c_vh->point());
c_hh = 0;
result = true;
break;
}
/* 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();
}
float
sdf( const Point& q, const Mesh &mesh, const vector<double>& weights,
KdTree& kd_tree, const Triangulation& triang )
{
VECTOR3 query (CGAL::to_double(q.x()),
CGAL::to_double(q.y()),
CGAL::to_double(q.z()));
kd_tree.queryPosition(query);
// Initialize the search structure, and search all N points
int n_vid = kd_tree.getNeighbourPositionIndex(0);
if(n_vid == -1) throw std::runtime_error("No nearest neighbor. MDS empty?");
CGAL_assertion( ! mesh.vert_list[n_vid].iso());
MVertex nv = mesh.vert_list[n_vid];
double min_sq_d = HUGE;
int n_fid = -1;
for(int i = 0; i < nv.num_inc_face; i ++)
{
MFace f = mesh.face_list[nv.inc_face(i)];
Point p[3] = {mesh.vert_list[f.get_corner(0)].point(),
mesh.vert_list[f.get_corner(1)].point(),
mesh.vert_list[f.get_corner(2)].point()};
Triangle_3 t (p[0], p[1], p[2]);
Plane_3 H (p[0], p[1], p[2]);
Point _q = H.projection(q);
// check if _q is inside t.
if( t.has_on(_q) )
{
double sq_d = CGAL::to_double((q-_q)*(q-_q));
if( sq_d < min_sq_d ) { min_sq_d = sq_d; n_fid = nv.inc_face(i); }
}
else
{
for(int j = 0; j < 3; j ++)
{
double _d = CGAL::to_double(CGAL::squared_distance(_q,Segment(p[j], p[(j+1)%3])));
double sq_d = CGAL::to_double((q-_q)*(q-_q)) + _d;
if( sq_d < min_sq_d ) { min_sq_d = sq_d; n_fid = nv.inc_face(i); }
}
}
}
// locate the query point in the triang which is already tagged
// with in-out flag by the reconstruction.
bool is_q_outside = false;
Triangulation::Locate_type lt;
int u = -1, v = -1;
Cell_handle c = triang.locate(q, lt, u, v);
if( lt == Triangulation::OUTSIDE_AFFINE_HULL )
{
is_q_outside = true;
cerr << "Point " << q << " is outside the affine hull." << endl;
}
else if( lt == Triangulation::OUTSIDE_CONVEX_HULL )
is_q_outside = true;
else
{
if( lt == Triangulation::CELL )
{
if( c->outside )
is_q_outside = true;
else
is_q_outside = false;
}
else if( lt == Triangulation::FACET )
{
Cell_handle _c = c->neighbor(u);
if( c->outside && _c->outside )
is_q_outside = true;
else
is_q_outside = false;
}
else if( lt == Triangulation::EDGE )
{
if( is_outside_VF(triang, Edge(c, u, v)) )
is_q_outside = true;
else
is_q_outside = false;
}
else
{
CGAL_assertion( lt == Triangulation::VERTEX );
is_q_outside = false;
}
}
double w;
if(weights.size() && mesh.face_list[n_fid].label != -1) w = weights[mesh.face_list[n_fid].label];
else w = 1.0;
// double w = mesh.face_list[n_fid].w;
double gen_sdf = 0;
if( is_q_outside ) gen_sdf = w*sqrt(min_sq_d);
else gen_sdf = -w*min_sq_d;
#if 0
double MAX = 10, MIN = -10;
if( gen_sdf > MAX ) gen_sdf = MAX;
if( gen_sdf < MIN ) gen_sdf = MIN;
#endif
return gen_sdf;
}