feat: add CubicBSplineApprox

src/common/bspline.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <Eigen/Eigen>
+#include <iostream>
+
+template <int N>
+Eigen::Matrix<double, N, 1> CubicBSplineApprox(
+ const Eigen::Matrix<double, N, 1> &p_1,
+ const Eigen::Matrix<double, N, 1> &p0,
+ const Eigen::Matrix<double, N, 1> &p1,
+ const Eigen::Matrix<double, N, 1> &p2,
+ double s) {
+ double s2 = s * s;
+ double s3 = s * s * s;
+
+ return (p_1 * std::pow(1 - s, 3) + p0 * (3 * s3 - 6 * s2 + 4) + p1 * (-3 * s3 + 3 * s2 + 3 * s + 1) + p2 * s3) / 6;
+}

src/common/bspline_test.cc

@@ -0,0 +1,26 @@
+#include <gtest/gtest.h>
+
+#include "common/bspline.h"
+
+using Vector1d = Eigen::Matrix<double, 1, 1>;
+
+TEST(BSplineTest, ArithmeticProgression) {
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{1}, Vector1d{2}, Vector1d{3}, Vector1d{4}, 0)[0], 2);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{1}, Vector1d{2}, Vector1d{3}, Vector1d{4}, 1)[0], 3);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{1}, Vector1d{2}, Vector1d{3}, Vector1d{4}, 0.4)[0], 2.4);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{1}, Vector1d{2}, Vector1d{3}, Vector1d{4}, 0.5)[0], 2.5);
+}
+
+TEST(BSplineTest, Const) {
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{2}, Vector1d{2}, Vector1d{2}, Vector1d{2}, 0)[0], 2);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{2}, Vector1d{2}, Vector1d{2}, Vector1d{2}, 1)[0], 2);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{2}, Vector1d{2}, Vector1d{2}, Vector1d{2}, 0.5)[0], 2);
+ EXPECT_DOUBLE_EQ(CubicBSplineApprox(Vector1d{2}, Vector1d{2}, Vector1d{2}, Vector1d{2}, 0.4)[0], 2);
+}
+
+TEST(BSplineTest, 1) {
+ // for (double d = 0; d <= 1.01; d += 0.02) {
+ // auto ret = CubicBSpline(Vector1d{4}, Vector1d{3}, Vector1d{2}, Vector1d{1}, d);
+ // std::cout << d << " " << ret << std::endl;
+ // }
+}