332 lines
13 KiB
C++
332 lines
13 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2005 Imetric 3D GmbH *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#include "PreCompiled.h"
|
|
|
|
#ifndef _PreComp_
|
|
# include <algorithm>
|
|
#endif
|
|
|
|
#include "Tools.h"
|
|
#include "Iterator.h"
|
|
|
|
|
|
using namespace MeshCore;
|
|
|
|
MeshSearchNeighbours::MeshSearchNeighbours (const MeshKernel &rclM, float fSampleDistance)
|
|
: _rclMesh(rclM),
|
|
_rclFAry(rclM.GetFacets()),
|
|
_rclPAry(rclM.GetPoints()),
|
|
_clPt2Fa(rclM),
|
|
_fSampleDistance(fSampleDistance)
|
|
{
|
|
MeshAlgorithm(_rclMesh).ResetFacetFlag(MeshFacet::MARKED);
|
|
MeshAlgorithm(_rclMesh).ResetPointFlag(MeshPoint::MARKED);
|
|
}
|
|
|
|
void MeshSearchNeighbours::Reinit (float fSampleDistance)
|
|
{
|
|
_fSampleDistance = fSampleDistance;
|
|
MeshAlgorithm(_rclMesh).ResetFacetFlag(MeshFacet::MARKED);
|
|
MeshAlgorithm(_rclMesh).ResetPointFlag(MeshPoint::MARKED);
|
|
}
|
|
|
|
unsigned long MeshSearchNeighbours::NeighboursFromFacet (unsigned long ulFacetIdx, float fDistance, unsigned long ulMinPoints, std::vector<Base::Vector3f> &raclResultPoints)
|
|
{
|
|
bool bAddPoints = false;
|
|
|
|
_fMaxDistanceP2 = fDistance * fDistance;
|
|
_clCenter = _rclMesh.GetFacet(ulFacetIdx).GetGravityPoint();
|
|
|
|
unsigned long ulVisited = 1;
|
|
std::vector<MeshFacetArray::_TConstIterator> aclTestedFacet;
|
|
|
|
_aclResult.clear();
|
|
_aclOuter.clear();
|
|
|
|
// add start facet
|
|
bool bFound = CheckDistToFacet(_rclFAry[ulFacetIdx]);
|
|
_rclFAry[ulFacetIdx].SetFlag(MeshFacet::MARKED);
|
|
aclTestedFacet.push_back(_rclFAry.begin() + ulFacetIdx);
|
|
|
|
if ((bFound == false) && (_aclResult.size() < ulMinPoints)) {
|
|
bAddPoints = true;
|
|
bFound = ExpandRadius(ulMinPoints);
|
|
}
|
|
|
|
int nCtExpandRadius = 0;
|
|
// search neighbours, add not marked facets, test distance, add outer points
|
|
MeshFacetArray::_TConstIterator f_beg = _rclFAry.begin();
|
|
while ((bFound == true) && (nCtExpandRadius < 10)) {
|
|
bFound = false;
|
|
|
|
std::set<unsigned long> aclTmp;
|
|
aclTmp.swap(_aclOuter);
|
|
for (std::set<unsigned long>::iterator pI = aclTmp.begin(); pI != aclTmp.end(); ++pI) {
|
|
const std::set<unsigned long> &rclISet = _clPt2Fa[*pI];
|
|
// search all facets hanging on this point
|
|
for (std::set<unsigned long>::const_iterator pJ = rclISet.begin(); pJ != rclISet.end(); ++pJ) {
|
|
const MeshFacet &rclF = f_beg[*pJ];
|
|
|
|
if (rclF.IsFlag(MeshFacet::MARKED) == false) {
|
|
bool bLF = CheckDistToFacet(rclF);
|
|
bFound = bFound || bLF;
|
|
rclF.SetFlag(MeshFacet::MARKED);
|
|
aclTestedFacet.push_back(f_beg+*pJ);
|
|
}
|
|
}
|
|
ulVisited += rclISet.size();
|
|
}
|
|
|
|
// too few points inside radius found -> expand radius
|
|
if ((bFound == false) && (_aclResult.size() < ulMinPoints)) {
|
|
nCtExpandRadius++;
|
|
bAddPoints = true;
|
|
bFound = ExpandRadius(ulMinPoints);
|
|
}
|
|
else
|
|
nCtExpandRadius = 0;
|
|
}
|
|
|
|
// reset marked facets, points
|
|
for (std::vector<MeshFacetArray::_TConstIterator>::iterator pF = aclTestedFacet.begin();
|
|
pF != aclTestedFacet.end(); ++pF)
|
|
(*pF)->ResetFlag(MeshFacet::MARKED);
|
|
for (std::set<unsigned long>::iterator pR = _aclResult.begin(); pR != _aclResult.end(); ++pR)
|
|
_rclPAry[*pR].ResetFlag(MeshPoint::MARKED);
|
|
|
|
|
|
// copy points in result container
|
|
raclResultPoints.resize(_aclResult.size());
|
|
int i = 0;
|
|
for (std::set<unsigned long>::iterator pI = _aclResult.begin(); pI != _aclResult.end(); ++pI, i++)
|
|
raclResultPoints[i] = _rclPAry[*pI];
|
|
|
|
if (bAddPoints == true) {
|
|
// sort points, remove points lying furthest from center
|
|
std::sort(raclResultPoints.begin(), raclResultPoints.end(), CDistRad(_clCenter));
|
|
raclResultPoints.erase(raclResultPoints.begin() + ulMinPoints, raclResultPoints.end());
|
|
}
|
|
|
|
return ulVisited;
|
|
}
|
|
|
|
void MeshSearchNeighbours::SampleAllFacets (void)
|
|
{
|
|
if (_aclSampledFacets.size() == _rclMesh.CountFacets())
|
|
return; // already sampled, do nothing
|
|
|
|
_aclSampledFacets.resize(_rclMesh.CountFacets());
|
|
MeshFacetIterator clFIter(_rclMesh);
|
|
int i = 0;
|
|
for (clFIter.Init(); clFIter.More(); clFIter.Next(), i++) {
|
|
std::vector<Base::Vector3f> clPoints;
|
|
clFIter->SubSample(_fSampleDistance, clPoints);
|
|
_aclSampledFacets[i].resize(clPoints.size());
|
|
std::copy(clPoints.begin(), clPoints.end(), _aclSampledFacets[i].begin());
|
|
}
|
|
}
|
|
|
|
unsigned long MeshSearchNeighbours::NeighboursFromSampledFacets (unsigned long ulFacetIdx, float fDistance, std::vector<Base::Vector3f> &raclResultPoints)
|
|
{
|
|
SampleAllFacets();
|
|
|
|
_fMaxDistanceP2 = fDistance * fDistance;
|
|
_clCenter = _rclMesh.GetFacet(ulFacetIdx).GetGravityPoint();
|
|
|
|
_akSphere.Center = Wm4::Vector3<float>(_clCenter.x, _clCenter.y, _clCenter.z);
|
|
_akSphere.Radius = fDistance;
|
|
|
|
unsigned long ulVisited = 1;
|
|
std::vector<MeshFacetArray::_TConstIterator> aclTestedFacet;
|
|
|
|
_aclResult.clear();
|
|
_aclOuter.clear();
|
|
_aclPointsResult.clear();
|
|
|
|
// add start facet
|
|
bool bFound = AccumulateNeighbours(_rclFAry[ulFacetIdx], ulFacetIdx);
|
|
_rclFAry[ulFacetIdx].SetFlag(MeshFacet::MARKED);
|
|
|
|
// search neighbours, add not marked facets, test distance, add outer points
|
|
MeshFacetArray::_TConstIterator f_beg = _rclFAry.begin();
|
|
while (bFound == true) {
|
|
bFound = false;
|
|
|
|
std::set<unsigned long> aclTmp;
|
|
aclTmp.swap(_aclOuter);
|
|
for (std::set<unsigned long>::iterator pI = aclTmp.begin(); pI != aclTmp.end(); ++pI) {
|
|
const std::set<unsigned long> &rclISet = _clPt2Fa[*pI];
|
|
// search all facets hanging on this point
|
|
for (std::set<unsigned long>::const_iterator pJ = rclISet.begin(); pJ != rclISet.end(); ++pJ) {
|
|
const MeshFacet &rclF = f_beg[*pJ];
|
|
|
|
if (rclF.IsFlag(MeshFacet::MARKED) == false) {
|
|
bool bLF = AccumulateNeighbours(rclF, *pJ);
|
|
bFound = bFound || bLF;
|
|
rclF.SetFlag(MeshFacet::MARKED);
|
|
aclTestedFacet.push_back(f_beg+*pJ);
|
|
}
|
|
}
|
|
ulVisited += rclISet.size();
|
|
}
|
|
}
|
|
|
|
// reset marked facets
|
|
for (std::vector<MeshFacetArray::_TConstIterator>::iterator pF = aclTestedFacet.begin(); pF != aclTestedFacet.end(); ++pF)
|
|
(*pF)->ResetFlag(MeshFacet::MARKED);
|
|
|
|
// copy points in result container
|
|
raclResultPoints.resize(_aclPointsResult.size());
|
|
std::copy(_aclPointsResult.begin(), _aclPointsResult.end(), raclResultPoints.begin());
|
|
|
|
// facet points
|
|
for (std::set<unsigned long>::iterator pI = _aclResult.begin(); pI != _aclResult.end(); ++pI) {
|
|
if (InnerPoint(_rclPAry[*pI]) == true)
|
|
raclResultPoints.push_back(_rclPAry[*pI]);
|
|
}
|
|
|
|
return ulVisited;
|
|
}
|
|
|
|
bool MeshSearchNeighbours::AccumulateNeighbours (const MeshFacet &rclF, unsigned long ulFIdx)
|
|
{
|
|
int k = 0;
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
unsigned long ulPIdx = rclF._aulPoints[i];
|
|
_aclOuter.insert(ulPIdx);
|
|
_aclResult.insert(ulPIdx);
|
|
|
|
if (Base::DistanceP2(_clCenter, _rclPAry[ulPIdx]) < _fMaxDistanceP2)
|
|
k++;
|
|
}
|
|
|
|
bool bFound = false;
|
|
if (k == 3) { // add all sample points
|
|
_aclPointsResult.insert(_aclPointsResult.end(), _aclSampledFacets[ulFIdx].begin(), _aclSampledFacets[ulFIdx].end());
|
|
bFound = true;
|
|
}
|
|
else { // add points inner radius
|
|
bFound = TriangleCutsSphere(rclF);
|
|
|
|
if (bFound == true) {
|
|
std::vector<Base::Vector3f> &rclT = _aclSampledFacets[ulFIdx];
|
|
std::vector<Base::Vector3f> clTmp;
|
|
clTmp.reserve(rclT.size());
|
|
for (std::vector<Base::Vector3f>::iterator pI = rclT.begin(); pI != rclT.end(); ++pI) {
|
|
if (InnerPoint(*pI) == true)
|
|
clTmp.push_back(*pI);
|
|
}
|
|
_aclPointsResult.insert(_aclPointsResult.end(), clTmp.begin(), clTmp.end());
|
|
}
|
|
}
|
|
|
|
return bFound;
|
|
}
|
|
|
|
bool MeshSearchNeighbours::ExpandRadius (unsigned long ulMinPoints)
|
|
{
|
|
// add facets from current level
|
|
_aclResult.insert(_aclOuter.begin(), _aclOuter.end());
|
|
for (std::set<unsigned long>::iterator pI = _aclOuter.begin(); pI != _aclOuter.end(); ++pI)
|
|
_rclPAry[*pI].SetFlag(MeshPoint::MARKED);
|
|
|
|
if (_aclResult.size() < ulMinPoints) {
|
|
_fMaxDistanceP2 *= float(ulMinPoints) / float(_aclResult.size());
|
|
return true;
|
|
}
|
|
else
|
|
return false;
|
|
}
|
|
|
|
unsigned long MeshSearchNeighbours::NeighboursFacetFromFacet (unsigned long ulFacetIdx, float fDistance, std::vector<Base::Vector3f> &raclResultPoints, std::vector<unsigned long> &raclResultFacets)
|
|
{
|
|
std::set<unsigned long> aulFacetSet;
|
|
|
|
_fMaxDistanceP2 = fDistance * fDistance;
|
|
_clCenter = _rclMesh.GetFacet(ulFacetIdx).GetGravityPoint();
|
|
|
|
unsigned long ulVisited = 1;
|
|
std::vector<MeshFacetArray::_TConstIterator> aclTestedFacet;
|
|
|
|
_aclResult.clear();
|
|
_aclOuter.clear();
|
|
|
|
// add start facet
|
|
bool bFound = CheckDistToFacet(_rclFAry[ulFacetIdx]);
|
|
_rclFAry[ulFacetIdx].SetFlag(MeshFacet::MARKED);
|
|
aclTestedFacet.push_back(_rclFAry.begin() + ulFacetIdx);
|
|
|
|
aulFacetSet.insert(ulFacetIdx);
|
|
|
|
// search neighbours, add not marked facets, test distance, add outer points
|
|
MeshFacetArray::_TConstIterator f_beg = _rclFAry.begin();
|
|
while (bFound == true) {
|
|
bFound = false;
|
|
|
|
std::set<unsigned long> aclTmp;
|
|
aclTmp.swap(_aclOuter);
|
|
for (std::set<unsigned long>::iterator pI = aclTmp.begin(); pI != aclTmp.end(); ++pI) {
|
|
const std::set<unsigned long> &rclISet = _clPt2Fa[*pI];
|
|
// search all facets hanging on this point
|
|
for (std::set<unsigned long>::const_iterator pJ = rclISet.begin(); pJ != rclISet.end(); ++pJ) {
|
|
const MeshFacet &rclF = f_beg[*pJ];
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
if (Base::DistanceP2(_clCenter, _rclPAry[rclF._aulPoints[i]]) < _fMaxDistanceP2) {
|
|
aulFacetSet.insert(*pJ);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (rclF.IsFlag(MeshFacet::MARKED) == false) {
|
|
bool bLF = CheckDistToFacet(rclF);
|
|
|
|
bFound = bFound || bLF;
|
|
rclF.SetFlag(MeshFacet::MARKED);
|
|
aclTestedFacet.push_back(f_beg+*pJ);
|
|
}
|
|
}
|
|
ulVisited += rclISet.size();
|
|
}
|
|
}
|
|
|
|
// reset marked facets, points
|
|
for (std::vector<MeshFacetArray::_TConstIterator>::iterator pF = aclTestedFacet.begin(); pF != aclTestedFacet.end(); ++pF)
|
|
(*pF)->ResetFlag(MeshFacet::MARKED);
|
|
for (std::set<unsigned long>::iterator pR = _aclResult.begin(); pR != _aclResult.end(); ++pR)
|
|
_rclPAry[*pR].ResetFlag(MeshPoint::MARKED);
|
|
|
|
// copy points in result container
|
|
raclResultPoints.resize(_aclResult.size());
|
|
int i = 0;
|
|
for (std::set<unsigned long>::iterator pI = _aclResult.begin(); pI != _aclResult.end(); ++pI, i++)
|
|
raclResultPoints[i] = _rclPAry[*pI];
|
|
|
|
// copy facets in result container
|
|
raclResultFacets.insert(raclResultFacets.begin(), aulFacetSet.begin(), aulFacetSet.end());
|
|
|
|
return ulVisited;
|
|
}
|