Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[SofaBaseMechanics] BarycentricMapping: spatial hashing, handle limit cases #896

Merged
merged 7 commits into from Feb 6, 2019
Merged

[SofaBaseMechanics] BarycentricMapping: spatial hashing, handle limit cases #896

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ff99818
[SofaBaseMechanics] fixed spatial hashing in case of multiple grid ce…
EulalieCoevoet Jan 15, 2019
fc185a8
[SofaBaseMechanics] BarycentricMapping: check neighbors if nearest el…
EulalieCoevoet Jan 16, 2019
116c0ba
[SofaBaseMechanics] BarycentricMapperTopologyContainer: cleaning & re…
EulalieCoevoet Jan 17, 2019
7de9171
[SofaBaseMechanics] BarycentricMapperTopologyContainer: fix windows c…
EulalieCoevoet Jan 21, 2019
2bfcb76
[SofaBaseMechanics] added doxygen comment and changes regarding review
EulalieCoevoet Jan 23, 2019
9348ca4
[SofaBaseMechanics] BarycentricMapper: minor changes
EulalieCoevoet Jan 23, 2019
c263054
[SofaBaseMechanics] uses unordered_map
EulalieCoevoet Feb 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
60 changes: 52 additions & 8 deletions SofaKernel/modules/SofaBaseMechanics/BarycentricMappers/BarycentricMapperTopologyContainer.h
View file
Open in desktop
Expand Up @@ -39,6 +39,7 @@ namespace _barycentricmappertopologycontainer_

using sofa::defaulttype::Mat3x3d;
using sofa::defaulttype::Vector3;
using sofa::defaulttype::Vec3i;
using sofa::defaulttype::Vec3dTypes;
using sofa::defaulttype::Vec3fTypes;
using sofa::defaulttype::ExtVec3fTypes;
Expand Down Expand Up @@ -90,17 +91,47 @@ class BarycentricMapperTopologyContainer : public TopologyBarycentricMapper<In,O

protected:

struct HashEntry
{
HashEntry(const int& xId, const int& yId, const int& zId, const int& elementId)
{
this->xId=xId;
this->yId=yId;
this->zId=zId;
this->elementId=elementId;
}

int xId,yId,zId; // cell indices
unsigned int elementId;
};

struct NearestParams
{
NearestParams()
{
distance = std::numeric_limits<double>::max();
elementId = std::numeric_limits<unsigned int>::max();
}

Vector3 baryCoords;
double distance;
unsigned int elementId;
};

using Inherit1::m_fromTopology;

topology::PointData< helper::vector<MappingDataType > > d_map;
MatrixType* m_matrixJ {nullptr};
bool m_updateJ {false};

helper::vector<Mat3x3d> m_bases;
helper::vector<Vector3> m_centers;

// Spacial hashing utils
Real m_gridCellSize;
Real m_convFactor;
unsigned int m_hashTableSize;
helper::vector<helper::vector<unsigned int>> m_hashTable;
helper::vector<helper::vector<HashEntry>> m_hashTable;
damienmarchal marked this conversation as resolved.
Show resolved Hide resolved

BarycentricMapperTopologyContainer(core::topology::BaseMeshTopology* fromTopology, topology::PointSetTopologyContainer* toTopology);

Expand All @@ -113,20 +144,33 @@ class BarycentricMapperTopologyContainer : public TopologyBarycentricMapper<In,O
virtual void addPointInElement(const int elementIndex, const SReal* baryCoords)=0;
virtual void computeDistance(double& d, const Vector3& v)=0;

void exhaustiveSearch ( defaulttype::Vec3d outPos,
const typename In::VecCoord& in,
const helper::vector<Mat3x3d>& bases,
const helper::vector<Vector3>& centers);
/// Compute the distance between outPos and the element e. If this distance is smaller than the previously stored one,
/// update nearestParams.
/// \param e id of the element
/// \param outPos position of the point we want to compute the barycentric coordinates
/// \param inPos position of one point of the element
/// \param nearestParams output parameters (nearest element id, distance, and barycentric coordinates)
void checkDistanceFromElement(unsigned int e,
const Vector3& outPos,
const Vector3& inPos,
NearestParams& nearestParams);


