soc.hpp

Go to the documentation of this file.

/*
 * Copyright © 2025-2026 Akreon LLC
 * SPDX-License-Identifier: Apache-2.0
 */
 
#pragma once
 
#include "akreon/linalg/constants.hpp"
#include "cone.hpp"
 
namespace conic {
 
template <affine::AffineMapLike Map>
struct Rotated2SOC {
  using Mat = affine::affine_mat_t<Map>;
  using T = linalg::matrix_scalar_t<Mat>;
  static constexpr std::size_t M = linalg::mat_rows_v<Mat>;
  static constexpr std::size_t N = linalg::mat_cols_v<Mat>;
 
  constexpr auto operator()(const Map& map) {
    static_assert(M >= 3,
                  "Rotated2SOC requires RSOC coords with at least 3 entries "
                  "(u, v, w)");
 
    const auto I = linalg::make_identity<T, M - 2>();
 
    linalg::Matrix<T, M, M> A{};
 
    A = linalg::set_block<0, M - 2, 0, M - 2>(A, I);
    A(M - 2, M - 2) = linalg::inv_sqrt2_v<T>;
    A(M - 2, M - 1) = -linalg::inv_sqrt2_v<T>;
    A(M - 1, M - 2) = linalg::inv_sqrt2_v<T>;
    A(M - 1, M - 1) = linalg::inv_sqrt2_v<T>;
 
    affine::AffineMap<decltype(A)> r2soc(A);
    return r2soc.compose(map);
  }
};
 
template <linalg::VectorContainer Vec>
struct SOCCirc {
  using T = linalg::vector_element_t<Vec>;
  static constexpr std::size_t N = linalg::vector_dim_v<Vec>;
 
  constexpr auto operator()(const Vec& x, const Vec& y) const {
    const auto primal = std::get<0>(linalg::splice<N - 1>(x));
    const auto t1 = x(N - 1);
    const auto dual = std::get<0>(linalg::splice<N - 1>(y));
    const auto t2 = y(N - 1);
 
    const auto a = t2 * primal + t1 * dual;
    const auto b = linalg::inner_product(primal, dual) + t1 * t2;
 
    Vec res{};
    for (std::size_t i = 0; i < N - 1; ++i) res(i) = a(i);
    res(N - 1) = b;
    return res;
  }
};
 
template <linalg::VectorContainer Slack>
struct SecondOrderCone
    : public Cone<Slack, SecondOrderCone<Slack>, ConeType::SYMMETRIC> {
  using ConeBase = Cone<Slack, SecondOrderCone<Slack>, ConeType::SYMMETRIC>;
 
  using ConeBase::eps;
  using ConeBase::M;
  using typename ConeBase::HessianMat;
  using typename ConeBase::T;
 
  constexpr Slack x0_impl() const {
    Slack s{};
    s(M - 1) = T(1);
    return s;
  }
 
  constexpr Slack y0_impl() const { return x0_impl(); }
 
  constexpr bool in_cone_impl(const Slack& x) const {
    T res{};
    for (std::size_t i = 0; i < M - 1; ++i) res += x(i) * x(i);
    res = linalg::constexpr_sqrt_safe(res);
    return res < x(M - 1);
  }
 
  constexpr bool in_dual_cone_impl(const Slack& y) const {
    return in_cone_impl(y);
  }
 
  constexpr T alpha_max_impl(const Slack& x,
                             const linalg::Vector<T, M>& p) const {
    constexpr T eta{0.99};
 
    if (in_cone_impl(x + p)) return eta;
 
    T alpha{1};
    T alpha_t{1};
 
    const auto var = std::get<0>(linalg::splice<M - 1>(x));
    const auto t = x(M - 1);
    const auto dvar = std::get<0>(linalg::splice<M - 1>(p));
    const auto dt = p(M - 1);
 
    const auto a = dt * dt - linalg::inner_product(dvar, dvar);
    const auto b = T(2) * (t * dt - linalg::inner_product(var, dvar));
    const auto c = t * t - linalg::inner_product(var, var);
 
    if (linalg::constexpr_abs(a) <= eps) {
      if (b < T(0)) alpha = std::min(alpha, -c / b);
    } else {
      const T disc = b * b - T(4) * a * c;
      if (disc > T(0)) {
        const T s = linalg::constexpr_sqrt_safe(disc);
        const T inv2a = T(1) / (T(2) * a);
        const T r1 = (-b - s) * inv2a;
        const T r2 = (-b + s) * inv2a;
 
        T hit = T(1);
        if (r1 > T(0)) hit = std::min(hit, r1);
        if (r2 > T(0)) hit = std::min(hit, r2);
 
        alpha = std::min(alpha, hit);
      }
    }
 
    if (dt < T(0)) alpha_t = std::min(alpha_t, -t / dt);
 
    alpha = std::max(T(0), std::min(T(1), alpha));
 
    return eta * std::min(alpha, alpha_t);
  }
 
  constexpr T alpha_max_dual_impl(const Slack& y,
                                  const linalg::Vector<T, M>& p) const {
    return alpha_max_impl(y, p);
  }
 
  constexpr auto H_impl(const Slack& x, const Slack& y) const {
    const auto [w, event] = w_impl(x, y);
    if (linalg::is_fatal_event(event))
      return std::tuple(
          linalg::QuadraticRep<HessianMat, SOCCirc<Slack>>(e_impl()), event);
    const auto [w_inv, inv_event] = inv_impl(w);
    if (linalg::is_fatal_event(inv_event))
      return std::tuple(
          linalg::QuadraticRep<HessianMat, SOCCirc<Slack>>(e_impl()),
          inv_event);
    const auto out_event = (event != linalg::NumericalEvent::NONE ||
                            inv_event != linalg::NumericalEvent::NONE)
                               ? linalg::NumericalEvent::NUMERICAL_SAFEGUARD
                               : linalg::NumericalEvent::NONE;
    return std::tuple(linalg::QuadraticRep<HessianMat, SOCCirc<Slack>>(w_inv),
                      out_event);
  }
 
  constexpr auto grad_impl(const Slack& x) const {
    const auto [x_inv, event] = inv_impl(x);
    return std::tuple(T(-1) * x_inv, event);
  }
 
  constexpr T nu_impl() const { return T(2); }
 
  constexpr auto e_impl() const {
    Slack e{};
    e(M - 1) = T(1);
    return e;
  }
 
  constexpr auto w_impl(const Slack& x, const Slack& y) const {
    const auto x_sqrt = sqrt(x);
    linalg::QuadraticRep<HessianMat, SOCCirc<Slack>> P(x_sqrt);
 
    const auto b = P.apply(y);
    const auto [inner, event] = inv_impl(sqrt(b));
    if (linalg::is_fatal_event(event)) return std::tuple(Slack{}, event);
 
    return std::tuple(P.apply(inner), event);
  }
 
  constexpr auto circ_impl(const Slack& x, const Slack& y) const {
    return SOCCirc<Slack>()(x, y);
  }
 
  constexpr T trace_impl(const Slack& x, const Slack& y) const {
    return nu_impl() * circ_impl(x, y)(M - 1);
  }
 
  constexpr auto gamma_F_impl(const Slack& x, const Slack& y) const {
    const auto lambdas_x = eigs(x);
    const auto lambdas_y = eigs(y);
 
    const auto det_x = std::get<0>(lambdas_x) * std::get<1>(lambdas_x);
    const auto det_y = std::get<0>(lambdas_y) * std::get<1>(lambdas_y);
 
    const auto tr_xy = trace_impl(x, y);
 
    return std::tuple(nu_impl() * linalg::constexpr_ln_safe(tr_xy / nu_impl()) -
                          linalg::constexpr_ln_safe(det_x) -
                          linalg::constexpr_ln_safe(det_y),
                      linalg::NumericalEvent::NONE);
  }
 
  constexpr auto inv_impl(const Slack& x) const {
    const auto v = std::get<0>(linalg::splice<M - 1>(x));
    const auto t = x(M - 1);
    Slack adj_v;
    for (std::size_t i = 0; i < M - 1; ++i) adj_v(i) = T(-1) * v(i);
    adj_v(M - 1) = t;
 
    const auto det = t * t - linalg::inner_product(v, v);
    if (std::isnan(det) || linalg::constexpr_abs(det) <= eps)
      return std::tuple((T(1) / eps) * adj_v,
                        linalg::NumericalEvent::NUMERICAL_SAFEGUARD);
 
    const auto res = (T(1) / det) * adj_v;
    return std::tuple(res, linalg::NumericalEvent::NONE);
  }
 
  constexpr auto sqrt_impl(const Slack& x) const {
    return std::tuple(sqrt(x), linalg::NumericalEvent::NONE);
  }
 
  constexpr auto sqrt(const Slack& x) const {
    const auto v = std::get<0>(linalg::splice<M - 1>(x));
    const auto v_norm = linalg::L2<decltype(v)>()(v);
 
    const auto lambdas = eigs(x);
    const auto lam1 = std::get<0>(lambdas);
    const auto lam2 = std::get<1>(lambdas);
 
    const auto alpha = T(0.5) * (linalg::constexpr_sqrt_safe(lam1) +
                                 linalg::constexpr_sqrt_safe(lam2));
    const auto beta = T(0.5) * (linalg::constexpr_sqrt_safe(lam1) -
                                linalg::constexpr_sqrt_safe(lam2));
 
    Slack v_zero{};
    for (std::size_t i = 0; i < M - 1; ++i) v_zero(i) = v(i);
 
    auto x_sqrt = alpha * e_impl();
    if (v_norm > eps) x_sqrt = x_sqrt + beta * (T(1) / v_norm) * v_zero;
 
    return x_sqrt;
  }
 
  constexpr auto eigs(const Slack& x) const {
    const auto v = std::get<0>(linalg::splice<M - 1>(x));
    const auto t = x(M - 1);
 
    const auto v_norm = linalg::L2<decltype(v)>()(v);
    const auto lam1 = t + v_norm;
    const auto lam2 = t - v_norm;
 
    return std::tuple(lam1, lam2);
  }
};
 
} // namespace conic