STYLE: Cleanup the ConstantDensityConstraint, move to source

Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp

@@ -28,13 +28,6 @@ void
 PbdConstantDensityConstraint::initConstraint(const VecDataArray<double, 3>& initVertexPositions, const double)
 {
     const size_t numParticles = initVertexPositions.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(numParticles);
     m_densities.resize(numParticles);
     m_deltaPositions.resize(numParticles);
@@ -71,31 +64,6 @@ PbdConstantDensityConstraint::projectConstraint(const DataArray<double>& imstkNo
     });
 }
 
-double
-PbdConstantDensityConstraint::wPoly6(const Vec3d& pi, const Vec3d& pj)
-{
-    double rLengthSqr = (Vec3d(pi - pj)).squaredNorm();
-
-    return (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20) ?
-           0 :
-           m_wPoly6Coeff* pow(m_maxDistSqr - rLengthSqr, 3);
-}
-
-Vec3d
-PbdConstantDensityConstraint::gradSpiky(const Vec3d& pi, const Vec3d& pj)
-{
-    Vec3d        r = pi - pj;
-    const double rLengthSqr = r.squaredNorm();
-
-    if (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20)
-    {
-        return Vec3d(0., 0., 0.);
-    }
-
-    const double rLength = std::sqrt(rLengthSqr);
-    return r * (m_wSpikyCoeff * (-3.0) * (m_maxDist - rLength) * (m_maxDist - rLength));
-}
-
 void
 PbdConstantDensityConstraint::computeDensity(const Vec3d& pi,
                                              const size_t index,
@@ -115,11 +83,13 @@ PbdConstantDensityConstraint::computeLambdaScalingFactor(const Vec3d& pi,
                                                          const size_t index,
                                                          const VecDataArray<double, 3>& positions)
 {
-    const double densityConstraint = (m_densities[index] / m_restDensity) - 1;
+    const double invRestDensity    = 1.0 / m_restDensity;
+    const double densityConstraint = (m_densities[index] * invRestDensity) - 1.0;
     double       gradientSum       = 0.0;
+
     for (auto q : m_neighborList[index])
     {
-        gradientSum += gradSpiky(pi, positions[q]).squaredNorm() / m_restDensity;
+        gradientSum += gradSpiky(pi, positions[q]).squaredNorm() * invRestDensity;
     }
 
     m_lambdas[index] = densityConstraint / (gradientSum + m_relaxationParameter);
@@ -130,12 +100,12 @@ PbdConstantDensityConstraint::updatePositions(const Vec3d& pi,
                                               const size_t index,
                                               VecDataArray<double, 3>& positions)
 {
-    //Make sure the point is valid
-    Vec3d gradientLambdaSum(0., 0., 0.);
+    // Make sure the point is valid
+    Vec3d gradientLambdaSum(0.0, 0.0, 0.0);
     for (auto q : m_neighborList[index])
     {
-        double lambdasDiff = (m_lambdas[index] + m_lambdas[q]);
-        Vec3d  gradKernal  = gradSpiky(pi, positions[q]);
+        const double lambdasDiff = (m_lambdas[index] + m_lambdas[q]);
+        const Vec3d  gradKernal  = gradSpiky(pi, positions[q]);
         gradientLambdaSum += (gradKernal * lambdasDiff);
     }
 
@@ -146,8 +116,10 @@ PbdConstantDensityConstraint::updatePositions(const Vec3d& pi,
 void
 PbdConstantDensityConstraint::setMaxNeighborDistance(const double dist)
 {
-    m_maxDist    = dist;
-    m_maxDistSqr = dist * dist;
+    m_maxDist     = dist;
+    m_maxDistSqr  = dist * dist;
+    m_wPoly6Coeff = 315.0 / (64.0 * PI * pow(m_maxDist, 9));
+    m_wSpikyCoeff = 15.0 / (PI * pow(m_maxDist, 6)) * -3.0;
     if (m_NeighborSearcher)
     {
         m_NeighborSearcher->setSearchRadius(m_maxDist);

Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h

@@ -41,11 +41,7 @@ public:
     ///
     PbdConstantDensityConstraint() : PbdConstraint()
     {
-        // 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));
+        setMaxNeighborDistance(m_maxDist);
     }
 
     ///
@@ -78,12 +74,36 @@ private:
     ///
     /// \brief Smoothing kernel WPoly6 for density estimation
     ///
-    double wPoly6(const Vec3d& pi, const Vec3d& pj);
+    inline double wPoly6(const Vec3d& pi, const Vec3d& pj) const
+    {
+        const double rLengthSqr = (Vec3d(pi - pj)).squaredNorm();
+        if (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20)
+        {
+            return 0.0;
+        }
+        else
+        {
+            const double maxDiff = m_maxDistSqr - rLengthSqr;
+            return m_wPoly6Coeff * maxDiff * maxDiff * maxDiff;
+        }
+    }
 
     ///
     /// \brief Gradient of density kernel
     ///
-    Vec3d gradSpiky(const Vec3d& pi, const Vec3d& pj);
+    inline Vec3d gradSpiky(const Vec3d& pi, const Vec3d& pj) const
+    {
+        const Vec3d  r = pi - pj;
+        const double rLengthSqr = r.squaredNorm();
+
+        if (rLengthSqr > m_maxDistSqr || rLengthSqr < 1e-20)
+        {
+            return Vec3d::Zero();
+        }
+
+        const double rLength = std::sqrt(rLengthSqr);
+        return r * (m_wSpikyCoeff * (m_maxDist - rLength) * (m_maxDist - rLength));
+    }
 
     ///
     /// \brief
@@ -104,22 +124,21 @@ private:
     /// \brief Set/Get rest density
     ///
     void setDensity(const double density) { m_restDensity = density; }
-    double getDensity() { return m_restDensity; }
+    double getDensity() const { return m_restDensity; }
 
     ///
     /// \brief Set/Get max. neighbor distance
     ///
     void setMaxNeighborDistance(const double dist);
-    double getMaxNeighborDistance() { return m_maxDist; }
+    double getMaxNeighborDistance() const { return m_maxDist; }
 
     ///
     /// \brief Set/Get neighbor search method
     ///
     void setNeighborSearchMethod(NeighborSearch::Method method) { m_NeighborSearchMethod = method; }
-    NeighborSearch::Method getNeighborSearchMethod() { return m_NeighborSearchMethod; }
+    NeighborSearch::Method getNeighborSearchMethod() const { return m_NeighborSearchMethod; }
 
 private:
-
     double m_wPoly6Coeff;
     double m_wSpikyCoeff;