BarrierContactForm.cpp

Go to the documentation of this file.

#include "BarrierContactForm.hpp"
 
#include <polyfem/utils/Logger.hpp>
#include <polyfem/utils/MaybeParallelFor.hpp>
#include <polyfem/utils/Timer.hpp>
#include <polyfem/utils/Types.hpp>
 
#include <ipc/barrier/adaptive_stiffness.hpp>
#include <ipc/utils/world_bbox_diagonal_length.hpp>
 
#include <algorithm>
#include <cassert>
#include <cmath>
 
namespace polyfem::solver
{
    BarrierContactForm::BarrierContactForm(const ipc::CollisionMesh &collision_mesh,
                                           const double dhat,
                                           const double avg_mass,
                                           const bool use_area_weighting,
                                           const bool use_improved_max_operator,
                                           const bool use_physical_barrier,
                                           const bool use_adaptive_barrier_stiffness,
                                           const bool is_time_dependent,
                                           const bool enable_shape_derivatives,
                                           const ipc::BroadPhaseMethod broad_phase_method,
                                           const double ccd_tolerance,
                                           const int ccd_max_iterations) : ContactForm(collision_mesh, dhat, avg_mass, use_adaptive_barrier_stiffness, is_time_dependent, enable_shape_derivatives, broad_phase_method, ccd_tolerance, ccd_max_iterations), barrier_potential_(dhat, 1.0, use_physical_barrier)
    {
        // collision_set_.set_use_convergent_formulation(use_convergent_formulation);
        collision_set_.set_use_area_weighting(use_area_weighting);
        collision_set_.set_use_improved_max_approximator(use_improved_max_operator);
        collision_set_.set_enable_shape_derivatives(enable_shape_derivatives);
    }
 
    void BarrierContactForm::update_barrier_stiffness(const Eigen::VectorXd &x, const Eigen::MatrixXd &grad_energy)
    {
        if (!use_adaptive_barrier_stiffness())
            return;
 
        const Eigen::MatrixXd displaced_surface = compute_displaced_surface(x);
 
        // The adative stiffness is designed for the non-convergent formulation,
        // so we need to compute the gradient of the non-convergent barrier.
        // After we can map it to a good value for the convergent formulation.
        ipc::NormalCollisions nonconvergent_constraints;
        // nonconvergent_constraints.set_use_convergent_formulation(false);
        nonconvergent_constraints.set_use_area_weighting(false);
        nonconvergent_constraints.set_use_improved_max_approximator(false);
        nonconvergent_constraints.build(
            collision_mesh_, displaced_surface, dhat_, dmin_, broad_phase_.get());
        Eigen::VectorXd grad_barrier = barrier_potential_.gradient(
            nonconvergent_constraints, collision_mesh_, displaced_surface);
        grad_barrier = collision_mesh_.to_full_dof(grad_barrier);
 
        barrier_stiffness_ = ipc::initial_barrier_stiffness(
            ipc::world_bbox_diagonal_length(displaced_surface), barrier_potential_.barrier(), dhat_, avg_mass_,
            grad_energy, grad_barrier, max_barrier_stiffness_);
 
        if (use_convergent_formulation())
        {
            double scaling_factor = 0;
            if (!nonconvergent_constraints.empty())
            {
                const double nonconvergent_potential = barrier_potential_(
                    nonconvergent_constraints, collision_mesh_, displaced_surface);
 
                update_collision_set(displaced_surface);
                const double convergent_potential = barrier_potential_(
                    collision_set_, collision_mesh_, displaced_surface);
 
                scaling_factor = nonconvergent_potential / convergent_potential;
            }
            else
            {
                // Hardcoded difference between the non-convergent and convergent barrier
                scaling_factor = dhat_ * std::pow(dhat_ + 2 * dmin_, 2);
            }
            barrier_stiffness_ *= scaling_factor;
            max_barrier_stiffness_ *= scaling_factor;
        }
 
        // The barrier stiffness is choosen based on including the acceleration scaling,
        // but the acceleration scaling will be applied later. Therefore, we need to remove it.
        barrier_stiffness_ /= weight_;
        max_barrier_stiffness_ /= weight_;
 
        logger().debug(
            "Setting adaptive barrier stiffness to {} (max barrier stiffness: {})",
            barrier_stiffness(), max_barrier_stiffness_);
    }
 
    void BarrierContactForm::update_collision_set(const Eigen::MatrixXd &displaced_surface)
    {
        // Store the previous value used to compute the constraint set to avoid duplicate computation.
        static Eigen::MatrixXd cached_displaced_surface;
        if (cached_displaced_surface.size() == displaced_surface.size() && cached_displaced_surface == displaced_surface)
            return;
 
        if (use_cached_candidates_)
            collision_set_.build(
                candidates_, collision_mesh_, displaced_surface, dhat_);
        else
            collision_set_.build(
                collision_mesh_, displaced_surface, dhat_, dmin_, broad_phase_.get());
        cached_displaced_surface = displaced_surface;
    }
 
