Commit d81e0a28 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

ENH: Add get set functions for density and max. neighbor distance

In the PbdConstantDensityConstraint class:
1. Style changes
2. Remove unused functions
3. Remove casting warnings
4. Add get set functions for density and max. neighbor distance
5. Add comments for variables and functions
6. Remove grid based neighbor search functions
parent bf741fba
......@@ -27,14 +27,12 @@ namespace imstk
void
PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
{
auto state = model.getCurrentState();
auto np = state->getPositions().size();
//constraint parameters
m_maxNumNeighbors = 100;
m_maxDist = 0.2;
m_relaxationParameter = 600.0;
m_restDensity = 6378.0;
const auto state = model.getCurrentState();
const auto np = state->getPositions().size();
// constraint parameters
// Refer: Miller, et al 2003 "Particle-Based Fluid Simulation for Interactive Applications."
// TODO: Check if these numbers can be variable
m_wPoly6Coeff = 315.0 / (64.0 * PI * pow(m_maxDist, 9));
m_wSpikyCoeff = 15.0 / (PI * pow(m_maxDist, 6));
......@@ -52,96 +50,58 @@ PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
bool
PbdConstantDensityConstraint::solvePositionConstraint(PbdModel& model)
{
auto state = model.getCurrentState();
const auto state = model.getCurrentState();
auto& pos = state->getPositions();
auto np = pos.size();
const auto np = pos.size();
ClearNeighbors(np);
clearNeighbors(np);
//This loop should be replaced with parallellization
/*for (int index = 0; index < np; index++)
{
PointTable(pos[index], index);
}*/
for (int index = 0; index < np; index++)
{
//UpdateNeighbors(index, pos);
UpdateNeighborsBruteForce(pos[index], index, pos);
}
for (int index = 0; index < np; index++)
for (auto index = 0; index < np; ++index)
{
CalculateDensityEstimate(pos[index], index, pos);
this->updateNeighborsBruteForce(pos[index], index, pos);
}
for (int index = 0; index < np; index++)
for (auto index = 0; index < np; ++index)
{
CalculateLambdaScalingFactor(pos[index], index, pos);
this->calculateDensityEstimate(pos[index], index, pos);
}
for (int index = 0; index < np; index++)
for (auto index = 0; index < np; ++index)
{
DeltaPosition(pos[index], index, pos);
this->calculateLambdaScalingFactor(pos[index], index, pos);
}
for (int index = 0; index < np; index++)
for (auto index = 0; index < np; ++index)
{
UpdatePositions(pos[index], index);
this->updatePositions(pos[index], index, pos);
}
return true;
}
//Smoothing Kernal WPoly6 for density estimation
inline double
PbdConstantDensityConstraint::WPoly6(const Vec3d &pi, const Vec3d &pj)
PbdConstantDensityConstraint::wPoly6(const Vec3d &pi, const Vec3d &pj)
{
Vec3d r = pi - pj;
double rLength = Length(r);
if (rLength > m_maxDist || rLength == 0)
{
return 0;
}
double rLength = (pi - pj).norm();
return m_wPoly6Coeff * pow((m_maxDist * m_maxDist) - (rLength * rLength), 3);
}
//Smoothing Kernal Spiky for gradient calculation
inline double
PbdConstantDensityConstraint::WSpiky(const Vec3d &pi, const Vec3d &pj)
{
Vec3d r = pi - pj;
double rLength = Length(r);
if (rLength > m_maxDist || rLength == 0)
{
return 0;
}
return m_wSpikyCoeff * pow(m_maxDist - rLength, 3);
return (rLength > m_maxDist || rLength == 0) ?
0 :
m_wPoly6Coeff * pow((m_maxDist * m_maxDist) - (rLength * rLength), 3);
}
inline Vec3d
PbdConstantDensityConstraint::GradSpiky(const Vec3d &pi, const Vec3d &pj)
PbdConstantDensityConstraint::gradSpiky(const Vec3d &pi, const Vec3d &pj)
{
Vec3d r = pi - pj;
Vec3d zero;
zero[0] = 0;
zero[1] = 0;
zero[2] = 0;
double rLength = Length(r);
if (rLength > m_maxDist || rLength == 0)
{
return zero;
}
const double rLength = r.norm();
return r * (m_wSpikyCoeff * (-3.0) * (m_maxDist - rLength) * (m_maxDist - rLength));
}
inline double
PbdConstantDensityConstraint::Length(const Vec3d &p1)
{
return sqrt(p1[0] * p1[0] + p1[1] * p1[1] + p1[2] * p1[2]);
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 int &np)
PbdConstantDensityConstraint::clearNeighbors(const size_t &np)
{
m_numNeighbors.clear();
m_neighbors.clear();
......@@ -150,79 +110,25 @@ PbdConstantDensityConstraint::ClearNeighbors(const int &np)
}
inline void
PbdConstantDensityConstraint::PointTable(const Vec3d &pi, const int &index)
{
m_xPosIndexes[index] = (pi[0] - pi[0] * m_maxDist) / m_maxDist;
m_yPosIndexes[index] = (pi[1] - pi[1] * m_maxDist) / m_maxDist;
m_zPosIndexes[index] = (pi[2] - pi[2] * m_maxDist) / m_maxDist;
}
inline void
PbdConstantDensityConstraint::UpdateNeighbors(const int &index, const StdVectorOfVec3d &positions)
PbdConstantDensityConstraint::updateNeighborsBruteForce(const Vec3d &pi,
const size_t &index,
const StdVectorOfVec3d &positions)
{
int ip = m_xPosIndexes[index];
int jp = m_yPosIndexes[index];
int kp = m_zPosIndexes[index];
int np = m_zPosIndexes.size();
const double neighborRadius = m_maxDist;
int neighborCount = 0;
int ibound = ip - 2;
if (ibound < 0)
{
ibound = 0;
}
int jbound = jp - 2;
if (jbound < 0)
{
jbound = 0;
}
int kbound = kp - 2;
if (kbound < 0)
{
kbound = 0;
}
for (int i = 0; i < np; i++)
{
if (neighborCount >= m_maxNumNeighbors || i == index)
{
continue;
}
if (m_xPosIndexes[i] > ibound && m_xPosIndexes[i] < (ip + 2))
{
if (m_yPosIndexes[i] > jbound && m_yPosIndexes[i] < (jp + 2))
{
if (m_zPosIndexes[i] > kbound && m_zPosIndexes[i] < (kp + 2))
{
m_neighbors[index * m_maxNumNeighbors + neighborCount] = i;
neighborCount++;
}
}
}
}
m_numNeighbors[index] = neighborCount;
}
//brute Force
inline
void PbdConstantDensityConstraint::UpdateNeighborsBruteForce(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions)
{
double neighborRadius = 2 * m_maxDist;
Vec3d r;
double rLength;
int neighborCount = 0;
//loop over all points
for (int j = 0; j < positions.size(); j++)
for (auto j = 0; j < positions.size(); ++j)
{
if (j != index)
{
if (neighborCount >= m_maxNumNeighbors)
{
LOG(WARNING) << "Neighbor count reached max. for point: " << index;
continue;
}
r = pi - positions[j];
rLength = Length(r);
if (rLength < neighborRadius)
if ((pi - positions[j]).norm() < neighborRadius)
{
m_neighbors[index * m_maxNumNeighbors + neighborCount] = j;
neighborCount++;
......@@ -234,55 +140,49 @@ void PbdConstantDensityConstraint::UpdateNeighborsBruteForce(const Vec3d &pi, co
inline void
PbdConstantDensityConstraint::CalculateDensityEstimate(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions)
PbdConstantDensityConstraint::calculateDensityEstimate(const Vec3d &pi,
const size_t &index,
const StdVectorOfVec3d &positions)
{
double densitySum = 0.0;
for (int j = 0; j < m_numNeighbors[index]; j++)
{
densitySum += WPoly6(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
densitySum += wPoly6(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
}
m_densities[index] = densitySum;
}
inline void
PbdConstantDensityConstraint::CalculateLambdaScalingFactor(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions)
PbdConstantDensityConstraint::calculateLambdaScalingFactor(const Vec3d &pi,
const size_t &index,
const StdVectorOfVec3d &positions)
{
double densityConstraint = (m_densities[index] / m_restDensity) - 1;
const double densityConstraint = (m_densities[index] / m_restDensity) - 1;
double gradientSum = 0.0;
for (int j = 0; j < m_numNeighbors[index]; j++)
{
gradientSum += pow(Length(GradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]])), 2) / m_restDensity;
gradientSum += gradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]).squaredNorm() / m_restDensity;
}
m_lambdas[index] = densityConstraint / (gradientSum + m_relaxationParameter);
}
inline void
PbdConstantDensityConstraint::DeltaPosition(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions)
PbdConstantDensityConstraint::updatePositions(const Vec3d &pi,
const size_t &index,
StdVectorOfVec3d &positions)
{
//Make sure the point is valid
Vec3d gradientLambdaSum;
gradientLambdaSum[0] = 0;
gradientLambdaSum[1] = 0;
gradientLambdaSum[2] = 0;
Vec3d gradientLambdaSum(0., 0., 0.);
for (int j = 0; j < m_numNeighbors[index]; j++)
{
int neighborInd = m_neighbors[index * m_maxNumNeighbors + j];
double lambdasDiff = (m_lambdas[index] + m_lambdas[m_neighbors[index * m_maxNumNeighbors + j]]);
Vec3d gradKernal = GradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
Vec3d gradKernal = gradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]);
gradientLambdaSum += (gradKernal * lambdasDiff);
}
m_deltaPositions[index] = gradientLambdaSum / m_restDensity;
}
inline void
PbdConstantDensityConstraint::UpdatePositions(Vec3d &pi, const int &index)
{
//Make sure the point is valid
pi[0] += m_deltaPositions[index][0];
pi[1] += m_deltaPositions[index][1];
pi[2] += m_deltaPositions[index][2];
positions[index] += m_deltaPositions[index];
}
} // imstk
\ No newline at end of file
......@@ -23,10 +23,14 @@ limitations under the License.
#define imstkPbdConstantDensityConstraint_h
#include "imstkPbdConstraint.h"
#include "../Collision/CollisionDetection/DataStructures/imstkSpatialHashTableSeparateChaining.h"
namespace imstk
{
///
/// \class PbdConstantDensityConstraint
///
/// \brief Implements the constant density constraint to simulate fluids
///
class PbdConstantDensityConstraint : public PbdConstraint
{
public:
......@@ -34,6 +38,7 @@ public:
/// \brief constructor
///
PbdConstantDensityConstraint() : PbdConstraint() { m_vertexIds.resize(1); }
///
/// \Constant Density Constraint Initialization
///
......@@ -50,33 +55,73 @@ public:
bool solvePositionConstraint(PbdModel &model);
private:
double WPoly6(const Vec3d &pi, const Vec3d &pj);
double WSpiky(const Vec3d &pi, const Vec3d &pj);
Vec3d GradSpiky(const Vec3d &pi, const Vec3d &pj);
double Length(const Vec3d &);
void PointTable(const Vec3d &pi, const int &index);
void UpdateNeighbors(const int &index, const StdVectorOfVec3d &positions);
void UpdateNeighborsBruteForce(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions);
void ClearNeighbors(const int &np);
void CalculateDensityEstimate(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions);
void CalculateLambdaScalingFactor(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions);
void DeltaPosition(const Vec3d &pi, const int &index, const StdVectorOfVec3d &positions);
void UpdatePositions(Vec3d &pi, const int &index);
///
/// \brief Smoothing kernel WPoly6 for density estimation
///
double wPoly6(const Vec3d &pi, const Vec3d &pj);
///
/// \brief Smoothing kernel Spiky for gradient calculation
///
double wSpiky(const Vec3d &pi, const Vec3d &pj);
///
/// \brief
///
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);
///
/// \brief
///
void calculateLambdaScalingFactor(const Vec3d &pi, const size_t &index, const StdVectorOfVec3d &positions);
///
/// \brief
///
void updatePositions(const Vec3d &pi, const size_t &index, StdVectorOfVec3d &positions);
///
/// \brief Set/Get rest density
///
void setDensity(const double density) { m_restDensity = density; }
double getDensity() { return m_restDensity; }
///
/// \brief Set/Get max. neighbor distance
///
void setMaxNeighborDistance(const double dist) { m_maxDist = dist; }
double getMaxNeighborDistance() { return m_restDensity; }
private:
double m_wPoly6Coeff;
double m_wSpikyCoeff;
double m_maxDist;
double m_relaxationParameter;
double m_restDensity;
int m_maxNumNeighbors;
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
double m_maxDist = 0.2; ///> Max. neighbor 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<int> m_xPosIndexes;
std::vector<int> m_yPosIndexes;
std::vector<int> m_zPosIndexes;
......
......@@ -1863,10 +1863,12 @@ void testPbdFluidBenchmarking()
{
for (int k = 0; k < nPointsPerSide; k++)
{
vertList[i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k] = Vec3d((double)i * spacing, (double)j * spacing, (double)k * spacing);
vertList[i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k] =
Vec3d((double)i * spacing, (double)j * spacing, (double)k * spacing);
}
}
}
std::vector<imstk::SurfaceMesh::TriangleArray> triangles;
for (size_t i = 0; i < nPointsPerSide - 1; i++)
{
......@@ -1875,8 +1877,15 @@ void testPbdFluidBenchmarking()
for (size_t k = 0; k < nPointsPerSide - 1; k++)
{
imstk::SurfaceMesh::TriangleArray tri[3];
tri[0] = { { i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k, i*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k, (i + 1)*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k } };
tri[1] = { { i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k, (i + 1)*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k, (i + 1)*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k } };
tri[0] = { { i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k,
i*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k,
(i + 1)*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k } };
tri[1] = { { i*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k,
(i + 1)*nPointsPerSide*nPointsPerSide + j*nPointsPerSide + k,
(i + 1)*nPointsPerSide*nPointsPerSide + (j + 1)*nPointsPerSide + k } };
triangles.push_back(tri[0]);
triangles.push_back(tri[1]);
}
......@@ -1892,9 +1901,10 @@ void testPbdFluidBenchmarking()
auto material1 = std::make_shared<RenderMaterial>();
material1->setDisplayMode(RenderMaterial::DisplayMode::POINTS);
material1->setDiffuseColor(Color::Blue);
material1->setPointSize(3.0);
cubeMeshVisual->setRenderMaterial(material1);
auto cubeMapP2V = std::make_shared<imstk::OneToOneMap>();
cubeMapP2V->setMaster(cubeMeshPhysics);
cubeMapP2V->setSlave(cubeMeshVisual);
......@@ -2044,6 +2054,12 @@ void testPbdFluidBenchmarking()
LOG(INFO) << "\n-- Post cleanup of " << module->getName() << " module";
});
// Light (white)
auto whiteLight = std::make_shared<imstk::Light>("whiteLight");
whiteLight->setPosition(Vec3d(5, 8, 5));
whiteLight->setColor(Color::White);
scene->addLight(whiteLight);
scene->getCamera()->setPosition(0, 10.0, 10.0);
sdk->setCurrentScene(scene);
......@@ -2058,8 +2074,8 @@ void testPbdFluid()
scene->getCamera()->setPosition(0, 10.0, 15.0);
// dragon
auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT "/turtle/turtle-volumetric-homogeneous.veg");
//auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg");
//auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT "/turtle/turtle-volumetric-homogeneous.veg");
auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg");
if (!tetMesh)
{
LOG(WARNING) << "Could not read mesh from file.";
......@@ -2077,6 +2093,13 @@ void testPbdFluid()
volTetMesh->extractSurfaceMesh(surfMesh);
volTetMesh->extractSurfaceMesh(surfMeshVisual);
auto material1 = std::make_shared<RenderMaterial>();
material1->setDisplayMode(RenderMaterial::DisplayMode::POINTS);
material1->setDiffuseColor(Color::Blue);
material1->setSpecularColor(Color::Blue);
material1->setPointSize(6.0);
surfMeshVisual->setRenderMaterial(material1);
auto deformMapP2V = std::make_shared<imstk::OneToOneMap>();
deformMapP2V->setMaster(tetMesh);
deformMapP2V->setSlave(surfMeshVisual);
......@@ -2277,6 +2300,12 @@ void testPbdFluid()
colGraph->addInteractionPair(pair);
// Light (white)
auto whiteLight = std::make_shared<imstk::Light>("whiteLight");
whiteLight->setPosition(Vec3d(5, 8, 5));
whiteLight->setColor(Color::White);
scene->addLight(whiteLight);
sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
......
Supports Markdown
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