diff --git a/src/analysis/PolynomialP.hpp b/src/analysis/PolynomialP.hpp
index 8b8bdb512bb6ad879594bf9eabec484f67abb147..42f0c46663019d7f4e65220c8e25fbd6a89c94e6 100644
--- a/src/analysis/PolynomialP.hpp
+++ b/src/analysis/PolynomialP.hpp
@@ -173,8 +173,8 @@ class PolynomialP
static_assert(Dimension == 3);
size_t total_degree = relative_pos[0] + relative_pos[1] + relative_pos[2];
size_t total_sub_degree = relative_pos[1] + relative_pos[2];
- return total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
- total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
+ absolute_position = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
+ total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
}
return absolute_coefs[absolute_position];
@@ -267,9 +267,28 @@ class PolynomialP
value += valuex;
}
} else {
- throw NotImplementedError("Not yet Available in 3D");
+ TinyVector<Dimension, size_t> relative_pos(0, 0, N);
+ value = P[relative_pos];
+ for (size_t i = N; i > 0; --i) {
+ value *= x[2];
+ relative_pos = TinyVector<Dimension, size_t>(0, N - i + 1, i - 1);
+ double valuey = P[relative_pos];
+ for (size_t j = N - i + 1; j > 0; --j) {
+ valuey *= x[1];
+ relative_pos = TinyVector<Dimension, size_t>(N - i - j + 2, j - 1, i - 1);
+ double valuex = P[relative_pos];
+ for (size_t k = N - i - j + 2; k > 0; --k) {
+ valuex *= x[0];
+ relative_pos = TinyVector<Dimension, size_t>(k - 1, j - 1, i - 1);
+
+ valuex += P[relative_pos];
+ }
+ valuey += valuex;
+ }
+ value += valuey;
+ // throw NotImplementedError("Not yet Available in 3D");
+ }
}
-
return value;
}
diff --git a/src/analysis/TaylorPolynomial.hpp b/src/analysis/TaylorPolynomial.hpp
index ff417f1430ffd80e6b9395bc6dc8a64023720702..d3f03154f1f621328ba3821ce6bb7d23794b5a75 100644
--- a/src/analysis/TaylorPolynomial.hpp
+++ b/src/analysis/TaylorPolynomial.hpp
@@ -191,8 +191,8 @@ class TaylorPolynomial
static_assert(Dimension == 3);
size_t total_degree = relative_pos[0] + relative_pos[1] + relative_pos[2];
size_t total_sub_degree = relative_pos[1] + relative_pos[2];
- return total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
- total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
+ absolute_position = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
+ total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
}
return absolute_coefs[absolute_position];
@@ -285,7 +285,27 @@ class TaylorPolynomial
value += valuex;
}
} else {
- throw NotImplementedError("Not yet Available in 3D");
+ TinyVector<Dimension, size_t> relative_pos(0, 0, N);
+ value = P[relative_pos];
+ for (size_t i = N; i > 0; --i) {
+ value *= (x[2] - m_x0[2]);
+ relative_pos = TinyVector<Dimension, size_t>(0, N - i + 1, i - 1);
+ double valuey = P[relative_pos];
+ for (size_t j = N - i + 1; j > 0; --j) {
+ valuey *= (x[1] - m_x0[1]);
+ relative_pos = TinyVector<Dimension, size_t>(N - i - j + 2, j - 1, i - 1);
+ double valuex = P[relative_pos];
+ for (size_t k = N - i - j + 2; k > 0; --k) {
+ valuex *= (x[0] - m_x0[0]);
+ relative_pos = TinyVector<Dimension, size_t>(k - 1, j - 1, i - 1);
+
+ valuex += P[relative_pos];
+ }
+ valuey += valuex;
+ }
+ value += valuey;
+ // throw NotImplementedError("Not yet Available in 3D");
+ }
}
return value;
diff --git a/tests/test_PolynomialP.cpp b/tests/test_PolynomialP.cpp
index a6f2cef9fabd65c3c3952e8707ec4b3bb9622721..7b46193c38434c2400a34fa0248f143461a2adac 100644
--- a/tests/test_PolynomialP.cpp
+++ b/tests/test_PolynomialP.cpp
@@ -118,19 +118,32 @@ TEST_CASE("PolynomialP", "[analysis]")
{
TinyVector<6> coef(1, -2, 10, 7, 2, 9);
TinyVector<10> coef2(2, -4, -1, -3, 3, -5, -6, 0, 1, 7);
+ TinyVector<10> coef3(2, -4, -1, -3, 3, -5, -6, 0, 1, 7);
PolynomialP<2, 2> P(coef);
PolynomialP<3, 2> Q(coef2);
+ PolynomialP<2, 3> R(coef3);
TinyVector<6> coefx(1, -2, 0, 7, 0, 0);
TinyVector<6> coefy(2, 0, -2, 0, 0, 7);
TinyVector<2> pos(1, -1);
TinyVector<2> pos2(-1, 2);
+ TinyVector<3> pos3(-1, 2, 1);
PolynomialP<2, 2> Px(coefx);
PolynomialP<2, 2> Py(coefy);
+ TinyVector<10> coef3x(2, -4, 0, 0, 3, 0, 0, 0, 0, 0);
+ TinyVector<10> coef3y(2, 0, -1, 0, 0, 0, 0, 1, 0, 0);
+ TinyVector<10> coef3z(2, 0, 0, -3, 0, 0, 0, 0, 0, 7);
+ PolynomialP<2, 3> Rx(coef3x);
+ PolynomialP<2, 3> Ry(coef3y);
+ PolynomialP<2, 3> Rz(coef3z);
REQUIRE(Px(pos) == 6);
REQUIRE(Py(pos) == 11);
REQUIRE(Px(pos2) == 10);
REQUIRE(P(pos2) == 62);
REQUIRE(Q(pos) == -24);
+ REQUIRE(Rx(pos3) == 9);
+ REQUIRE(Ry(pos3) == 4);
+ REQUIRE(Rz(pos3) == 6);
+ REQUIRE(R(pos3) == 29);
}
SECTION("derivation")
diff --git a/tests/test_TaylorPolynomial.cpp b/tests/test_TaylorPolynomial.cpp
index 41ba9d4b4cfaf53ee92053a07663ac2048092d2e..b5871d246d4841c50e55f4618f93d731c7fe1ea4 100644
--- a/tests/test_TaylorPolynomial.cpp
+++ b/tests/test_TaylorPolynomial.cpp
@@ -119,12 +119,16 @@ TEST_CASE("TaylorPolynomial", "[analysis]")
{
TinyVector<6> coef(1, -2, 10, 7, 2, 9);
TinyVector<10> coef2(2, -4, -1, -3, 3, -5, -6, 0, 1, 7);
+ TinyVector<10> coef3(2, -4, -1, -3, 3, -5, -6, 0, 1, 7);
TaylorPolynomial<2, 2> P(coef, x0);
TaylorPolynomial<3, 2> Q(coef2, x0);
+ TaylorPolynomial<2, 3> R(coef3, x1);
TinyVector<6> coefx(1, -2, 0, 7, 0, 0);
TinyVector<6> coefy(2, 0, -2, 0, 0, 7);
TinyVector<2> pos(1, -1);
TinyVector<2> pos2(-1, 2);
+ TinyVector<3> pos3(-1, 2, 1);
+
TaylorPolynomial<2, 2> Px(coefx, x0);
TaylorPolynomial<2, 2> Py(coefy, x0);
REQUIRE(Px(pos) == 1);
@@ -132,6 +136,7 @@ TEST_CASE("TaylorPolynomial", "[analysis]")
REQUIRE(Px(pos2) == 33);
REQUIRE(P(pos2) == 132);
REQUIRE(Q(pos) == 2);
+ REQUIRE(R(pos3) == 49);
}
SECTION("derivation")