/// Compute the datas needed to find the nearest element
/// \param in is the vector of points
void computeBasesAndCenters( const typename In::VecCoord& in );
epernod marked this conversation as resolved.
Show resolved Hide resolved

// Spacial hashing following paper:
// M.Teschner et al "Optimized Spatial Hashing for Collision Detection of Deformable Objects" (2003)
unsigned int getHashIndexFromCoord(const Vector3& x);
unsigned int getHashIndexFromCoord(const Vector3& pos);
unsigned int getHashIndexFromIndices(const Vec3i& ids);
unsigned int getHashIndexFromIndices(const int& x, const int& y, const int& z);
defaulttype::Vec3i getGridIndices(const Vector3& x);
void addToHashTable(const unsigned int& hId, const unsigned int& vertexId);
defaulttype::Vec3i getGridIndices(const Vector3& pos);
void initHashing(const typename In::VecCoord& in);
void computeHashingCellSize(const typename In::VecCoord& in);
void computeHashTable(const typename In::VecCoord& in);
bool sameGridCell(const HashEntry& entry, const Vec3i& gridIds);

};

#if !defined(SOFA_COMPONENT_MAPPING_BARYCENTRICMAPPERTOPOLOGYCONTAINER_CPP)
Expand Down
184 changes: 103 additions & 81 deletions ...ernel/modules/SofaBaseMechanics/BarycentricMappers/BarycentricMapperTopologyContainer.inl
View file
Open in desktop
Expand Up @@ -39,7 +39,7 @@ namespace _barycentricmappertopologycontainer_

using defaulttype::Vec3d;
using defaulttype::Vec3i;
typedef typename sofa::core::topology::BaseMeshTopology::SeqEdges SeqEdges;
typedef typename core::topology::BaseMeshTopology::SeqEdges SeqEdges;

template <class In, class Out, class MappingDataType, class Element>
BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::BarycentricMapperTopologyContainer(core::topology::BaseMeshTopology* fromTopology,
Expand Down Expand Up @@ -120,11 +120,11 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::compute

for(unsigned int j=0; j<element.size(); j++)
{
unsigned int elementId = element[j];
unsigned int pointId = element[j];
for(int k=0; k<3; k++)
{
if(in[elementId][k]<min[k]) min[k]=in[elementId][k];
if(in[elementId][k]>max[k]) max[k]=in[elementId][k];
if(in[pointId][k]<min[k]) min[k]=in[pointId][k];
if(in[pointId][k]>max[k]) max[k]=in[pointId][k];
}
}

Expand All @@ -136,7 +136,8 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::compute
for(int l=i_min[2]; l<=i_max[2]; l++)
{
unsigned int h = getHashIndexFromIndices(j,k,l);
addToHashTable(h, i);
HashEntry entry(j,k,l,i);
m_hashTable[h].push_back(entry);
}
}
}
Expand All @@ -146,94 +147,109 @@ template <class In, class Out, class MappingDataType, class Element>
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::init ( const typename Out::VecCoord& out, const typename In::VecCoord& in )
{
initHashing(in);

const helper::vector<Element>& elements = getElements();
helper::vector<Mat3x3d> bases;
helper::vector<Vector3> centers;

this->clear ( int(out.size()) );
bases.resize ( elements.size() );
centers.resize ( elements.size() );

// Compute bases and centers of each element
for ( unsigned int e = 0; e < elements.size(); e++ )
{
Element element = elements[e];

Mat3x3d base;
computeBase(base,in,element);
bases[e] = base;

Vector3 center;
computeCenter(center,in,element);
centers[e] = center;
}
computeBasesAndCenters(in);

// Compute distances to get nearest element and corresponding bary coef
const helper::vector<Element>& elements = getElements();
for ( unsigned int i=0; i<out.size(); i++ )
{
Vec3d outPos = Out::getCPos(out[i]);
Vector3 baryCoords;
int elementIndex = -1;
double distance = std::numeric_limits<double>::max();
Vector3 outPos = Out::getCPos(out[i]);
NearestParams nearestParams;

// Search nearest element in grid cell
Vec3i gridIds = getGridIndices(outPos);
unsigned int h = getHashIndexFromCoord(outPos);
for ( unsigned int j=0; j<m_hashTable[h].size(); j++)
for (unsigned int j=0; j<m_hashTable[h].size(); j++)
{
unsigned int e = m_hashTable[h][j];
Vec3d bary = bases[e] * ( outPos - in[elements[e][0]] );
double dist;
computeDistance(dist, bary);
if ( dist>0 )
dist = ( outPos-centers[e] ).norm2();
if ( dist<distance )
HashEntry entry = m_hashTable[h][j];
if(sameGridCell(entry, gridIds))
{
baryCoords = bary;
distance = dist;
elementIndex = int(e);
unsigned int e = entry.elementId;
Vector3 inPos = in[elements[e][0]];
checkDistanceFromElement(e, outPos, inPos, nearestParams);
}
}

if(elementIndex==-1)
if(nearestParams.elementId==UINT_MAX) // No element in grid cell, perform exhaustive search
{
exhaustiveSearch(outPos, in, bases, centers);
for ( unsigned int e = 0; e < elements.size(); e++ )
{
Vector3 inPos = in[elements[e][0]];
checkDistanceFromElement(e, outPos, inPos, nearestParams);
}
addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
}
else if(abs(nearestParams.distance)>m_gridCellSize/2.) // Nearest element in grid cell may not be optimal, check neighbors
{
Vec3i centerGridIds = gridIds;
for(int xId=-1; xId<=1; xId++)
for(int yId=-1; yId<=1; yId++)
for(int zId=-1; zId<=1; zId++)
{
gridIds = Vec3i(centerGridIds[0]+xId,centerGridIds[1]+yId,centerGridIds[2]+zId);
h = getHashIndexFromIndices(gridIds);
for ( unsigned int j=0; j<m_hashTable[h].size(); j++)
{
HashEntry entry = m_hashTable[h][j];
if(sameGridCell(entry, gridIds))
{
unsigned int e = entry.elementId;
Vector3 inPos = in[elements[e][0]];
checkDistanceFromElement(e, outPos, inPos, nearestParams);
}
}
}
addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
}
else
addPointInElement(elementIndex, baryCoords.ptr());
addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
}
}


