imstkSphModel.cpp

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#include "imstkSphModel.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
#include "imstkTaskGraph.h"
#include "imstkVTKMeshIO.h"

namespace imstk
{
SphModelConfig::SphModelConfig(const double particleRadius)
{
    // \todo Warning in all paths?
    if (std::abs(particleRadius) > 1.0e-6)
    {
        LOG_IF(WARNING, (particleRadius < 0)) << "Particle radius supplied is negative! Using absolute value of the supplied radius.";
        m_particleRadius = std::abs(particleRadius);
    }
    else
    {
        LOG(WARNING) << "Particle radius too small! Setting to 1.e-6";
        m_particleRadius = 1.e-6;
    }
    initialize();
}

SphModelConfig::SphModelConfig(const double particleRadius, const double speedOfSound, const double restDensity)
{
    if (std::abs(particleRadius) > 1.0e-6)
    {
        LOG_IF(WARNING, (particleRadius < 0)) << "Particle radius supplied is negative! Using absolute value of the supplied radius.";
        m_particleRadius = std::abs(particleRadius);
    }
    else
    {
        LOG(WARNING) << "Particle radius too small! Setting to 1.e-6";
        m_particleRadius = 1.e-6;
    }

    if (speedOfSound < 0)
    {
        LOG(WARNING) << "Speed of sound is negative! Setting speed of sound to default value.";
    }
    else
    {
        m_speedOfSound = speedOfSound;
    }

    if (restDensity < 0)
    {
        LOG(WARNING) << "Rest density is negative! Setting rest density to default value.";
    }
    else
    {
        m_restDensity = restDensity;
    }
    initialize();
}

void
SphModelConfig::initialize()
{
    // Compute the derived quantities
    m_particleRadiusSqr = m_particleRadius * m_particleRadius;

    m_particleMass   = std::pow(2.0 * m_particleRadius, 3) * m_restDensity * m_particleMassScale;
    m_restDensitySqr = m_restDensity * m_restDensity;
    m_restDensityInv = 1.0 / m_restDensity;

    m_kernelRadius    = m_particleRadius * m_kernelOverParticleRadiusRatio;
    m_kernelRadiusSqr = m_kernelRadius * m_kernelRadius;

    m_pressureStiffness = m_restDensity * m_speedOfSound * m_speedOfSound / 7.0;
}

SphModel::SphModel() : DynamicalModel<SphState>(DynamicalModelType::SmoothedParticleHydrodynamics)
{
    m_validGeometryTypes = { "PointSet" };

    m_findParticleNeighborsNode = m_taskGraph->addFunction("SPHModel_Partition", std::bind(&SphModel::findParticleNeighbors, this));
    m_computeDensityNode = m_taskGraph->addFunction("SPHModel_ComputeDensity", [&]()
        {
            computeNeighborRelativePositions();
            computeDensity();
        });

    m_normalizeDensityNode = m_taskGraph->addFunction("SPHModel_NormalizeDensity", std::bind(&SphModel::normalizeDensity, this));

    m_collectNeighborDensityNode = m_taskGraph->addFunction("SPHModel_CollectNeighborDensity", std::bind(&SphModel::collectNeighborDensity, this));

    m_computeTimeStepSizeNode =
        m_taskGraph->addFunction("SPHModel_ComputeTimestep", std::bind(&SphModel::computeTimeStepSize, this));

    m_computePressureAccelNode =
        m_taskGraph->addFunction("SPHModel_ComputePressureAccel", std::bind(&SphModel::computePressureAcceleration, this));

    m_computeSurfaceTensionNode =
        m_taskGraph->addFunction("SPHModel_ComputeSurfaceTensionAccel", std::bind(&SphModel::computeSurfaceTension, this));

    m_computeViscosityNode =
        m_taskGraph->addFunction("SPHModel_ComputeViscosity", std::bind(&SphModel::computeViscosity, this));

    m_integrateNode =
        m_taskGraph->addFunction("SPHModel_Integrate", std::bind(&SphModel::sumAccels, this));

    m_updateVelocityNode =
        m_taskGraph->addFunction("SPHModel_UpdateVelocity", [&]()
            {
                updateVelocity(getTimeStep());
        });

    m_moveParticlesNode =
        m_taskGraph->addFunction("SPHModel_MoveParticles", [&]()
            {
                moveParticles(getTimeStep());
        });

    //m_computePositionNode =
    //    m_taskGraph->addFunction("SPHModel_ComputePositions", [&]()
    //    {
    //        moveParticles(getTimeStep());
    //        });
}

bool
SphModel::initialize()
{
    LOG_IF(FATAL, (!this->getModelGeometry())) << "Model geometry is not yet set! Cannot initialize without model geometry.";
    m_pointSetGeometry = std::dynamic_pointer_cast<PointSet>(m_geometry);
    const int numParticles = m_pointSetGeometry->getNumVertices();

    // Allocate init and current state
    m_initialState = std::make_shared<SphState>(numParticles);
    m_currentState = std::make_shared<SphState>(numParticles);

    // If there were initial velocities (set them)
    if (m_initialVelocities != nullptr)
    {
        m_currentState->setVelocities(m_initialVelocities);
    }

    // Copy current to initial
    m_initialState->setState(m_currentState);

    // Share geometry and state position arrays
    m_currentState->setPositions(m_pointSetGeometry->getVertexPositions());
    m_initialState->setPositions(m_pointSetGeometry->getInitialVertexPositions());

    // Initialize simulation dependent parameters and kernel data
    m_kernels.initialize(m_modelParameters->m_kernelRadius);

    // Initialize neighbor searcher
    m_neighborSearcher = std::make_shared<NeighborSearch>(m_modelParameters->m_neighborSearchMethod,
      m_modelParameters->m_kernelRadius);

    m_pressureAccels = std::make_shared<VecDataArray<double, 3>>(numParticles);
    std::fill_n(m_pressureAccels->getPointer(), m_pressureAccels->size(), Vec3d(0, 0, 0));

    // initialize surface tension to 0 in case you remove the surface tension node
    m_surfaceTensionAccels = std::make_shared<VecDataArray<double, 3>>(numParticles);
    std::fill_n(m_surfaceTensionAccels->getPointer(), m_surfaceTensionAccels->size(), Vec3d(0, 0, 0));

    m_viscousAccels = std::make_shared<VecDataArray<double, 3>>(numParticles);
    std::fill_n(m_viscousAccels->getPointer(), m_viscousAccels->size(), Vec3d(0, 0, 0));

    m_neighborVelContr = std::make_shared<VecDataArray<double, 3>>(numParticles);
    std::fill_n(m_neighborVelContr->getPointer(), m_neighborVelContr->size(), Vec3d(0, 0, 0));

    m_particleShift = std::make_shared<VecDataArray<double, 3>>(numParticles);
    std::fill_n(m_particleShift->getPointer(), m_particleShift->size(), Vec3d(0, 0, 0));

    // Add all the attributes to the geometry
    m_pointSetGeometry->setVertexAttribute("Pressure Accels", m_pressureAccels);
    m_pointSetGeometry->setVertexAttribute("Surface Tension Accels", m_surfaceTensionAccels);
    m_pointSetGeometry->setVertexAttribute("Viscous Accels", m_viscousAccels);
    m_pointSetGeometry->setVertexAttribute("Densities", m_currentState->getDensities());
    m_pointSetGeometry->setVertexAttribute("Velocities", m_currentState->getVelocities());
    m_pointSetGeometry->setVertexAttribute("Diffuse Velocities", m_currentState->getDiffuseVelocities());
    m_pointSetGeometry->setVertexAttribute("Normals", m_currentState->getNormals());
    m_pointSetGeometry->setVertexAttribute("Accels", m_currentState->getAccelerations());

    return true;
}

void
SphModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
{
    // Setup graph connectivity
    m_taskGraph->addEdge(source, m_findParticleNeighborsNode);
    m_taskGraph->addEdge(m_findParticleNeighborsNode, m_computeDensityNode);
    m_taskGraph->addEdge(m_computeDensityNode, m_normalizeDensityNode);
    m_taskGraph->addEdge(m_normalizeDensityNode, m_collectNeighborDensityNode);

    // Pressure, Surface Tension, and time step size can be done in parallel
    m_taskGraph->addEdge(m_collectNeighborDensityNode, m_computePressureAccelNode);
    m_taskGraph->addEdge(m_collectNeighborDensityNode, m_computeSurfaceTensionNode);
    m_taskGraph->addEdge(m_collectNeighborDensityNode, m_computeViscosityNode);
    m_taskGraph->addEdge(m_collectNeighborDensityNode, m_computeTimeStepSizeNode);

    m_taskGraph->addEdge(m_computePressureAccelNode, m_integrateNode);
    m_taskGraph->addEdge(m_computeSurfaceTensionNode, m_integrateNode);
    m_taskGraph->addEdge(m_computeViscosityNode, m_integrateNode);
    m_taskGraph->addEdge(m_computeTimeStepSizeNode, m_integrateNode);

    m_taskGraph->addEdge(m_integrateNode, m_updateVelocityNode);
    m_taskGraph->addEdge(m_updateVelocityNode, m_moveParticlesNode);
    m_taskGraph->addEdge(m_moveParticlesNode, sink);
}

void
SphModel::computeTimeStepSize()
{
    m_dt = (this->m_timeStepSizeType == TimeSteppingType::Fixed) ? m_defaultDt : computeCFLTimeStepSize();
}

double
SphModel::computeCFLTimeStepSize()
{
    auto maxVel = ParallelUtils::findMaxL2Norm(*getCurrentState()->getFullStepVelocities());

    // dt = CFL * 2r / (speed of sound + max{|| v ||})
    double timestep = maxVel > 1.0e-6 ?
                      m_modelParameters->m_cflFactor * (2.0 * m_modelParameters->m_particleRadius / (m_modelParameters->m_speedOfSound + maxVel)) :
                      m_modelParameters->m_maxTimestep;

    // clamp the time step size to be within a given range
    if (timestep > m_modelParameters->m_maxTimestep)
    {
        timestep = m_modelParameters->m_maxTimestep;
    }
    else if (timestep < m_modelParameters->m_minTimestep)
    {
        timestep = m_modelParameters->m_minTimestep;
    }
    return timestep;
}

void
SphModel::findParticleNeighbors()
{
    m_neighborSearcher->getNeighbors(getCurrentState()->getFluidNeighborLists(), *getCurrentState()->getPositions());

    if (m_modelParameters->m_bDensityWithBoundary)   // if considering boundary particles for computing fluid density
    {
        m_neighborSearcher->getNeighbors(getCurrentState()->getBoundaryNeighborLists(),
            *getCurrentState()->getPositions(),
            *getCurrentState()->getBoundaryParticlePositions());
    }
}

void
SphModel::computeNeighborRelativePositions()
{
    auto computeRelativePositions = [&](const Vec3d& ppos, const std::vector<size_t>& neighborList,
                                        const VecDataArray<double, 3>& allPositions, std::vector<NeighborInfo>& neighborInfo)
                                    {
                                        for (const size_t q : neighborList)
                                        {
                                            const Vec3d& qpos = allPositions[q];
                                            const Vec3d  r    = ppos - qpos;
                                            neighborInfo.push_back({ r, m_modelParameters->m_restDensity });
                                        }
                                    };

    std::shared_ptr<VecDataArray<double, 3>> positionsPtr = getCurrentState()->getPositions();
    const VecDataArray<double, 3>&           positions = *positionsPtr;

    std::vector<std::vector<NeighborInfo>>& neighborInfos = getCurrentState()->getNeighborInfo();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions
                && m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            const Vec3d& ppos = positions[p];
            std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            neighborInfo.resize(0);
            neighborInfo.reserve(48);

            computeRelativePositions(ppos, getCurrentState()->getFluidNeighborLists()[p], *getCurrentState()->getPositions(), neighborInfo);
            // if considering boundary particles then also cache relative positions with them
            if (m_modelParameters->m_bDensityWithBoundary)
            {
                computeRelativePositions(ppos, getCurrentState()->getBoundaryNeighborLists()[p], *getCurrentState()->getBoundaryParticlePositions(), neighborInfo);
            }
      });
}

