From 392abd6eab436dcb9971f9cd1acf65348a09f352 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Mar 2025 17:29:00 -0400
Subject: [PATCH] Make Clenshaw-Curtis weights faster in case [-1,1]

---
 gtsam/basis/Chebyshev2.cpp           | 22 +++++++++++++---------
 gtsam/basis/Chebyshev2.h             | 19 ++++---------------
 gtsam/basis/tests/testChebyshev2.cpp |  4 ++--
 3 files changed, 19 insertions(+), 26 deletions(-)

diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp
index 015a1ec47..fae26eedc 100644
--- a/gtsam/basis/Chebyshev2.cpp
+++ b/gtsam/basis/Chebyshev2.cpp
@@ -227,28 +227,32 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, dou
   return D / ((b - a) / 2.0);
 }
 
-Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+Weights Chebyshev2::IntegrationWeights(size_t N) {
   Weights weights(N);
-  size_t K = N - 1,  // number of intervals between N points
+  const size_t K = N - 1,  // number of intervals between N points
     K2 = K * K;
 
   // Compute endpoint weight.
-  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
+  weights(0) = 1.0 / (K2 + K % 2 - 1);
   weights(N - 1) = weights(0);
 
   // Compute up to the middle; mirror symmetry holds.
-  size_t mid = (N - 1) / 2;
+  const size_t mid = (N - 1) / 2;
+  double dTheta = M_PI / K;
   for (size_t i = 1; i <= mid; ++i) {
-    double theta = i * M_PI / K;
-    double w = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
-    size_t last_k = K / 2 + K % 2 - 1;
+    double w = (K % 2 == 0) ? 1 - cos(i * M_PI) / (K2 - 1) : 1;
+    const size_t last_k = K / 2 + K % 2 - 1;
+    const double theta = i * dTheta;
     for (size_t k = 1; k <= last_k; ++k)
-      w -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
-    w *= (b - a) / K;
+      w -= 2.0 * cos(2 * k * theta) / (4 * k * k - 1);
+    w *= 2.0 / K;
     weights(i) = w;
     weights(N - 1 - i) = w;
   }
   return weights;
 }
 
+Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+  return IntegrationWeights(N) * (b - a) / 2.0;
+}
 }  // namespace gtsam
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index a66de20d5..124e65b7c 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -105,22 +105,11 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   /**
    *  Evaluate Clenshaw-Curtis integration weights.
    *  Trefethen00book, pg 128, clencurt.m
-   *  Note that N in clencurt.m is 1 less than our N
-   *  K = N-1;
-      theta = pi*(0:K)'/K;
-      w = zeros(1,N); ii = 2:K; v = ones(K-1, 1);
-      if mod(K,2) == 0
-          w(1) = 1/(K^2-1); w(N) = w(1);
-          for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
-          v = v - cos(K*theta(ii))/(K^2-1);
-      else
-          w(1) = 1/K^2; w(N) = w(1);
-          for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
-      end
-      w(ii) = 2*v/K;
-
    */
-  static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
+  static Weights IntegrationWeights(size_t N);
+
+  /// Evaluate Clenshaw-Curtis integration weights, for interval [a,b]
+  static Weights IntegrationWeights(size_t N, double a, double b);
 
   /**
    * Create matrix of values at Chebyshev points given vector-valued function.
diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp
index 8e8d88239..1702d7a62 100644
--- a/gtsam/basis/tests/testChebyshev2.cpp
+++ b/gtsam/basis/tests/testChebyshev2.cpp
@@ -480,7 +480,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
 //******************************************************************************
 TEST(Chebyshev2, IntegralWeights) {
   const size_t N7 = 7;
-  Vector actual = Chebyshev2::IntegrationWeights(N7);
+  Vector actual = Chebyshev2::IntegrationWeights(N7, -1, 1);
   Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
                      0.457142857142857, 0.520634920634921, 0.457142857142857,
                      0.253968253968254, 0.0285714285714286)
@@ -488,7 +488,7 @@ TEST(Chebyshev2, IntegralWeights) {
   EXPECT(assert_equal(expected, actual));
 
   const size_t N8 = 8;
-  Vector actual2 = Chebyshev2::IntegrationWeights(N8);
+  Vector actual2 = Chebyshev2::IntegrationWeights(N8, -1, 1);
   Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
                       0.352242423718159, 0.437208405798326, 0.437208405798326,
                       0.352242423718159, 0.190141007218208, 0.0204081632653061)