    double BarrierContactForm::value_unweighted(const Eigen::VectorXd &x) const
    {
        return barrier_potential_(collision_set_, collision_mesh_, compute_displaced_surface(x));
    }
 
    Eigen::VectorXd BarrierContactForm::value_per_element_unweighted(const Eigen::VectorXd &x) const
    {
        const Eigen::MatrixXd V = compute_displaced_surface(x);
        assert(V.rows() == collision_mesh_.num_vertices());
 
        const size_t num_vertices = collision_mesh_.num_vertices();
 
        if (collision_set_.empty())
        {
            return Eigen::VectorXd::Zero(collision_mesh_.full_num_vertices());
        }
 
        const Eigen::MatrixXi &E = collision_mesh_.edges();
        const Eigen::MatrixXi &F = collision_mesh_.faces();
 
        auto storage = utils::create_thread_storage<Eigen::VectorXd>(Eigen::VectorXd::Zero(num_vertices));
 
        utils::maybe_parallel_for(collision_set_.size(), [&](int start, int end, int thread_id) {
            Eigen::VectorXd &local_storage = utils::get_local_thread_storage(storage, thread_id);
 
            for (size_t i = start; i < end; i++)
            {
                // Quadrature weight is premultiplied by compute_potential
                const double potential = barrier_potential_(collision_set_[i], collision_set_[i].dof(V, E, F));
 
                const int n_v = collision_set_[i].num_vertices();
                const auto vis = collision_set_[i].vertex_ids(E, F);
                for (int j = 0; j < n_v; j++)
                {
                    assert(0 <= vis[j] && vis[j] < num_vertices);
                    local_storage[vis[j]] += potential / n_v;
                }
            }
        });
 
        Eigen::VectorXd out = Eigen::VectorXd::Zero(num_vertices);
        for (const auto &local_potential : storage)
        {
            out += local_potential;
        }
 
        Eigen::VectorXd out_full = Eigen::VectorXd::Zero(collision_mesh_.full_num_vertices());
        for (int i = 0; i < out.size(); i++)
            out_full[collision_mesh_.to_full_vertex_id(i)] = out[i];
 
        assert(std::abs(value_unweighted(x) - out_full.sum()) < std::max(1e-10 * out_full.sum(), 1e-10));
 
        return out_full;
    }
 
    void BarrierContactForm::first_derivative_unweighted(const Eigen::VectorXd &x, Eigen::VectorXd &gradv) const
    {
        gradv = barrier_potential_.gradient(collision_set_, collision_mesh_, compute_displaced_surface(x));
        gradv = collision_mesh_.to_full_dof(gradv);
    }
 
    void BarrierContactForm::second_derivative_unweighted(const Eigen::VectorXd &x, StiffnessMatrix &hessian) const
    {
        POLYFEM_SCOPED_TIMER("barrier hessian");
 
        ipc::PSDProjectionMethod psd_projection_method;
 
        if (project_to_psd_)
        {
            psd_projection_method = ipc::PSDProjectionMethod::CLAMP;
        }
        else
        {
            psd_projection_method = ipc::PSDProjectionMethod::NONE;
        }
 
        hessian = barrier_potential_.hessian(collision_set_, collision_mesh_, compute_displaced_surface(x), psd_projection_method);
        hessian = collision_mesh_.to_full_dof(hessian);
    }
 
    void BarrierContactForm::post_step(const polysolve::nonlinear::PostStepData &data)
    {
        const Eigen::MatrixXd displaced_surface = compute_displaced_surface(data.x);
 
        const double curr_distance = collision_set_.compute_minimum_distance(collision_mesh_, displaced_surface);
        if (!std::isinf(curr_distance))
        {
            const double ratio = sqrt(curr_distance) / dhat();
            const auto log_level = (ratio < 1e-6) ? spdlog::level::err : ((ratio < 1e-4) ? spdlog::level::warn : spdlog::level::debug);
            polyfem::logger().log(log_level, "Minimum distance during solve: {}, dhat: {}", sqrt(curr_distance), dhat());
        }
 
        if (data.iter_num == 0)
            return;
 
        if (use_adaptive_barrier_stiffness_)
        {
            if (is_time_dependent_)
            {
                const double prev_barrier_stiffness = barrier_stiffness();
 
                barrier_stiffness_ = ipc::update_barrier_stiffness(
                    prev_distance_, curr_distance, max_barrier_stiffness_,
                    barrier_stiffness(), ipc::world_bbox_diagonal_length(displaced_surface));
 
                if (barrier_stiffness() != prev_barrier_stiffness)
                {
                    polyfem::logger().debug(
                        "updated barrier stiffness from {:g} to {:g} (max barrier stiffness: )",
                        prev_barrier_stiffness, barrier_stiffness(), max_barrier_stiffness_);
                }
            }
            else
            {
                // TODO: missing feature
                // update_barrier_stiffness(data.x);
            }
        }
 
        prev_distance_ = curr_distance;
    }
} // namespace polyfem::solver