void
pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
const PointIHS& pt_1,
const PointIHS& pt_2,
const PointIHS& pt_3,
VertexIndex& vi_0,
VertexIndex& vi_1,
VertexIndex& vi_2,
VertexIndex& vi_3,
const MeshPtr& mesh) const
{
// Treated bitwise
// 2 - 1
// | |
// 3 - 0
const unsigned char is_finite = static_cast <unsigned char> (
(1 * !boost::math::isnan (pt_0.x)) |
(2 * !boost::math::isnan (pt_1.x)) |
(4 * !boost::math::isnan (pt_2.x)) |
(8 * !boost::math::isnan (pt_3.x)));
switch (is_finite)
{
case 7: this->addToMesh (pt_0, pt_1, pt_2, vi_0, vi_1, vi_2, mesh); break; // 0-1-2
case 11: this->addToMesh (pt_0, pt_1, pt_3, vi_0, vi_1, vi_3, mesh); break; // 0-1-3
case 13: this->addToMesh (pt_0, pt_2, pt_3, vi_0, vi_2, vi_3, mesh); break; // 0-2-3
case 14: this->addToMesh (pt_1, pt_2, pt_3, vi_1, vi_2, vi_3, mesh); break; // 1-2-3
case 15: // 0-1-2-3
{
if (!distanceThreshold (pt_0, pt_1, pt_2, pt_3)) break;
if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
if (!vi_3.isValid ()) vi_3 = mesh->addVertex (pt_3);
if (vi_0==vi_1 || vi_0==vi_2 || vi_0==vi_3 || vi_1==vi_2 || vi_1==vi_3 || vi_2==vi_3)
{
return;
}
mesh->addTrianglePair (vi_0, vi_1, vi_2, vi_3);
break;
}
}
}
void
pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
const PointIHS& pt_1,
const PointIHS& pt_2,
VertexIndex& vi_0,
VertexIndex& vi_1,
VertexIndex& vi_2,
const MeshPtr& mesh) const
{
if (!distanceThreshold (pt_0, pt_1, pt_2)) return;
if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
if (vi_0==vi_1 || vi_0==vi_2 || vi_1==vi_2)
{
return;
}
mesh->addFace (vi_0, vi_1, vi_2);
}
// 3 - 2 3 - 2
// | / | or | \ |
// 0 - 1 0 - 1
FaceIndexPair
addFace (const VertexIndex& idx_v_0,
const VertexIndex& idx_v_1,
const VertexIndex& idx_v_2,
const VertexIndex& idx_v_3,
const FaceData& face_data = FaceData ())
{
// Try to add two faces
// 3 - 2
// | / |
// 0 - 1
FaceIndex idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data);
FaceIndex idx_face_1 = this->addFace (idx_v_0, idx_v_2, idx_v_3, face_data);
if (idx_face_0.isValid ())
{
return (std::make_pair (idx_face_0, idx_face_1));
}
else if (idx_face_1.isValid ())
{
idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data); // might be possible to add now
return (std::make_pair (idx_face_1, idx_face_0));
}
// Try to add two faces
// 3 - 2
// | \ |
// 0 - 1
idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data);
idx_face_1 = this->addFace (idx_v_0, idx_v_1, idx_v_3, face_data);
if (idx_face_0.isValid ())
{
return (std::make_pair (idx_face_0, idx_face_1));
}
else if (idx_face_1.isValid ())
{
idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data); // might be possible to add now
return (std::make_pair (idx_face_1, idx_face_0));
}
// Connect the triangle pair if possible
if (!idx_v_0.isValid () || !idx_v_1.isValid () || !idx_v_2.isValid () || !idx_v_3.isValid ())
{
return (std::make_pair (FaceIndex (), FaceIndex ()));
}
VertexIndexes vi; vi.reserve (4);
vi.push_back (idx_v_0);
vi.push_back (idx_v_1);
vi.push_back (idx_v_2);
vi.push_back (idx_v_3);
if (!this->uniqueCheck (vi))
{
return (std::make_pair (FaceIndex (), FaceIndex ()));
}
HalfEdgeIndex idx_he_01, idx_he_12, idx_he_23, idx_he_30;
// Check manifoldness
bool is_new_01 = true;
bool is_new_12 = true;
bool is_new_23 = true;
bool is_new_30 = true;
if (!Base::checkTopology1 (idx_v_0,idx_v_1, idx_he_01, is_new_01) ||
!Base::checkTopology1 (idx_v_1,idx_v_2, idx_he_12, is_new_12) ||
!Base::checkTopology1 (idx_v_2,idx_v_3, idx_he_23, is_new_23) ||
!Base::checkTopology1 (idx_v_3,idx_v_0, idx_he_30, is_new_30))
{
return (std::make_pair (FaceIndex (), FaceIndex ()));
}
// Connect the triangle pair
if (!is_new_01 && is_new_12 && !is_new_23 && is_new_30)
{
return (this->connectTrianglePair (idx_he_01, idx_he_23, idx_v_0, idx_v_1, idx_v_2, idx_v_3, face_data));
}
else if (is_new_01 && !is_new_12 && is_new_23 && !is_new_30)
{
return (this->connectTrianglePair (idx_he_12, idx_he_30, idx_v_1, idx_v_2, idx_v_3, idx_v_0, face_data));
}
else
{
assert (true); // This should not happen!
return (std::make_pair (FaceIndex (), FaceIndex ()));
}
}
Pmwx::Halfedge_handle InsertOneSegment(
const RawCoordPair& p1,
const RawCoordPair& p2,
VertexIndex& index,
Pmwx& ioMap)
{
string key1 = RawCoordToKey(p1);
Point_2 pt1 = RawCoordToCoord(p1);
string key2 = RawCoordToKey(p2);
Point_2 pt2 = RawCoordToCoord(p2);
VertexIndex::iterator i1 = index.find(key1);
VertexIndex::iterator i2 = index.find(key2);
Pmwx::Halfedge_handle he = Pmwx::Halfedge_handle();
#if 0
Pmwx::Locate_type loc1, loc2;
ioMap.locate(pt1, loc1);
ioMap.locate(pt2, loc2);
CGAL_precondition_msg(loc1 != Pmwx::EDGE, "Pt1 on an edge, will cause CHAOS");
CGAL_precondition_msg(loc2 != Pmwx::EDGE, "Pt2 on an edge, will cause CHAOS");
if (i1 == index.end())
CGAL_precondition_msg(loc1 != Pmwx::VERTEX, "Pt1 on an unindexed vertex, will cause CHAOS");
if (i2 == index.end())
CGAL_precondition_msg(loc2 != Pmwx::VERTEX, "Pt2 on an unindexed vertex, will cause CHAOS");
#endif
if (i1 == index.end())
{
if (i2 == index.end())
{
// Totally unknown segment.
Pmwx::Locate_type lt;
zeroV.Start();
he = ioMap.locate(pt1, lt);
CGAL_precondition_msg(lt == Pmwx::FACE || lt == Pmwx::UNBOUNDED_FACE, "Inserting a segment in unknown territory but it's NOT on a face!!");
Pmwx::Face_handle fe = (lt == Pmwx::UNBOUNDED_FACE) ? ioMap.unbounded_face() : he->face();
// he = ioMap.non_intersecting_insert(PM_Curve_2(pt1, pt2));
he = ioMap.insert_in_face_interior(PM_Curve_2(pt1, pt2), fe);
zeroV.Stop();
if (he != Pmwx::Halfedge_handle())
{
index[key1] = he->source();
index[key2] = he->target();
}
} else {
// We know pt 2 but pt 1 is floating. Make a vector
// using the vertex handle from 2 and 1's raw value.
oneV.Start();
he = ioMap.Planar_map_2::insert_from_vertex(
PM_Curve_2(i2->second->point(), pt1),
i2->second);
oneV.Stop();
// Now pt 1 gets stored...it is the target of the new halfedge.
if (he != Pmwx::Halfedge_handle())
{
index[key1] = he->target();
he = he->twin(); // This halfedge goes from 2 to 1, turn it around!
}
}
} else {
if (i2 == index.end())
{
oneV.Start();
// We know pt 1 but not pt 2
he = ioMap.Planar_map_2::insert_from_vertex(
PM_Curve_2(i1->second->point(), pt2),
i1->second);
oneV.Stop();
// Now pt 1 gets stored...it is the target of the new halfedge.
if (he != Pmwx::Halfedge_handle())
index[key2] = he->target();
} else {
twoV.Start();
// Both pts are known
he = ioMap.Planar_map_2::insert_at_vertices(
PM_Curve_2(i1->second->point(), i2->second->point()),
i1->second,
i2->second);
twoV.Stop();
}
}
if (he == Pmwx::Halfedge_handle())
{
return ioMap.halfedges_end();
}
// Whenever we create a half edge we have to pick dominance...this works.
he->mDominant = true;
return he;
}
FaceIndex
addFace (const VertexIndex& idx_v_0,
const VertexIndex& idx_v_1,
const VertexIndex& idx_v_2,
const FaceData& face_data = FaceData ())
{
if (!idx_v_0.isValid () || !idx_v_1.isValid () || !idx_v_2.isValid ())
{
return (FaceIndex ());
}
VertexIndexes vi; vi.reserve (3);
vi.push_back (idx_v_0);
vi.push_back (idx_v_1);
vi.push_back (idx_v_2);
if (!this->uniqueCheck (vi))
{
return (FaceIndex ());
}
HalfEdgeIndex idx_he_01, idx_he_12, idx_he_20;
HalfEdgeIndex idx_he_10, idx_he_21, idx_he_02;
if (Base::getElement (idx_v_0).isIsolated () && Base::getElement (idx_v_1).isIsolated () && Base::getElement (idx_v_2).isIsolated ())
{
Base::addHalfEdgePair (idx_v_0,idx_v_1, HalfEdgeData (),HalfEdgeData (), idx_he_01,idx_he_10);
Base::addHalfEdgePair (idx_v_1,idx_v_2, HalfEdgeData (),HalfEdgeData (), idx_he_12,idx_he_21);
Base::addHalfEdgePair (idx_v_2,idx_v_0, HalfEdgeData (),HalfEdgeData (), idx_he_20,idx_he_02);
Base::connectHalfEdges (true,true, idx_he_01,idx_he_10, idx_he_12,idx_he_21, idx_v_1);
Base::connectHalfEdges (true,true, idx_he_12,idx_he_21, idx_he_20,idx_he_02, idx_v_2);
Base::connectHalfEdges (true,true, idx_he_20,idx_he_02, idx_he_01,idx_he_10, idx_v_0);
return (this->connectFace (face_data, idx_he_01,idx_he_12,idx_he_20));
}
// Check for topological errors
bool is_new_01 = true;
bool is_new_12 = true;
bool is_new_20 = true;
if (!Base::checkTopology1 (idx_v_0,idx_v_1, idx_he_01, is_new_01) ||
!Base::checkTopology1 (idx_v_1,idx_v_2, idx_he_12, is_new_12) ||
!Base::checkTopology1 (idx_v_2,idx_v_0, idx_he_20, is_new_20))
{
return (FaceIndex ());
}
if (!Base::checkTopology2 (is_new_01,is_new_12, Base::getElement (idx_v_1).isIsolated ()) ||
!Base::checkTopology2 (is_new_12,is_new_20, Base::getElement (idx_v_2).isIsolated ()) ||
!Base::checkTopology2 (is_new_20,is_new_01, Base::getElement (idx_v_0).isIsolated ()))
{
return (FaceIndex ());
}
// Reconnect the existing half-edges if needed
bool make_adjacent_01_12 = false;
bool make_adjacent_12_20 = false;
bool make_adjacent_20_01 = false;
HalfEdgeIndex idx_he_boundary_01_12;
HalfEdgeIndex idx_he_boundary_12_20;
HalfEdgeIndex idx_he_boundary_20_01;
if (!Base::checkAdjacency (idx_he_01,idx_he_12, is_new_01,is_new_12, make_adjacent_01_12, idx_he_boundary_01_12) ||
!Base::checkAdjacency (idx_he_12,idx_he_20, is_new_12,is_new_20, make_adjacent_12_20, idx_he_boundary_12_20) ||
!Base::checkAdjacency (idx_he_20,idx_he_01, is_new_20,is_new_01, make_adjacent_20_01, idx_he_boundary_20_01))
{
return (FaceIndex ());
}
Base::makeAdjacent (idx_he_01,idx_he_12, make_adjacent_01_12, idx_he_boundary_01_12);
Base::makeAdjacent (idx_he_12,idx_he_20, make_adjacent_12_20, idx_he_boundary_12_20);
Base::makeAdjacent (idx_he_20,idx_he_01, make_adjacent_20_01, idx_he_boundary_20_01);
// Add the new half-edges if needed
if (is_new_01) Base::addHalfEdgePair (idx_v_0,idx_v_1, HalfEdgeData (),HalfEdgeData (), idx_he_01,idx_he_10);
else idx_he_10 = Base::getElement (idx_he_01).getOppositeHalfEdgeIndex ();
if (is_new_12) Base::addHalfEdgePair (idx_v_1,idx_v_2, HalfEdgeData (),HalfEdgeData (), idx_he_12,idx_he_21);
else idx_he_21 = Base::getElement (idx_he_12).getOppositeHalfEdgeIndex ();
if (is_new_20) Base::addHalfEdgePair (idx_v_2,idx_v_0, HalfEdgeData (),HalfEdgeData (), idx_he_20,idx_he_02);
else idx_he_02 = Base::getElement (idx_he_20).getOppositeHalfEdgeIndex ();
// Connect the half-edges and vertexes
Base::connectHalfEdges (is_new_01,is_new_12, idx_he_01,idx_he_10, idx_he_12,idx_he_21, idx_v_1);
Base::connectHalfEdges (is_new_12,is_new_20, idx_he_12,idx_he_21, idx_he_20,idx_he_02, idx_v_2);
Base::connectHalfEdges (is_new_20,is_new_01, idx_he_20,idx_he_02, idx_he_01,idx_he_10, idx_v_0);
// Connect the face
return (this->connectFace (face_data, idx_he_01,idx_he_12,idx_he_20));
}