void
SphModel::collectNeighborDensity()
{
    // After computing particle densities, cache them into neighborInfo variable, next to relative positions
    // this is useful because relative positions and densities are accessed together multiple times
    // caching relative positions and densities therefore can reduce computation time significantly (tested)
    std::shared_ptr<DataArray<double>> densitiesPtr = getCurrentState()->getDensities();
    DataArray<double>&                 densities    = *densitiesPtr;

    const std::vector<std::vector<size_t>>&                 neighborLists = getCurrentState()->getFluidNeighborLists();
    const std::vector<SphBoundaryConditions::ParticleType>& particleTypes = m_sphBoundaryConditions->getParticleTypes();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions && particleTypes[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            auto& neighborInfo = getCurrentState()->getNeighborInfo()[p];
            if (neighborInfo.size() <= 1)
            {
                return; // the particle has no neighbor
            }

            const auto& fluidNeighborList = neighborLists[p];
            for (size_t i = 0; i < fluidNeighborList.size(); ++i)
            {
                auto q = fluidNeighborList[i];
                neighborInfo[i].density = densities[q];
            }
      });
}

void
SphModel::computeDensity()
{
    std::shared_ptr<DataArray<double>> densitiesPtr = getCurrentState()->getDensities();
    DataArray<double>&                 densities    = *densitiesPtr;

    const std::vector<std::vector<NeighborInfo>>& neighborInfos = getCurrentState()->getNeighborInfo();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions && m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            const std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            if (neighborInfo.size() <= 1)
            {
                return; // the particle has no neighbor
            }

            double pdensity = 0.0;
            for (const auto& qInfo : neighborInfo)
            {
                pdensity += m_kernels.W(qInfo.relativePos);
            }
            pdensity    *= m_modelParameters->m_particleMass;
            densities[p] = pdensity;
      });
}

