Add semi-implicit grid integration for snow MPM

Includes/Core/Math/CG-Impl.hpp

@@ -13,6 +13,8 @@
 
 #include <Core/Math/MathUtils.hpp>
 
+#include <limits>
+
 namespace CubbyFlow
 {
 template <typename BLASType>
@@ -32,6 +34,75 @@
                                lastResidualNorm);
 }
 
+template <typename BLASType>
+void CR(const typename BLASType::MatrixType& A,
+        const typename BLASType::VectorType& b,
+        unsigned int maxNumberOfIterations, double tolerance,
+        typename BLASType::VectorType* x, typename BLASType::VectorType* r,
+        typename BLASType::VectorType* d, typename BLASType::VectorType* q,
+        typename BLASType::VectorType* s, unsigned int* lastNumberOfIterations,
+        double* lastResidualNorm)
+{
+    BLASType::Residual(A, *x, b, r);
+    BLASType::Set(*r, d);
+    BLASType::MVM(A, *r, s);
+    BLASType::Set(*s, q);
+
+    double rho = BLASType::Dot(*r, *s);
+    double residualNorm = BLASType::L2Norm(*r);
+    unsigned int iter = 0;
+
+    while (residualNorm > tolerance && iter < maxNumberOfIterations)
+    {
+        const double denominator = BLASType::Dot(*q, *q);
+
+        if (!std::isfinite(rho) || !std::isfinite(denominator) ||
+            denominator <= 0.0 || rho == 0.0)
+        {
+            residualNorm = std::numeric_limits<double>::infinity();
+            break;
+        }
+
+        const double alpha = rho / denominator;
+
+        BLASType::AXPlusY(alpha, *d, *x, x);
+        BLASType::AXPlusY(-alpha, *q, *r, r);
+
+        residualNorm = BLASType::L2Norm(*r);
+        ++iter;
+
+        if (!std::isfinite(residualNorm))
+        {
+            residualNorm = std::numeric_limits<double>::infinity();
+            break;
+        }
+
+        if (residualNorm <= tolerance)
+        {
+            break;
+        }
+
+        BLASType::MVM(A, *r, s);
+
+        const double rhoNew = BLASType::Dot(*r, *s);
+
+        if (!std::isfinite(rhoNew))
+        {
+            residualNorm = std::numeric_limits<double>::infinity();
+            break;
+        }
+
+        const double beta = rhoNew / rho;
+
+        BLASType::AXPlusY(beta, *d, *r, d);
+        BLASType::AXPlusY(beta, *q, *s, q);
+        rho = rhoNew;
+    }
+
+    *lastNumberOfIterations = iter;
+    *lastResidualNorm = residualNorm;
+}
+
 template <typename BLASType, typename PrecondType>
 void PCG(const typename BLASType::MatrixType& A,
          const typename BLASType::VectorType& b,
@@ -113,4 +184,4 @@
 }
 }  // namespace CubbyFlow
 
-#endif
+#endif

Includes/Core/Math/CG.hpp

@@ -48,6 +48,18 @@ void CG(const typename BLASType::MatrixType& A,
         typename BLASType::VectorType* s, unsigned int* lastNumberOfIterations,
         double* lastResidualNorm);
 
+//!
+//! \brief Solves a symmetric linear system with conjugate residual.
+//!
+template <typename BLASType>
+void CR(const typename BLASType::MatrixType& A,
+        const typename BLASType::VectorType& b,
+        unsigned int maxNumberOfIterations, double tolerance,
+        typename BLASType::VectorType* x, typename BLASType::VectorType* r,
+        typename BLASType::VectorType* d, typename BLASType::VectorType* q,
+        typename BLASType::VectorType* s, unsigned int* lastNumberOfIterations,
+        double* lastResidualNorm);
+
 //!
 //! \brief Solves pre-conditioned conjugate gradient.
 //!
