<commit_before>#include "chrono_parallel/solver/ChSolverAPGD.h"
#include <blaze/math/CompressedVector.h>
using namespace chrono;

ChSolverAPGD::ChSolverAPGD() : ChSolverParallel(), mg_tmp_norm(0), mb_tmp_norm(0), obj1(0), obj2(0), norm_ms(0), dot_g_temp(0), theta(1), theta_new(0), beta_new(0), t(0), L(0), g_diff(0) {}

void ChSolverAPGD::UpdateR() {

  if (data_container->num_constraints <= 0) {
    return;
  }

  if (!data_container->settings.solver.update_rhs) {
    return;
  }

  const CompressedMatrix<real>& D_n_T = data_container->host_data.D_n_T;
  const DynamicVector<real>& M_invk = data_container->host_data.M_invk;
  const DynamicVector<real>& b = data_container->host_data.b;
  DynamicVector<real>& R = data_container->host_data.R;
  DynamicVector<real>& s = data_container->host_data.s;

  uint num_contacts = data_container->num_contacts;

  s.resize(data_container->num_contacts);
  reset(s);

  rigid_rigid->Build_s();

  blaze::DenseSubvector<const DynamicVector<real> > b_n = blaze::subvector(b, 0, num_contacts);
  blaze::DenseSubvector<DynamicVector<real> > R_n = blaze::subvector(R, 0, num_contacts);
  blaze::DenseSubvector<DynamicVector<real> > s_n = blaze::subvector(s, 0, num_contacts);

  R_n = -b_n - D_n_T * M_invk + s_n;
}