//void
//SPHModel::computePressureOutlet()
//{
//  ParallelUtils::parallelFor(getState().getNumParticles(),
//    [&](const size_t p) {
//      if (getState().getPositions()[p].x() > m_outletRegionXCoord && getState().getPositions()[p].x() < m_maxXCoord)
//      {
//        getState().getDensities()[p] = m_outletDensity;
//      }
//    });
//}

void
SphModel::normalizeDensity()
{
    if (!m_modelParameters->m_bNormalizeDensity)
    {
        return;
    }

    std::shared_ptr<DataArray<double>> densitiesPtr = getCurrentState()->getDensities();
    DataArray<double>&                 densities    = *densitiesPtr;

    const std::vector<std::vector<size_t>>&                 neighborLists = getCurrentState()->getFluidNeighborLists();
    const std::vector<std::vector<NeighborInfo>>&           neighborInfos = getCurrentState()->getNeighborInfo();
    const std::vector<SphBoundaryConditions::ParticleType>& particleTypes = m_sphBoundaryConditions->getParticleTypes();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions && particleTypes[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            const std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            if (neighborInfo.size() <= 1)
            {
                return; // the particle has no neighbor
            }

            const auto& fluidNeighborList = neighborLists[p];
            double tmp = 0.0;

            for (size_t i = 0; i < fluidNeighborList.size(); ++i)
            {
                const auto& qInfo = neighborInfo[i];

                // because we're not done with density computation, qInfo does not contain desity of particle q yet
                const auto q = fluidNeighborList[i];
                const auto qdensity = densities[q];
                tmp += m_kernels.W(qInfo.relativePos) / qdensity;
            }

            densities[p] /= (tmp * m_modelParameters->m_particleMass);
      });
}