template <class In, class Out, class MappingDataType, class Element>
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::exhaustiveSearch ( Vec3d outPos,
const typename In::VecCoord& in,
const helper::vector<Mat3x3d>& bases,
const helper::vector<Vector3>& centers)
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::computeBasesAndCenters( const typename In::VecCoord& in )
{
const helper::vector<Element>& elements = getElements();
m_bases.resize ( elements.size() );
m_centers.resize ( elements.size() );

// Compute distances to get nearest element and corresponding bary coef
Vector3 baryCoords;
int elementIndex = -1;
double distance = std::numeric_limits<double>::max();
for ( unsigned int e = 0; e < elements.size(); e++ )
{
Vec3d bary = bases[e] * ( outPos - in[elements[e][0]] );
double dist;
computeDistance(dist, bary);
if ( dist>0 )
dist = ( outPos-centers[e] ).norm2();
if ( dist<distance )
{
baryCoords = bary;
distance = dist;
elementIndex = e;
}
Element element = elements[e];

Mat3x3d base;
computeBase(base,in,element);
m_bases[e] = base;

Vector3 center;
computeCenter(center,in,element);
m_centers[e] = center;
}
addPointInElement(elementIndex, baryCoords.ptr());
}


template <class In, class Out, class MappingDataType, class Element>
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::checkDistanceFromElement(unsigned int e,
const Vector3& outPos,
const Vector3& inPos,
NearestParams& nearestParams)
{
Vector3 bary = m_bases[e] * ( outPos - inPos);
double dist;
computeDistance(dist, bary);
if ( dist>0 )
dist = ( outPos-m_centers[e] ).norm2();
if ( dist<nearestParams.distance )
{
nearestParams.baryCoords = bary;
nearestParams.distance = dist;
nearestParams.elementId = e;
}
};