uint ChSolverAPGD::SolveAPGD(const uint max_iter, const uint size, const blaze::DynamicVector<real>& r, blaze::DynamicVector<real>& gamma) {
  real& residual = data_container->measures.solver.residual;
  real& objective_value = data_container->measures.solver.objective_value;
  custom_vector<real>& iter_hist = data_container->measures.solver.iter_hist;

  blaze::DynamicVector<real> one(size, 1.0);
  data_container->system_timer.start("ChSolverParallel_Solve");

  N_gamma_new.resize(size);
  temp.resize(size);
  g.resize(size);
  gamma_new.resize(size);
  y.resize(size);

  residual = 10e30;
  g_diff = 1.0 / pow(size, 2.0);

  theta = 1;
  theta_new = theta;
  beta_new = 0.0;
  mb_tmp_norm = 0, mg_tmp_norm = 0;
  obj1 = 0.0, obj2 = 0.0;
  dot_g_temp = 0, norm_ms = 0;

  // Is the initial projection necessary?
  // Project(gamma.data());
  // gamma_hat = gamma;
  // ShurProduct(gamma, mg);
  // mg = mg - r;

  temp = gamma - one;
  L = sqrt((real)(temp, temp));
  ShurProduct(temp, temp);
  L = L == 0 ? 1 : L;
  L = sqrt((real)(temp, temp)) / L;

  t = 1.0 / L;
  y = gamma;

  for (current_iteration = 0; current_iteration < max_iter; current_iteration++) {

    ShurProduct(y, g);
    g = g - r;
    gamma_new = y - t * g;

    Project(gamma_new.data());

    ShurProduct(gamma_new, N_gamma_new);
    obj1 = 0.5 * (gamma_new, N_gamma_new) - (gamma_new, r);

    ShurProduct(y, temp);
    obj2 = 0.5 * (y, temp) - (y, r);

    temp = gamma_new - y;
    dot_g_temp = (g, temp);
    norm_ms = (temp, temp);

    while (obj1 > obj2 + dot_g_temp + 0.5 * L * norm_ms) {
      L = 2.0 * L;
      t = 1.0 / L;
      gamma_new = y - t * g;
      Project(gamma_new.data());
      ShurProduct(gamma_new, N_gamma_new);
      obj1 = 0.5 * (gamma_new, N_gamma_new) - (gamma_new, r);
      temp = gamma_new - y;
      dot_g_temp = (g, temp);
      norm_ms = (temp, temp);
    }
    theta_new = (-pow(theta, 2.0) + theta * sqrt(pow(theta, 2.0) + 4.0)) / 2.0;
    beta_new = theta * (1.0 - theta) / (pow(theta, 2.0) + theta_new);

    temp = gamma_new - gamma;
    y = beta_new * temp + gamma_new;
    dot_g_temp = (g, temp);

    // Compute the residual
    temp = gamma_new - g_diff * (N_gamma_new - r);
    Project(temp.data());
    temp = (1.0 / g_diff) * (gamma_new - temp);
    real res = sqrt((real)(temp, temp));

    if (res < residual) {
      residual = res;
      gamma_hat = gamma_new;
    }

    // Compute the objective value
    temp = 0.5 * N_gamma_new - r;
    objective_value = (gamma_new, temp);

    AtIterationEnd(residual, objective_value);

    if (data_container->settings.solver.test_objective) {
      if (objective_value <= data_container->settings.solver.tolerance_objective) {
        break;
      }
    } else {
      if (residual < data_container->settings.solver.tol_speed) {
        break;
      }
    }

    if (dot_g_temp > 0) {
      y = gamma_new;
      theta_new = 1.0;
    }

    L = 0.9 * L;
    t = 1.0 / L;
    theta = theta_new;

    gamma = gamma_new;

    UpdateR();
  }<commit_msg>Small fixes for APGD<commit_after>#include "chrono_parallel/solver/ChSolverAPGD.h"
#include <blaze/math/CompressedVector.h>
using namespace chrono;

ChSolverAPGD::ChSolverAPGD() : ChSolverParallel(), mg_tmp_norm(0), mb_tmp_norm(0), obj1(0), obj2(0), norm_ms(0), dot_g_temp(0), theta(1), theta_new(0), beta_new(0), t(0), L(0), g_diff(0) {}

void ChSolverAPGD::UpdateR() {
  const CompressedMatrix<real>& D_n_T = data_container->host_data.D_n_T;
  const DynamicVector<real>& M_invk = data_container->host_data.M_invk;
  const DynamicVector<real>& b = data_container->host_data.b;
  DynamicVector<real>& R = data_container->host_data.R;
  DynamicVector<real>& s = data_container->host_data.s;

  uint num_contacts = data_container->num_contacts;

  s.resize(data_container->num_contacts);
  reset(s);

  rigid_rigid->Build_s();

  blaze::DenseSubvector<const DynamicVector<real> > b_n = blaze::subvector(b, 0, num_contacts);
  blaze::DenseSubvector<DynamicVector<real> > R_n = blaze::subvector(R, 0, num_contacts);
  blaze::DenseSubvector<DynamicVector<real> > s_n = blaze::subvector(s, 0, num_contacts);

  R_n = -b_n - D_n_T * M_invk + s_n;
}

uint ChSolverAPGD::SolveAPGD(const uint max_iter, const uint size, const blaze::DynamicVector<real>& r, blaze::DynamicVector<real>& gamma) {
  real& residual = data_container->measures.solver.residual;
  real& objective_value = data_container->measures.solver.objective_value;
  custom_vector<real>& iter_hist = data_container->measures.solver.iter_hist;

  blaze::DynamicVector<real> one(size, 1.0);
  data_container->system_timer.start("ChSolverParallel_Solve");
  gamma_hat.resize(size);
  N_gamma_new.resize(size);
  temp.resize(size);
  g.resize(size);
  gamma_new.resize(size);
  y.resize(size);

  residual = 10e30;
  g_diff = 1.0 / pow(size, 2.0);

  theta = 1;
  theta_new = theta;
  beta_new = 0.0;
  mb_tmp_norm = 0, mg_tmp_norm = 0;
  obj1 = 0.0, obj2 = 0.0;
  dot_g_temp = 0, norm_ms = 0;

  // Is the initial projection necessary?
  // Project(gamma.data());
  // gamma_hat = gamma;
  // ShurProduct(gamma, mg);
  // mg = mg - r;

  temp = gamma - one;
  real norm_temp = sqrt((real)(temp, temp));

  //If gamma is one temp should be zero, in that case set L to one
  //We cannot divide by 0
  if (norm_temp == 0) {
    L = 1.0;
  } else {
    //If the N matrix is zero for some reason, temp will be zero
    ShurProduct(temp, temp);
    // If temp is zero then L will be zero
    L = sqrt((real)(temp, temp)) / norm_temp;
  }
  //When L is zero the step length can't be computed, in this case just return
  //If the N is indeed zero then solving doesn't make sense
  if (L == 0) {
    return 0;
  } else {
    //Compute the step size
    t = 1.0 / L;
  }
  y = gamma;
  //If no iterations are performed or the residual is NAN (which is shouldnt be)
  //make sure that gamma_hat has something inside of it. Otherwise gamma will be
  //overwritten with a vector of zero size
  gamma_hat = gamma;

  for (current_iteration = 0; current_iteration < max_iter; current_iteration++) {

    ShurProduct(y, g);
    g = g - r;
    gamma_new = y - t * g;

    Project(gamma_new.data());

    ShurProduct(gamma_new, N_gamma_new);
    obj1 = 0.5 * (gamma_new, N_gamma_new) - (gamma_new, r);

    ShurProduct(y, temp);
    obj2 = 0.5 * (y, temp) - (y, r);

    temp = gamma_new - y;
    dot_g_temp = (g, temp);
    norm_ms = (temp, temp);
    while (obj1 > obj2 + dot_g_temp + 0.5 * L * norm_ms) {
      L = 2.0 * L;
      t = 1.0 / L;
      gamma_new = y - t * g;
      Project(gamma_new.data());
      ShurProduct(gamma_new, N_gamma_new);
      obj1 = 0.5 * (gamma_new, N_gamma_new) - (gamma_new, r);
      temp = gamma_new - y;
      dot_g_temp = (g, temp);
      norm_ms = (temp, temp);
    }
    theta_new = (-pow(theta, 2.0) + theta * sqrt(pow(theta, 2.0) + 4.0)) / 2.0;
    beta_new = theta * (1.0 - theta) / (pow(theta, 2.0) + theta_new);

    temp = gamma_new - gamma;
    y = beta_new * temp + gamma_new;
    dot_g_temp = (g, temp);

    // Compute the residual
    temp = gamma_new - g_diff * (N_gamma_new - r);
    real temp_dota = (real)(temp, temp);
    //Project(temp.data());
    temp = (1.0 / g_diff) * (gamma_new - temp);
    real temp_dotb = (real)(temp, temp);
    real res = sqrt(temp_dotb);

    if (res < residual) {
      residual = res;
      gamma_hat = gamma_new;

    }

    // Compute the objective value
    temp = 0.5 * N_gamma_new - r;
    objective_value = (gamma_new, temp);

    AtIterationEnd(residual, objective_value);

    if (data_container->settings.solver.test_objective) {
      if (objective_value <= data_container->settings.solver.tolerance_objective) {
        break;
      }
    } else {
      if (residual < data_container->settings.solver.tol_speed) {
        break;
      }
    }

    if (dot_g_temp > 0) {
      y = gamma_new;
      theta_new = 1.0;
    }

    L = 0.9 * L;
    t = 1.0 / L;
    theta = theta_new;

    if (data_container->settings.solver.update_rhs) {
    gamma = gamma_new;
    UpdateR();
    }
  }
{'fraction_non_alphanumeric': 0.14586919104991394, 'fraction_numerical': 0.01882530120481928, 'mean_word_length': 3.1356761565836297, 'pattern_counts': {'":': 0, '<': 44, '<?xml version=': 0, '>': 72, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4552488', 'n_tokens_mistral': 3779, 'n_tokens_neox': 3530, 'n_words': 787}