void
SphModel::computePressureAcceleration()
{
    std::shared_ptr<DataArray<double>> densitiesPtr   = getCurrentState()->getDensities();
    const DataArray<double>&           densities      = *densitiesPtr;
    VecDataArray<double, 3>&           pressureAccels = *m_pressureAccels;

    const std::vector<std::vector<NeighborInfo>>&           neighborInfos = getCurrentState()->getNeighborInfo();
    const std::vector<SphBoundaryConditions::ParticleType>& particleTypes = m_sphBoundaryConditions->getParticleTypes();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions && particleTypes[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            Vec3d accel = Vec3d::Zero();
            const std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            if (neighborInfo.size() <= 1)
            {
                pressureAccels[p] = accel;
                return;
            }

            const auto pdensity  = densities[p];
            const auto ppressure = getParticlePressure(pdensity);

            for (size_t idx = 0; idx < neighborInfo.size(); ++idx)
            {
                const auto& qInfo    = neighborInfo[idx];
                const auto r         = qInfo.relativePos;
                const auto qdensity  = qInfo.density;
                const auto qpressure = getParticlePressure(qdensity);
                // pressure forces
                accel += -(ppressure / (pdensity * pdensity) + qpressure / (qdensity * qdensity)) * m_kernels.gradW(r);
            }

            accel *= m_modelParameters->m_particleMass;

            //getState().getAccelerations()[p] = accel;
            pressureAccels[p] = accel;
      });
}