@@ -63,4 +75,4 @@ void PCG(const typename BLASType::MatrixType& A,
 
 #include <Core/Math/CG-Impl.hpp>
 
-#endif
+#endif

Includes/Core/Particle/MPM/SnowConstitutiveModel-Impl.hpp

@@ -60,7 +60,7 @@ SnowConstitutiveModel<N>::SnowConstitutiveModel(double youngsModulus,
 }
 
 template <size_t N>
-typename SnowConstitutiveModel<N>::State SnowConstitutiveModel<N>::Update(
+SnowConstitutiveModel<N>::State SnowConstitutiveModel<N>::Update(
     const MatrixType& deformationGradientIncrement, const State& state) const
 {
     ValidateDeformation(deformationGradientIncrement);
@@ -99,7 +99,7 @@ typename SnowConstitutiveModel<N>::State SnowConstitutiveModel<N>::Update(
 }
 
 template <size_t N>
-typename SnowConstitutiveModel<N>::MatrixType
+SnowConstitutiveModel<N>::MatrixType
 SnowConstitutiveModel<N>::ComputeKirchhoffStress(const State& state) const
 {
     ValidateDeformation(state.elastic);
@@ -130,6 +130,75 @@ SnowConstitutiveModel<N>::ComputeKirchhoffStress(const State& state) const
     return stress;
 }
 
+template <size_t N>
+SnowConstitutiveModel<N>::MatrixType
+SnowConstitutiveModel<N>::ComputeFirstPiolaStressDifferential(
+    const State& state, const MatrixType& differential) const
+{
+    ValidateDeformation(state.elastic);
+    ValidateDeformation(state.plastic);
+
+    if (!IsFinite(differential))
+    {
+        throw std::invalid_argument{ "Invalid snow deformation differential." };
+    }
+
+    MatrixType u;
+    Vector<double, N> singularValues;
+    MatrixType v;
+
+    SVD(state.elastic, u, singularValues, v);
+
+    const MatrixType principalDifferential = u.Transposed() * differential * v;
+    MatrixType omega;
+
+    for (size_t i = 0; i < N; ++i)
+    {
+        for (size_t j = i + 1; j < N; ++j)
+        {
+            const double value =
+                (principalDifferential(i, j) - principalDifferential(j, i)) /
+                (singularValues[i] + singularValues[j]);
+            omega(i, j) = value;
+            omega(j, i) = -value;
+        }
+    }
+    const MatrixType rotationDifferential = u * omega * v.Transposed();
+    const MatrixType& f = state.elastic;
+    const double determinant = f.Determinant();
+    const MatrixType inverseTranspose = f.Inverse().Transposed();
+    const MatrixType cofactor = determinant * inverseTranspose;
+    double determinantDifferential = 0.0;
+
+    for (size_t row = 0; row < N; ++row)
+    {
+        for (size_t column = 0; column < N; ++column)
+        {
+            determinantDifferential +=
+                cofactor(row, column) * differential(row, column);
+        }
+    }
+
+    const MatrixType cofactorDifferential =
+        determinantDifferential * inverseTranspose -
+        determinant * inverseTranspose * differential.Transposed() *
+            inverseTranspose;
+    const double hardening = ComputeHardening(state);
+    const double mu = m_mu0 * hardening;
+    const double lambda = m_lambda0 * hardening;
+    const MatrixType result =
+        2.0 * mu * (differential - rotationDifferential) +
+        lambda * (determinantDifferential * cofactor +
+                  (determinant - 1.0) * cofactorDifferential);
+
+    if (!IsFinite(result))
+    {
+        throw std::invalid_argument{ "Non-finite snow stress differential." };
+    }
+
+    return result;
+}
+
 template <size_t N>
 double SnowConstitutiveModel<N>::ComputeWaveSpeed(const State& state,
                                                   double referenceDensity) const

Includes/Core/Particle/MPM/SnowConstitutiveModel.hpp

@@ -90,6 +90,15 @@ class SnowConstitutiveModel final
     //! \return Fixed-corotated Kirchhoff stress.
     [[nodiscard]] MatrixType ComputeKirchhoffStress(const State& state) const;
 
+    //!
+    //! \brief Computes the first Piola stress differential.
+    //!
+    //! Evaluates `(d^2 Psi / d F_E d F_E) : differential` while holding the
+    //! plastic deformation fixed.
+    //!
+    [[nodiscard]] MatrixType ComputeFirstPiolaStressDifferential(
+        const State& state, const MatrixType& differential) const;
+
     //!
     //! \brief Estimates the fastest elastic wave speed for the state.
     //!