template <class In, class Out, class MappingDataType, class Element>
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::applyJT ( typename In::MatrixDeriv& out, const typename Out::MatrixDeriv& in )
{
Expand All @@ -252,7 +268,7 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::applyJT
for ( ; colIt != colItEnd; ++colIt)
{
unsigned indexIn = colIt.index();
InDeriv data = (InDeriv) Out::getDPos(colIt.val());
InDeriv data = InDeriv(Out::getDPos(colIt.val()));

const Element& element = elements[d_map.getValue()[indexIn].in_index];

Expand All @@ -267,13 +283,13 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::applyJT


template <class In, class Out, class MappingDataType, class Element>
const sofa::defaulttype::BaseMatrix* BarycentricMapperTopologyContainer<In,Out,MappingDataType, Element>::getJ(int outSize, int inSize)
const defaulttype::BaseMatrix* BarycentricMapperTopologyContainer<In,Out,MappingDataType, Element>::getJ(int outSize, int inSize)
{
if (m_matrixJ && !m_updateJ)
return m_matrixJ;

if (!m_matrixJ) m_matrixJ = new MatrixType;
if (m_matrixJ->rowBSize() != (MatrixTypeIndex)outSize || m_matrixJ->colBSize() != (MatrixTypeIndex)inSize)
if (m_matrixJ->rowBSize() != MatrixTypeIndex(outSize) || m_matrixJ->colBSize() != MatrixTypeIndex(inSize))
m_matrixJ->resize(outSize*NOut, inSize*NIn);
else
m_matrixJ->clear();
Expand Down Expand Up @@ -406,15 +422,21 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::draw (
}
}
}
vparams->drawTool()->drawLines ( points, 1, sofa::defaulttype::Vec<4,float> ( 0,1,0,1 ) );
vparams->drawTool()->drawLines ( points, 1, defaulttype::Vec<4,float> ( 0,1,0,1 ) );
}


template <class In, class Out, class MappingDataType, class Element>
unsigned int BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::getHashIndexFromCoord(const Vector3& x)
unsigned int BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::getHashIndexFromCoord(const Vector3& pos)
{
Vec3i v = getGridIndices(x);
return getHashIndexFromIndices(v[0],v[1],v[2]);
Vec3i gridIds = getGridIndices(pos);
return getHashIndexFromIndices(gridIds[0],gridIds[1],gridIds[2]);
}

template <class In, class Out, class MappingDataType, class Element>
unsigned int BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::getHashIndexFromIndices(const Vec3i& ids)
{
return getHashIndexFromIndices(ids[0],ids[1],ids[2]);
}

template <class In, class Out, class MappingDataType, class Element>
Expand All @@ -426,33 +448,33 @@ unsigned int BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>:
if(h<0)
h += m_hashTableSize;

return h;
return static_cast<unsigned int>(h);
epernod marked this conversation as resolved.
Show resolved Hide resolved
}

template <class In, class Out, class MappingDataType, class Element>
Vec3i BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::getGridIndices(const Vector3& x)
bool BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::sameGridCell(const HashEntry& entry, const Vec3i& gridIds)
{
return (gridIds[0]==entry.xId && gridIds[1]==entry.yId && gridIds[2]==entry.zId);
}

template <class In, class Out, class MappingDataType, class Element>
Vec3i BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::getGridIndices(const Vector3& pos)
{
Vec3i i_x;
for(int i=0; i<3; i++)
i_x[i]=floor(x[i]*m_convFactor);
i_x[i]=floor(pos[i]*m_convFactor);

return i_x;
}

template <class In, class Out, class MappingDataType, class Element>
void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::addToHashTable(const unsigned int& hId, const unsigned int& vertexId)
{
if(hId<m_hashTableSize)
m_hashTable[hId].push_back(vertexId);
}

template<class In, class Out, class MappingData, class Element>
std::istream& operator >> ( std::istream& in, BarycentricMapperTopologyContainer<In, Out, MappingData, Element> &b )
{
unsigned int size_vec;

in >> size_vec;
sofa::helper::vector<MappingData>& m = *(b.d_map.beginEdit());
helper::vector<MappingData>& m = *(b.d_map.beginEdit());
m.clear();

MappingData value;
Expand Down