void
pcl::ihs::Integration::addToMesh (const CloudModel::const_iterator& it_pt_0,
const CloudModel::const_iterator& it_pt_1,
const CloudModel::const_iterator& it_pt_2,
const CloudModel::const_iterator& it_pt_3,
const VertexIndexes::iterator& it_vi_0,
const VertexIndexes::iterator& it_vi_1,
const VertexIndexes::iterator& it_vi_2,
const VertexIndexes::iterator& it_vi_3,
const MeshPtr& mesh) const
{
// Treated bitwise
// 2 - 1
// | |
// 3 - 0
const unsigned char is_finite = static_cast <unsigned char> ((1 * pcl::isFinite (*it_pt_0)) | (2 * pcl::isFinite (*it_pt_1)) | (4 * pcl::isFinite (*it_pt_2)) | (8 * pcl::isFinite (*it_pt_3)));
switch (is_finite)
{
case 7: this->addToMesh (it_pt_0, it_pt_1, it_pt_2, it_vi_0, it_vi_1, it_vi_2, mesh); break; // 0-1-2
case 11: this->addToMesh (it_pt_0, it_pt_1, it_pt_3, it_vi_0, it_vi_1, it_vi_3, mesh); break; // 0-1-3
case 13: this->addToMesh (it_pt_0, it_pt_2, it_pt_3, it_vi_0, it_vi_2, it_vi_3, mesh); break; // 0-2-3
case 14: this->addToMesh (it_pt_1, it_pt_2, it_pt_3, it_vi_1, it_vi_2, it_vi_3, mesh); break; // 1-2-3
case 15: // 0-1-2-3
{
if (!distanceThreshold (*it_pt_0, *it_pt_1, *it_pt_2, *it_pt_3)) break;
if (!it_vi_0->isValid ()) *it_vi_0 = mesh->addVertex (*it_pt_0);
if (!it_vi_1->isValid ()) *it_vi_1 = mesh->addVertex (*it_pt_1);
if (!it_vi_2->isValid ()) *it_vi_2 = mesh->addVertex (*it_pt_2);
if (!it_vi_3->isValid ()) *it_vi_3 = mesh->addVertex (*it_pt_3);
mesh->addFace (*it_vi_0, *it_vi_1, *it_vi_2, *it_vi_3);
break;
}
}
}
void
pcl::ihs::Integration::addToMesh (const CloudModel::const_iterator& it_pt_0,
const CloudModel::const_iterator& it_pt_1,
const CloudModel::const_iterator& it_pt_2,
const VertexIndexes::iterator& it_vi_0,
const VertexIndexes::iterator& it_vi_1,
const VertexIndexes::iterator& it_vi_2,
const MeshPtr& mesh) const
{
if (!distanceThreshold (*it_pt_0, *it_pt_1, *it_pt_2)) return;
if (!it_vi_0->isValid ()) *it_vi_0 = mesh->addVertex (*it_pt_0);
if (!it_vi_1->isValid ()) *it_vi_1 = mesh->addVertex (*it_pt_1);
if (!it_vi_2->isValid ()) *it_vi_2 = mesh->addVertex (*it_pt_2);
mesh->addFace (*it_vi_0, *it_vi_1, *it_vi_2);
}
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);
}