From 067753d4a923cbe74aebae251833e0d86a8f0903 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 21 Dec 2023 13:48:20 -0800
Subject: [PATCH] Only analyze sparsity pattern of left-hand side once

This gives a 30% speedup on average iteration times for the cart-pole
problem. The flywheel problem was unaffected.
---
 src/optimization/RegularizedLDLT.hpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/optimization/RegularizedLDLT.hpp b/src/optimization/RegularizedLDLT.hpp
index f310925f..8276cdcc 100644
--- a/src/optimization/RegularizedLDLT.hpp
+++ b/src/optimization/RegularizedLDLT.hpp
@@ -54,7 +54,11 @@ class RegularizedLDLT {
     double δ = 0.0;
     double γ = 0.0;
 
-    m_solver.compute(lhs);
+    if (!m_analyzedPattern) {
+      m_solver.analyzePattern(lhs);
+      m_analyzedPattern = true;
+    }
+    m_solver.factorize(lhs);
     Inertia inertia{m_solver};
 
     // If the decomposition succeeded and the inertia is ideal, don't regularize
@@ -71,7 +75,7 @@ class RegularizedLDLT {
         m_solver.info() != Eigen::Success) {
       γ = 1e-8 * std::pow(μ, 0.25);
 
-      m_solver.compute(lhs + Regularization(δ, γ));
+      m_solver.factorize(lhs + Regularization(δ, γ));
       inertia = Inertia{m_solver};
 
       if (m_solver.info() == Eigen::Success && inertia == idealInertia) {
@@ -95,7 +99,7 @@ class RegularizedLDLT {
       //
       // lhs = [H + AᵢᵀΣAᵢ + δI   Aₑᵀ]
       //       [       Aₑ        −γI ]
-      m_solver.compute(lhs + Regularization(δ, γ));
+      m_solver.factorize(lhs + Regularization(δ, γ));
       Inertia inertia{m_solver};
 
       // If the inertia is ideal, store that value of δ and return.
@@ -130,6 +134,7 @@ class RegularizedLDLT {
 
  private:
   Solver m_solver;
+  bool m_analyzedPattern = false;
 
   Eigen::ComputationInfo m_info = Eigen::Success;