Updates will be applied on October 27th between 12pm - 12:45pm EDT (UTC-0400). Gitlab may be slow during the maintenance window.

Commit 3292223b authored by Nghia Truong's avatar Nghia Truong
Browse files

ENH: Implement parallel constant density constraints

parent ee2703ca
......@@ -21,14 +21,15 @@ limitations under the License.
#include "imstkPbdConstantDensityConstraint.h"
#include "imstkPbdModel.h"
#include "imstkParallelUtils.h"
namespace imstk
{
void
PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double)
{
const auto state = model.getCurrentState();
const auto np = state->getPositions().size();
const auto& state = model.getCurrentState();
const auto numParticles = state->getPositions().size();
// constraint parameters
// Refer: Miller, et al 2003 "Particle-Based Fluid Simulation for Interactive Applications."
......@@ -36,148 +37,122 @@ PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
m_wPoly6Coeff = 315.0 / (64.0 * PI * pow(m_maxDist, 9));
m_wSpikyCoeff = 15.0 / (PI * pow(m_maxDist, 6));
m_lambdas.resize(np);
m_densities.resize(np);
m_numNeighbors.resize(np);
m_deltaPositions.resize(np);
m_neighbors.resize(np * m_maxNumNeighbors);
m_lambdas.resize(numParticles);
m_densities.resize(numParticles);
m_deltaPositions.resize(numParticles);
m_neighborList.resize(numParticles);
// Initialize neighbor searcher
m_NeighborSearcher = std::make_shared<NeighborSearch>(m_NeighborSearchMethod, m_maxDist);
}
bool
PbdConstantDensityConstraint::solvePositionConstraint(PbdModel& model)
{
const auto state = model.getCurrentState();
auto& pos = state->getPositions();
const auto np = pos.size();
const auto& state = model.getCurrentState();
auto& positions = state->getPositions();
const auto numParticles = positions.size();
clearNeighbors(np);
// Search neighbor for each particle
m_NeighborSearcher->getNeighbors(m_neighborList, positions);
for (auto index = 0; index < np; ++index)
{
this->updateNeighborsBruteForce(pos[index], index, pos);
}
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx){
computeDensity(positions[idx], idx, positions);
});
for (auto index = 0; index < np; ++index)
{
this->calculateDensityEstimate(pos[index], index, pos);
}
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx){
computeLambdaScalingFactor(positions[idx], idx, positions);
});
for (auto index = 0; index < np; ++index)
{
this->calculateLambdaScalingFactor(pos[index], index, pos);
}
for (auto index = 0; index < np; ++index)
{
this->updatePositions(pos[index], index, pos);
}
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx){
updatePositions(positions[idx], idx, positions);
});
return true;
}
inline double
double
PbdConstantDensityConstraint::wPoly6(const Vec3d &pi, const Vec3d &pj)
{
double rLength = (pi - pj).norm();
double rLengthSqr = (pi - pj).squaredNorm();
return (rLength > m_maxDist || rLength == 0) ?
return (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20) ?
0 :
m_wPoly6Coeff * pow((m_maxDist * m_maxDist) - (rLength * rLength), 3);
m_wPoly6Coeff * pow(m_maxDistSqr - rLengthSqr, 3);
}
inline Vec3d
Vec3d
PbdConstantDensityConstraint::gradSpiky(const Vec3d &pi, const Vec3d &pj)
{
Vec3d r = pi - pj;
const double rLength = r.norm();
return (rLength > m_maxDist || rLength == 0) ?
Vec3d(0., 0., 0.) :
r * (m_wSpikyCoeff * (-3.0) * (m_maxDist - rLength) * (m_maxDist - rLength));
}
inline void
PbdConstantDensityConstraint::clearNeighbors(const size_t np)
{
m_numNeighbors.clear();
m_neighbors.clear();
m_numNeighbors.resize(np);
m_neighbors.resize(np * m_maxNumNeighbors);
}
inline void
PbdConstantDensityConstraint::updateNeighborsBruteForce(const Vec3d &pi,
const size_t index,
const StdVectorOfVec3d &positions)
{
const double neighborRadius = m_maxDist;
int neighborCount = 0;
const double rLengthSqr = r.squaredNorm();
//loop over all points
for (auto j = 0; j < positions.size(); ++j)
if (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20)
{
if (j != index)
{
if (neighborCount >= m_maxNumNeighbors)
{
LOG(WARNING) << "Neighbor count reached max. for point: " << index;
continue;
}
if ((pi - positions[j]).norm() < neighborRadius)
{
m_neighbors[index * m_maxNumNeighbors + neighborCount] = j;
neighborCount++;
}
}
return Vec3d(0., 0., 0.);
}
m_numNeighbors[index] = neighborCount;
const double rLength = std::sqrt(rLengthSqr);
return r * (m_wSpikyCoeff * (-3.0) * (m_maxDist - rLength) * (m_maxDist - rLength));
}
inline void
PbdConstantDensityConstraint::calculateDensityEstimate(const Vec3d &pi,
const size_t index,
const StdVectorOfVec3d &positions)
void
PbdConstantDensityConstraint::computeDensity(const Vec3d &pi,
const size_t index,
const StdVectorOfVec3d &positions)
{
double densitySum = 0.0;
for (int j = 0; j < m_numNeighbors[index]; j++)
for (auto q : m_neighborList[index])
{
densitySum += wPoly6(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
densitySum += wPoly6(pi, positions[q]);
}
m_densities[index] = densitySum;
}
inline void
PbdConstantDensityConstraint::calculateLambdaScalingFactor(const Vec3d &pi,
const size_t index,
const StdVectorOfVec3d &positions)
void
PbdConstantDensityConstraint::computeLambdaScalingFactor(const Vec3d &pi,
const size_t index,
const StdVectorOfVec3d &positions)
{
const double densityConstraint = (m_densities[index] / m_restDensity) - 1;
double gradientSum = 0.0;
for (int j = 0; j < m_numNeighbors[index]; j++)
for (auto q : m_neighborList[index])
{
gradientSum += gradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]).squaredNorm() / m_restDensity;
gradientSum += gradSpiky(pi, positions[q]).squaredNorm() / m_restDensity;
}
m_lambdas[index] = densityConstraint / (gradientSum + m_relaxationParameter);
}
inline void
void
PbdConstantDensityConstraint::updatePositions(const Vec3d &pi,
const size_t index,
StdVectorOfVec3d &positions)
{
//Make sure the point is valid
Vec3d gradientLambdaSum(0., 0., 0.);
for (int j = 0; j < m_numNeighbors[index]; j++)
for (auto q : m_neighborList[index])
{
double lambdasDiff = (m_lambdas[index] + m_lambdas[m_neighbors[index * m_maxNumNeighbors + j]]);
Vec3d gradKernal = gradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
double lambdasDiff = (m_lambdas[index] + m_lambdas[q]);
Vec3d gradKernal = gradSpiky(pi, positions[q]);
gradientLambdaSum += (gradKernal * lambdasDiff);
}
m_deltaPositions[index] = gradientLambdaSum / m_restDensity;
positions[index] += m_deltaPositions[index];
}
} // imstk
\ No newline at end of file
void PbdConstantDensityConstraint::setMaxNeighborDistance(const double dist)
{
m_maxDist = dist;
m_maxDistSqr = dist * dist;
if (m_NeighborSearcher)
{
m_NeighborSearcher->setSearchRadius(m_maxDist);
}
}
} // imstk
......@@ -23,6 +23,7 @@ limitations under the License.
#define imstkPbdConstantDensityConstraint_h
#include "imstkPbdConstraint.h"
#include "imstkNeighborSearch.h"
namespace imstk
{
......@@ -70,25 +71,15 @@ private:
///
Vec3d gradSpiky(const Vec3d &pi, const Vec3d &pj);
///
/// \brief Update the neighbors of each node using burte force search O(n*n)
///
void updateNeighborsBruteForce(const Vec3d &pi, const size_t index, const StdVectorOfVec3d &positions);
///
/// \brief Clear the list of neighbors
///
void clearNeighbors(const size_t np);
///
/// \brief
///
void calculateDensityEstimate(const Vec3d &pi, const size_t index, const StdVectorOfVec3d &positions);
void computeDensity(const Vec3d &pi, const size_t index, const StdVectorOfVec3d &positions);
///
/// \brief
///
void calculateLambdaScalingFactor(const Vec3d &pi, const size_t index, const StdVectorOfVec3d &positions);
void computeLambdaScalingFactor(const Vec3d &pi, const size_t index, const StdVectorOfVec3d &positions);
///
/// \brief
......@@ -104,23 +95,32 @@ private:
///
/// \brief Set/Get max. neighbor distance
///
void setMaxNeighborDistance(const double dist) { m_maxDist = dist; }
double getMaxNeighborDistance() { return m_restDensity; }
void setMaxNeighborDistance(const double dist);
double getMaxNeighborDistance() { return m_maxDist; }
///
/// \brief Set/Get neighbor search method
///
void setNeighborSearchMethod(NeighborSearch::Method method) { m_NeighborSearchMethod = method; }
NeighborSearch::Method getNeighborSearchMethod() { return m_NeighborSearchMethod; }
private:
double m_wPoly6Coeff;
double m_wSpikyCoeff;
double m_maxDist = 0.2; ///> Max. neighbor distance
double m_maxDistSqr = 0.04; ///> Max. neighbor squared distance
double m_relaxationParameter = 600.0; ///> Relaxation parameter
double m_restDensity = 6378.0; ///> Fluid density
int m_maxNumNeighbors = 50; ///> Max. number of neighbors
std::vector<double> m_lambdas; ///> lambdas
std::vector<double> m_densities; ///> densities
std::vector<Vec3d> m_deltaPositions; ///> delta positions
std::vector<int> m_neighbors; ///> index of neighbors
std::vector<int> m_numNeighbors; ///> number of neighbors
std::vector<double> m_lambdas; ///> lambdas
std::vector<double> m_densities; ///> densities
std::vector<Vec3d> m_deltaPositions; ///> delta positions
std::vector<std::vector<size_t>> m_neighborList; ///> indices of neighbor particles
NeighborSearch::Method m_NeighborSearchMethod = NeighborSearch::Method::UniformGridBasedSearch;
std::shared_ptr<NeighborSearch> m_NeighborSearcher; ///> neighbor searcher, must be initialized during model initialization
};
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment