157 lines
4.3 KiB
C++
157 lines
4.3 KiB
C++
#pragma once
|
|
|
|
|
|
// STL includes
|
|
#include <expected>
|
|
|
|
// Library includes
|
|
#include <Eigen/Dense>
|
|
#include <spdlog/spdlog.h>
|
|
|
|
// Project includes
|
|
#include "util.hpp"
|
|
|
|
|
|
namespace hccd {
|
|
|
|
|
|
namespace detail {
|
|
template <typename T>
|
|
concept homotopy_c = requires(T) {
|
|
{ T::evaluate_H(Eigen::VectorXd()) } -> std::same_as<Eigen::VectorXd>;
|
|
{ T::evaluate_DH(Eigen::VectorXd()) } -> std::same_as<Eigen::MatrixXd>;
|
|
};
|
|
} // namespace detail
|
|
|
|
|
|
///
|
|
/// @brief Settings for PathTracker
|
|
///
|
|
struct Settings {
|
|
double euler_step_size = 0.05;
|
|
unsigned euler_max_tries = 5;
|
|
unsigned newton_max_iter = 5;
|
|
double newton_convergence_threshold = 0.01;
|
|
int sigma = 1; ///< Direction in which the path is traced
|
|
};
|
|
|
|
///
|
|
/// @brief Path tracker for the homotopy continuation method
|
|
/// @details Uses a predictor-corrector scheme to trace a path defined by a
|
|
/// homotopy.
|
|
/// @details References:
|
|
/// [1] T. Chen and T.-Y. Li, “Homotopy continuation method for solving
|
|
/// systems of nonlinear and polynomial equations,” Communications in
|
|
/// Information and Systems, vol. 15, no. 2, pp. 119-307, 2015
|
|
///
|
|
/// @tparam homotopy_c Homotopy defining the path
|
|
///
|
|
template <detail::homotopy_c Homotopy>
|
|
class PathTracker {
|
|
public:
|
|
enum Error { NewtonNotConverged };
|
|
|
|
PathTracker(Settings settings) : m_settings{settings} {
|
|
}
|
|
|
|
///
|
|
/// @brief Perform one predictor-corrector step
|
|
///
|
|
Eigen::VectorXd step(Eigen::VectorXd y) {
|
|
auto res = transparent_step(y);
|
|
|
|
if (res) {
|
|
return res.value().first;
|
|
} else {
|
|
return std::unexpected(res.error());
|
|
}
|
|
}
|
|
|
|
///
|
|
/// @brief Perform one predictor-corrector step, returning intermediate
|
|
/// results
|
|
///
|
|
std::expected<std::tuple<Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd,
|
|
Eigen::VectorXd>,
|
|
Error>
|
|
transparent_step(Eigen::VectorXd y) {
|
|
for (int i = 0; i < m_settings.euler_max_tries; ++i) {
|
|
double step_size = m_settings.euler_step_size / (1 << i);
|
|
|
|
const auto [y_hat, y_prime] =
|
|
perform_euler_predictor_step(y, step_size);
|
|
|
|
auto res = perform_newton_corrector_step(y_hat);
|
|
|
|
if (res) return {{y, y_prime, y_hat, res.value()}};
|
|
}
|
|
|
|
return std::unexpected(Error::NewtonNotConverged);
|
|
}
|
|
|
|
private:
|
|
Settings m_settings;
|
|
|
|
std::pair<Eigen::VectorXd, Eigen::VectorXd>
|
|
perform_euler_predictor_step(Eigen::VectorXd y, double step_size) {
|
|
/// Obtain y_prime
|
|
|
|
Eigen::MatrixXd DH = Homotopy::evaluate_DH(y);
|
|
auto qr = DH.transpose().colPivHouseholderQr();
|
|
Eigen::MatrixXd Q = qr.matrixQ();
|
|
Eigen::MatrixXd R = qr.matrixR();
|
|
|
|
Eigen::VectorXd y_prime = Q.col(2);
|
|
|
|
spdlog::debug("Q: \t\t{}x{}; det={}", Q.rows(), Q.cols(),
|
|
Q.determinant());
|
|
spdlog::debug("R.topRows(2): {}x{}; det={}", R.topRows(2).rows(),
|
|
R.topRows(2).cols(), R.topRows(2).determinant());
|
|
|
|
if (sign(Q.determinant() * R.topRows(2).determinant()) !=
|
|
sign(m_settings.sigma))
|
|
y_prime = -y_prime;
|
|
|
|
/// Perform prediction
|
|
|
|
Eigen::VectorXd y_hat = y + step_size * y_prime;
|
|
|
|
return {y_hat, y_prime};
|
|
}
|
|
|
|
std::expected<Eigen::VectorXd, Error>
|
|
perform_newton_corrector_step(Eigen::VectorXd y) {
|
|
Eigen::VectorXd prev_y = y;
|
|
|
|
for (int i = 0; i < m_settings.newton_max_iter; ++i) {
|
|
|
|
/// Perform correction
|
|
|
|
Eigen::MatrixXd DH = Homotopy::evaluate_DH(y);
|
|
Eigen::MatrixXd DH_pinv =
|
|
DH.completeOrthogonalDecomposition().pseudoInverse();
|
|
|
|
y = y - DH_pinv * Homotopy::evaluate_H(y);
|
|
|
|
/// Check stopping criterion
|
|
|
|
spdlog::debug("Newton iteration {}: ||y-prev_y||={}", i,
|
|
(y - prev_y).norm());
|
|
if ((y - prev_y).norm() < m_settings.newton_convergence_threshold)
|
|
return y;
|
|
|
|
prev_y = y;
|
|
}
|
|
|
|
return std::unexpected(Error::NewtonNotConverged);
|
|
}
|
|
|
|
template <typename T>
|
|
static int sign(T val) {
|
|
return -1 * (val < T(0)) + 1 * (val >= T(0));
|
|
}
|
|
};
|
|
|
|
|
|
} // namespace hccd
|