void
SphModel::computeViscosity()
{
    VecDataArray<double, 3>&       viscousAccels      = *m_viscousAccels;
    VecDataArray<double, 3>&       neighborVelContr   = *m_neighborVelContr;
    VecDataArray<double, 3>&       particleShift      = *m_particleShift;
    const VecDataArray<double, 3>& halfStepVelocities = *getCurrentState()->getHalfStepVelocities();

    const std::vector<std::vector<NeighborInfo>>& neighborInfos = getCurrentState()->getNeighborInfo();
    const std::vector<std::vector<size_t>>&       neighborLists = getCurrentState()->getFluidNeighborLists();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions
                && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                    || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
            {
                return;
            }

            const std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            if (neighborInfo.size() <= 1)
            {
                neighborVelContr[p] = Vec3d::Zero();
                viscousAccels[p]    = Vec3d::Zero();
                return;
            }

            Vec3d neighborVelContributionsNumerator    = Vec3d::Zero();
            double neighborVelContributionsDenominator = 0.0;
            Vec3d particleShifts = Vec3d::Zero();

            const Vec3d& pvel = halfStepVelocities[p];
            const std::vector<size_t>& fluidNeighborList = neighborLists[p];

            Vec3d diffuseFluid = Vec3d::Zero();
            for (size_t i = 0; i < fluidNeighborList.size(); ++i)
            {
                const auto q        = fluidNeighborList[i];
                const auto& qvel    = halfStepVelocities[q];
                const auto& qInfo   = neighborInfo[i];
                const auto r        = qInfo.relativePos;
                const auto qdensity = qInfo.density;
                diffuseFluid       += (1.0 / qdensity) * m_kernels.laplace(r) * (qvel - pvel);

                neighborVelContributionsNumerator   += (qvel - pvel) * m_kernels.W(r);
                neighborVelContributionsDenominator += m_kernels.W(r);
                particleShifts += m_kernels.gradW(r);
                //diffuseFluid       += (Real(1.0) / qdensity) * m_kernels.W(r) * (qvel - pvel);
            }
            //diffuseFluid *= m_modelParameters->m_dynamicViscosityCoeff / getState().getDensities()[p];
            const double particleRadius = m_modelParameters->m_particleRadius;
            particleShifts     *= 4 / 3 * PI * particleRadius * particleRadius * particleRadius * 0.5 * m_modelParameters->m_kernelRadius * halfStepVelocities[p].norm();
            diffuseFluid       *= m_modelParameters->m_dynamicViscosityCoeff * m_modelParameters->m_particleMass;
            neighborVelContr[p] = neighborVelContributionsNumerator * m_modelParameters->m_eta / neighborVelContributionsDenominator;
            particleShift[p]    = -particleShifts;

            viscousAccels[p] = diffuseFluid;
      });
}

void
SphModel::computeSurfaceTension()
{
    VecDataArray<double, 3>& surfaceNormals = *getCurrentState()->getNormals();

    const std::vector<std::vector<NeighborInfo>>& neighborInfos = getCurrentState()->getNeighborInfo();

    // First, compute surface normal for all particles
    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions && m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer)
            {
                return;
            }

            Vec3d n(0.0, 0.0, 0.0);
            const std::vector<NeighborInfo>& neighborInfo = neighborInfos[p];
            if (neighborInfo.size() <= 1)
            {
                surfaceNormals[p] = n;
                return;
            }

            for (size_t i = 0; i < neighborInfo.size(); ++i)
            {
                const auto& qInfo   = neighborInfo[i];
                const auto r        = qInfo.relativePos;
                const auto qdensity = qInfo.density;
                n += (1.0 / qdensity) * m_kernels.gradW(r);
            }

            n *= m_modelParameters->m_kernelRadius * m_modelParameters->m_particleMass;
            surfaceNormals[p] = n;
        });

    VecDataArray<double, 3>& surfaceTensionAccels = *m_surfaceTensionAccels;
    const DataArray<double>& densities = *getCurrentState()->getDensities();

    const std::vector<std::vector<size_t>>& neighborLists = getCurrentState()->getFluidNeighborLists();

    // Second, compute surface tension acceleration
    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions
                && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                    || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
            {
                return;
            }

            const std::vector<size_t>& fluidNeighborList = neighborLists[p];
            if (fluidNeighborList.size() <= 1)
            {
                return; // the particle has no neighbor
            }

            const Vec3d& ni          = surfaceNormals[p];
            const double pdensity    = densities[p];
            const auto& neighborInfo = neighborInfos[p];

            Vec3d accel = Vec3d::Zero();
            for (size_t i = 0; i < fluidNeighborList.size(); ++i)
            {
                const size_t q = fluidNeighborList[i];
                if (p == q)
                {
                    continue;
                }
                const NeighborInfo& qInfo = neighborInfo[i];
                const double qdensity     = qInfo.density;

                // Correction factor
                const double K_ij = 2.0 * m_modelParameters->m_restDensity / (pdensity + qdensity);

                // Cohesion acc
                const Vec3d& r  = qInfo.relativePos;
                const double d2 = r.squaredNorm();
                if (d2 > 1.0e-20)
                {
                    accel -= K_ij * m_modelParameters->m_particleMass * (r / std::sqrt(d2)) * m_kernels.cohesionW(r);
                }

                // Curvature acc
                const Vec3d& nj = surfaceNormals[q];
                accel -= K_ij * (ni - nj);
            }

            accel *= m_modelParameters->m_surfaceTensionStiffness;
            //getState().getAccelerations()[p] += accel;
            surfaceTensionAccels[p] = accel;
        });
}

