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

Merge branch 'PBDFluidSimulation' into 'master'

Pbd fluid simulation

See merge request !190
parents b0d27102 d81e0a28
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "imstkPbdConstantDensityConstraint.h"
#include "imstkPbdModel.h"
namespace imstk
{
void
PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
{
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));
m_lambdas.resize(np);
m_densities.resize(np);
m_numNeighbors.resize(np);
m_deltaPositions.resize(np);
m_neighbors.resize(np * m_maxNumNeighbors);
m_xPosIndexes.resize(np);
m_yPosIndexes.resize(np);
m_zPosIndexes.resize(np);
}
bool
PbdConstantDensityConstraint::solvePositionConstraint(PbdModel& model)
{
const auto state = model.getCurrentState();
auto& pos = state->getPositions();
const auto np = pos.size();
clearNeighbors(np);
for (auto index = 0; index < np; ++index)
{
this->updateNeighborsBruteForce(pos[index], index, pos);
}
for (auto index = 0; index < np; ++index)
{
this->calculateDensityEstimate(pos[index], index, pos);
}
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);
}
return true;
}
inline double
PbdConstantDensityConstraint::wPoly6(const Vec3d &pi, const Vec3d &pj)
{
double rLength = (pi - pj).norm();
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)
{
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;
//loop over all points
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;
}
if ((pi - positions[j]).norm() < neighborRadius)
{
m_neighbors[index * m_maxNumNeighbors + neighborCount] = j;
neighborCount++;
}
}
}
m_numNeighbors[index] = neighborCount;
}
inline void
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]]);
}
m_densities[index] = densitySum;
}
inline void
PbdConstantDensityConstraint::calculateLambdaScalingFactor(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++)
{
gradientSum += gradSpiky(pi, positions[m_neighbors[index * m_maxNumNeighbors + j]]).squaredNorm() / m_restDensity;
}
m_lambdas[index] = densityConstraint / (gradientSum + m_relaxationParameter);
}
inline 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++)
{
double lambdasDiff = (m_lambdas[index] + m_lambdas[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;
positions[index] += m_deltaPositions[index];
}
} // imstk
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#ifndef imstkPbdConstantDensityConstraint_h
#define imstkPbdConstantDensityConstraint_h
#include "imstkPbdConstraint.h"
namespace imstk
{
///
/// \class PbdConstantDensityConstraint
///
/// \brief Implements the constant density constraint to simulate fluids
///
class PbdConstantDensityConstraint : public PbdConstraint
{
public:
///
/// \brief constructor
///
PbdConstantDensityConstraint() : PbdConstraint() { m_vertexIds.resize(1); }
///
/// \Constant Density Constraint Initialization
///
void initConstraint(PbdModel& model, const double k);
///
/// \brief Returns PBD constraint of type Type::ConstantDensity
///
Type getType() const { return Type::ConstantDensity; }
///
/// \brief Solves the constant density constraint
///
bool solvePositionConstraint(PbdModel &model);
private:
///
/// \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 = 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;
};
}
#endif // imstkPbdConstantDensityConstraint_h
......@@ -45,6 +45,7 @@ public:
Volume,
FEMTet,
FEMHex,
ConstantDensity,
none
};
......
......@@ -28,6 +28,7 @@
#include "imstkPbdAreaConstraint.h"
#include "imstkPbdFETetConstraint.h"
#include "imstkPbdFEHexConstraint.h"
#include "imstkPbdConstantDensityConstraint.h"
#include <g3log/g3log.hpp>
......@@ -395,6 +396,24 @@ PbdModel::initializeDihedralConstraints(const double& stiffness)
return true;
}
bool
PbdModel::initializeConstantDensityConstraint(const double& stiffness)
{
// check if constraint type matches the mesh type
if (m_mesh->getType() != Geometry::Type::SurfaceMesh && m_mesh->getType() != Geometry::Type::TetrahedralMesh && m_mesh->getType() != Geometry::Type::LineMesh &&
m_mesh->getType() != Geometry::Type::HexahedralMesh)
{
LOG(WARNING) << "Constant constraint should come with a mesh"; //TODO: Really only need a point cloud, so may need to change this.
return false;
}
auto c = std::make_shared<PbdConstantDensityConstraint>();
c->initConstraint(*this, stiffness);
m_constraints.push_back(c);
return true;
}
void
PbdModel::projectConstraints()
{
......
......@@ -125,6 +125,13 @@ public:
/// \brief addConstraint add elastic constraint
/// \param constraint
///
bool initializeConstantDensityConstraint(const double& stiffness);
///
/// \todo: add the initialization parameters for the constraint
/// \param...
///
inline void addConstraint(std::shared_ptr<PbdConstraint> constraint) { m_constraints.push_back(constraint); }
///
......
......@@ -118,6 +118,13 @@ PbdObject::initialize(int nCons, ...)
LOG(INFO) << "Creating Dihedral constraints " << stiffness;
m_pbdModel->initializeDihedralConstraints(stiffness);
}
else if (strncmp("ConstantDensity", &s[0], len) == 0)
{
float stiffness;
sscanf(&s[len + 1], "%f", &stiffness);
LOG(INFO) << "Creating Constant Density constraints ";
m_pbdModel->initializeConstantDensityConstraint(stiffness);
}
else
{
exit(0);
......
......@@ -126,6 +126,8 @@ void testVectorPlotters();
void testPbdVolume();
void testPbdCloth();
void testPbdCollision();
void testPbdFluidBenchmarking();
void testPbdFluid();
void testLineMesh();
void testMshAndVegaIO();
void testLapToolController();
......@@ -179,6 +181,8 @@ int main()
//testPbdVolume();
//testPbdCloth();
//testPbdCollision();
testPbdFluidBenchmarking();
testPbdFluid();
//testDeformableBody();
//testDeformableBodyCollision();
//liverToolInteraction();
......@@ -208,9 +212,9 @@ int main()
------------------*/
//testScenesManagement();
//testVectorPlotters();
testVirtualCoupling();
//testVirtualCoupling();
//testBoneDrilling();
testVirtualCouplingCylinder();
//testVirtualCouplingCylinder();
return 0;
......@@ -1523,7 +1527,7 @@ void testPbdCollision()
auto sdk = std::make_shared<SimulationManager>();
auto scene = sdk->createNewScene("PbdCollisionTest");
scene->getCamera()->setPosition(0, 10.0, 25.0);
scene->getCamera()->setPosition(0, 10.0, 15.0);
// dragon
auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg");
......@@ -1834,6 +1838,478 @@ void testPbdCollision()
sdk->startSimulation(true);
}
void testPbdFluidBenchmarking()
{
std::vector<int> nPointsList = { 5, 10, 20 };
std::vector<int> cubeSizeList = { 1, 1, 2 };
int nPointsPerSide = 10;
double cubeLength = 1;
auto sdk = std::make_shared<SimulationManager>();
auto scene = sdk->createNewScene("PBDFluidBenchmarking");
scene->getCamera()->setPosition(0, 10.0, 25.0);
//Create Mesh
imstk::StdVectorOfVec3d vertList;
int nPoints = pow(nPointsPerSide, 3);
const double spacing = cubeLength / nPointsPerSide;
vertList.resize(nPoints);
for (int i = 0; i < nPointsPerSide; ++i)
{
for (int j = 0; j < nPointsPerSide; j++)
{
for (int k = 0; k < nPointsPerSide; k++)
{
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++)
{
for (size_t j = 0; j < nPointsPerSide - 1; j++)
{
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 } };
triangles.push_back(tri[0]);
triangles.push_back(tri[1]);
}
}
}
auto cubeMeshColliding = std::make_shared<imstk::SurfaceMesh>();
cubeMeshColliding->initialize(vertList, triangles);
auto cubeMeshVisual = std::make_shared<imstk::SurfaceMesh>();
cubeMeshVisual->initialize(vertList, triangles);
auto cubeMeshPhysics = std::make_shared<imstk::SurfaceMesh>();
cubeMeshPhysics->initialize(vertList, triangles);
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);
cubeMapP2V->compute();
auto cubeMapP2C = std::make_shared<imstk::OneToOneMap>();
cubeMapP2C->setMaster(cubeMeshPhysics);
cubeMapP2C->setSlave(cubeMeshColliding);
cubeMapP2C->compute();
auto cubeMapC2V = std::make_shared<imstk::OneToOneMap>();
cubeMapC2V->setMaster(cubeMeshColliding);
cubeMapC2V->setSlave(cubeMeshVisual);
cubeMapC2V->compute();
auto cube = std::make_shared<PbdObject>("Cube");
cube->setCollidingGeometry(cubeMeshColliding);
cube->setVisualGeometry(cubeMeshVisual);
cube->setPhysicsGeometry(cubeMeshPhysics);
cube->setPhysicsToCollidingMap(cubeMapP2C);
cube->setPhysicsToVisualMap(cubeMapP2V);
cube->setCollidingToVisualMap(cubeMapC2V);
auto pbdModel = std::make_shared<PbdModel>();
cube->setDynamicalModel(pbdModel);
cube->initialize(/*Number of Constraints*/ 1,
/*Constraint configuration*/ "ConstantDensity 1.0 0.3",
/*Mass*/ 1.0,
/*Gravity*/ "0 -9.8 0",
/*TimeStep*/ 0.005,
/*FixedPoint*/ "",
/*NumberOfIterationInConstraintSolver*/ 2,
/*Proximity*/ 0.1,
/*Contact stiffness*/ 1.0);
auto pbdSolver = std::make_shared<PbdSolver>();
pbdSolver->setPbdObject(cube);
scene->addNonlinearSolver(pbdSolver);
scene->addSceneObject(cube);
// plane
double width = 40.0;
double height = 40.0;
int nRows = 2;
int nCols = 2;
vertList.resize(nRows*nCols);
const double dy = width / (double)(nCols - 1);
const double dx = height / (double)(nRows - 1);
for (int i = 0; i < nRows; ++i)
{
for (int j = 0; j < nCols; j++)
{
const double y = (double)dy*j;
const double x = (double)dx*i;
vertList[i*nCols + j] = Vec3d(x - 20, -0.5, y - 20);
}
}
// c. Add connectivity data
triangles.clear();
for (std::size_t i = 0; i < nRows - 1; ++i)
{
for (std::size_t j = 0; j < nCols - 1; j++)
{
imstk::SurfaceMesh::TriangleArray tri[2];
tri[0] = { { i*nCols + j, i*nCols + j + 1, (i + 1)*nCols + j } };
tri[1] = { { (i + 1)*nCols + j + 1, (i + 1)*nCols + j, i*nCols + j + 1 } };
triangles.push_back(tri[0]);
triangles.push_back(tri[1]);
}
}
auto floorMeshColliding = std::make_shared<imstk::SurfaceMesh>();
floorMeshColliding->initialize(vertList, triangles);
auto floorMeshVisual = std::make_shared<imstk::SurfaceMesh>();
floorMeshVisual->initialize(vertList, triangles);
auto floorMeshPhysics = std::make_shared<imstk::SurfaceMesh>();
floorMeshPhysics->initialize(vertList, triangles);
auto floorMapP2V = std::make_shared<imstk::OneToOneMap>();
floorMapP2V->setMaster(floorMeshPhysics);
floorMapP2V->setSlave(floorMeshVisual);
floorMapP2V->compute();
auto floorMapP2C = std::make_shared<imstk::OneToOneMap>();
floorMapP2C->setMaster(floorMeshPhysics);
floorMapP2C->setSlave(floorMeshColliding);
floorMapP2C->compute();
auto floorMapC2V = std::make_shared<imstk::OneToOneMap>();