From ca8545c7577d90a29aa73ce9ed62ae029ee1ff5f Mon Sep 17 00:00:00 2001 From: kekeliu-whu Date: Sat, 22 Jul 2023 22:30:58 +0800 Subject: [PATCH] feat: add CubicBSplineApprox --- src/common/bspline.h | 17 +++++++++++++++++ src/common/bspline_test.cc | 26 ++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) create mode 100644 src/common/bspline.h create mode 100644 src/common/bspline_test.cc diff --git a/src/common/bspline.h b/src/common/bspline.h new file mode 100644 index 0000000..49d934a --- /dev/null +++ b/src/common/bspline.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +template +Eigen::Matrix CubicBSplineApprox( + const Eigen::Matrix &p_1, + const Eigen::Matrix &p0, + const Eigen::Matrix &p1, + const Eigen::Matrix &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; +} diff --git a/src/common/bspline_test.cc b/src/common/bspline_test.cc new file mode 100644 index 0000000..a67cc84 --- /dev/null +++ b/src/common/bspline_test.cc @@ -0,0 +1,26 @@ +#include + +#include "common/bspline.h" + +using Vector1d = Eigen::Matrix; + +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; + // } +}