void
SphModel::sumAccels()
{
    const VecDataArray<double, 3>& pressureAccels       = *m_pressureAccels;
    const VecDataArray<double, 3>& surfaceTensionAccels = *m_surfaceTensionAccels;
    const VecDataArray<double, 3>& viscousAccels = *m_viscousAccels;
    VecDataArray<double, 3>&       accels = *getCurrentState()->getAccelerations();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions
                && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                    || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
            {
                return;
            }

            accels[p] = pressureAccels[p] + surfaceTensionAccels[p] + viscousAccels[p];
      });
}

void
SphModel::updateVelocity(const double timestep)
{
    VecDataArray<double, 3>&       halfStepVelocities = *getCurrentState()->getHalfStepVelocities();
    VecDataArray<double, 3>&       fullStepVelocities = *getCurrentState()->getFullStepVelocities();
    const VecDataArray<double, 3>& positions = *getCurrentState()->getPositions();
    const VecDataArray<double, 3>& accels    = *getCurrentState()->getAccelerations();

    ParallelUtils::parallelFor(getCurrentState()->getNumParticles(),
        [&](const size_t p)
        {
            if (m_sphBoundaryConditions
                && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                    || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
            {
                return;
            }

            // todo - simply run SPH for half a time step to start to we don't need to perform this check at every time step
            if (m_timeStepCount == 0)
            {
                halfStepVelocities[p]  = fullStepVelocities[p] + (m_modelParameters->m_gravity + accels[p]) * timestep * 0.5;
                fullStepVelocities[p] += (m_modelParameters->m_gravity + accels[p]) * timestep;
            }
            else
            {
                halfStepVelocities[p] += (m_modelParameters->m_gravity + accels[p]) * timestep;
                fullStepVelocities[p]  = halfStepVelocities[p] + (m_modelParameters->m_gravity + accels[p]) * timestep * 0.5;
            }
            if (m_sphBoundaryConditions && m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Inlet)
            {
                halfStepVelocities[p] = m_sphBoundaryConditions->computeParabolicInletVelocity(positions[p]);
                fullStepVelocities[p] = m_sphBoundaryConditions->computeParabolicInletVelocity(positions[p]);
            }
      });
}

void
SphModel::moveParticles(const double timestep)
{
    //ParallelUtils::parallelFor(getState().getNumParticles(),
    //  [&](const size_t p) {

    VecDataArray<double, 3>& neighborVelContr = *m_neighborVelContr;
    VecDataArray<double, 3>& particleShift    = *m_particleShift;
    VecDataArray<double, 3>& positions = *getCurrentState()->getPositions();
    VecDataArray<double, 3>& halfStepVelocities = *getCurrentState()->getHalfStepVelocities();
    VecDataArray<double, 3>& fullStepVelocities = *getCurrentState()->getFullStepVelocities();

    for (int p = 0; p < static_cast<int>(getCurrentState()->getNumParticles()); p++)
    {
        if (m_sphBoundaryConditions
            && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
        {
            continue;
        }

        Vec3d oldPosition = positions[p];
        Vec3d newPosition = oldPosition + particleShift[p] * timestep + (halfStepVelocities[p] + neighborVelContr[p]) * timestep;

        positions[p] = newPosition;

        if (m_sphBoundaryConditions)
        {
            std::vector<SphBoundaryConditions::ParticleType>& particleTypes = m_sphBoundaryConditions->getParticleTypes();
            if (particleTypes[p] == SphBoundaryConditions::ParticleType::Inlet
                && !m_sphBoundaryConditions->isInInletDomain(newPosition))
            {
                // change particle type to fluid
                particleTypes[p] = SphBoundaryConditions::ParticleType::Fluid;
                // insert particle into inlet domain from buffer domain
                // todo: come up with a better way to find buffer indices
                // right now, the buffer index is limiting the parallel ability of this function
                const size_t bufferParticleIndex = m_sphBoundaryConditions->getBufferIndices().back();
                m_sphBoundaryConditions->getBufferIndices().pop_back();
                particleTypes[bufferParticleIndex] = SphBoundaryConditions::ParticleType::Inlet;

                positions[bufferParticleIndex] = m_sphBoundaryConditions->placeParticleAtInlet(oldPosition);
                halfStepVelocities[bufferParticleIndex] = m_sphBoundaryConditions->computeParabolicInletVelocity(positions[bufferParticleIndex]);
                fullStepVelocities[bufferParticleIndex] = m_sphBoundaryConditions->computeParabolicInletVelocity(positions[bufferParticleIndex]);
            }
            else if (particleTypes[p] == SphBoundaryConditions::ParticleType::Outlet
                     && !m_sphBoundaryConditions->isInOutletDomain(newPosition))
            {
                particleTypes[p] = SphBoundaryConditions::ParticleType::Buffer;
                // insert particle into buffer domain after it leaves outlet domain
                positions[p] = m_sphBoundaryConditions->getBufferCoord();
                m_sphBoundaryConditions->getBufferIndices().push_back(p);
            }
            else if (particleTypes[p] == SphBoundaryConditions::ParticleType::Fluid
                     && m_sphBoundaryConditions->isInOutletDomain(newPosition))
            {
                particleTypes[p] = SphBoundaryConditions::ParticleType::Outlet;
            }
            else if (particleTypes[p] == SphBoundaryConditions::ParticleType::Fluid
                     && !m_sphBoundaryConditions->isInFluidDomain(newPosition))
            {
                particleTypes[p] = SphBoundaryConditions::ParticleType::Buffer;
                positions[p]     = m_sphBoundaryConditions->getBufferCoord();
                m_sphBoundaryConditions->getBufferIndices().push_back(p);
            }
        }
    }
    m_timeStepCount++;
}

double
SphModel::getParticlePressure(const double density)
{
    const double d     = density / m_modelParameters->m_restDensity;
    const double d2    = d * d;
    const double d4    = d2 * d2;
    const double error = m_modelParameters->m_pressureStiffness * (d4 * d2 * d - 1.0);
    // clamp pressure error to zero to maintain stability
    return error > 0.0 ? error : 0.0;
}

void
SphModel::setInitialVelocities(const size_t numParticles, const Vec3d& initialVelocity)
{
    m_initialVelocities->clear();
    m_initialVelocities->reserve(static_cast<int>(numParticles));
    for (size_t p = 0; p < numParticles; p++)
    {
        if (m_sphBoundaryConditions
            && (m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Buffer
                || m_sphBoundaryConditions->getParticleTypes()[p] == SphBoundaryConditions::ParticleType::Wall))
        {
            m_initialVelocities->push_back(Vec3d::Zero());
        }
        else
        {
            m_initialVelocities->push_back(initialVelocity);
        }
    }
}

void
SphModel::findNearestParticleToVertex(const VecDataArray<double, 3>& points, const std::vector<std::vector<size_t>>& indices)
{
    const VecDataArray<double, 3>& positions = *getCurrentState()->getPositions();
    for (size_t i = 0; i < static_cast<size_t>(points.size()); i++)
    {
        double minDistance = 1e10;
        size_t minIndex    = 0;
        for (const size_t j : indices[i])
        {
            const Vec3d  p1       = positions[j];
            const double distance = (points[i] - p1).norm();
            if (distance < minDistance)
            {
                minDistance = distance;
                minIndex    = j;
            }
        }
        m_minIndices[i] = minIndex;
    }
}
} // namespace imstk