From 159b14e129760b892d4436774dee7e55cc8af1c4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Apr 2014 20:26:51 +0400 Subject: [PATCH] Array::packSquareMatrix(), Array::unpackSquareMatrix() --- libuavcan/include/uavcan/marshal/array.hpp | 169 +++++++++++++ .../include/uavcan/util/compile_time.hpp | 13 + libuavcan/test/marshal/array.cpp | 237 ++++++++++++++++++ 3 files changed, 419 insertions(+) diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index 2be0d39a68..1dd3ee5b3d 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -336,6 +337,108 @@ class Array : public ArrayImpl return -ErrLogic; } + template + void packSquareMatrixImpl(const InputIter src_row_major) + { + StaticAssert::check(); + enum { Width = CompileTimeIntSqrt::Result }; + + bool all_nans = true; + bool scalar_matrix = true; + bool diagonal_matrix = true; + /* + * Detecting how the matrix can be compressed: + * - Matrix that consists only of NANs will be eliminated completely; + * - Scalar matrix will be reduced to one value; + * - Diagonal matrix will be reduced to array of length Width. + */ + { + unsigned index = 0; + for (InputIter it = src_row_major; index < MaxSize; ++it, ++index) + { + const bool on_diagonal = (index / Width) == (index % Width); +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + const bool nan = std::isnan(*it); +#else + const bool nan = (*it) != (*it); +#endif + if (!nan) + { + all_nans = false; + } + if (!on_diagonal && (*it) != 0) // TODO: Proper float comparison + { + scalar_matrix = false; // This matrix cannot be compressed. + diagonal_matrix = false; + break; + } + if (on_diagonal && (*it) != (*src_row_major)) // TODO: Proper float comparison + { + scalar_matrix = false; + } + } + } + /* + * Actual packing is performed here. + */ + this->clear(); + if (!all_nans) + { + unsigned index = 0; + for (InputIter it = src_row_major; index < MaxSize; ++it, ++index) + { + const bool on_diagonal = (index / Width) == (index % Width); + if (diagonal_matrix && !on_diagonal) + { + continue; + } + this->push_back(ValueType(*it)); + if (scalar_matrix) + { + break; + } + } + } + } + + template + void unpackSquareMatrixImpl(OutputIter it) const + { + StaticAssert::check(); + enum { Width = CompileTimeIntSqrt::Result }; + /* + * Unpacking as follows: + * - Array of length 1 will be unpacked to scalar matrix + * - Array of length Width will be unpacked to diagonal matrix + * - Array of length MaxSize will be unpacked to full matrix + * - All other length values will yield zero matrix + */ + if (this->size() == Width || this->size() == 1) + { + for (unsigned index = 0; index < MaxSize; index++) + { + const bool on_diagonal = (index / Width) == (index % Width); + if (on_diagonal) + { + const SizeType source_index = (this->size() == 1) ? 0 : (index / Width); + *it++ = at(source_index); + } + else + { + *it++ = 0; + } + } + } + else if (this->size() == MaxSize) + { + std::copy(this->begin(), this->end(), it); + } + else + { + std::fill_n(it, MaxSize, 0); + } + } + public: typedef T RawValueType; typedef typename StorageType::Type ValueType; @@ -517,6 +620,72 @@ public: } } + /** + * Fills this array as a packed square matrix from a static array. + */ + template + void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize]) + { + packSquareMatrixImpl(src_row_major); + } + + /** + * Fills this array as a packed square matrix from any container that implements the methods begin() and size(). + */ + template + typename EnableIfbegin()) && sizeof(((const R*)(0U))->size())>::Type + packSquareMatrix(const R& src_row_major) + { + if (src_row_major.size() == MaxSize) + { + packSquareMatrixImpl(src_row_major.begin()); + } + else if (src_row_major.size() == 0) + { + this->clear(); + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::packSquareMatrix()"); +#else + assert(0); + this->clear(); +#endif + } + } + + /** + * Reconstructs full matrix, result will be saved into a static array. + */ + template + void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const + { + unpackSquareMatrixImpl(dst_row_major); + } + + /** + * Reconstructs full matrix, result will be saved into container that implements begin() and size(). + */ + template + typename EnableIfbegin()) && sizeof(((const R*)(0U))->size())>::Type + unpackSquareMatrix(R& dst_row_major) const + { + if (dst_row_major.size() == MaxSize) + { + unpackSquareMatrixImpl(dst_row_major.begin()); + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::unpackSquareMatrix()"); +#else + assert(0); +#endif + } + } + + typedef ValueType value_type; typedef SizeType size_type; }; diff --git a/libuavcan/include/uavcan/util/compile_time.hpp b/libuavcan/include/uavcan/util/compile_time.hpp index 1cbcb19325..52c4a429b4 100644 --- a/libuavcan/include/uavcan/util/compile_time.hpp +++ b/libuavcan/include/uavcan/util/compile_time.hpp @@ -118,4 +118,17 @@ To try_implicit_cast(const From& from) BooleanType::Result>()); } +/** + * Some arithmetics + */ +template struct CompileTimeIntSqrt; +template <> struct CompileTimeIntSqrt<4> { enum { Result = 2 }; }; +template <> struct CompileTimeIntSqrt<9> { enum { Result = 3 }; }; +template <> struct CompileTimeIntSqrt<16> { enum { Result = 4 }; }; +template <> struct CompileTimeIntSqrt<25> { enum { Result = 5 }; }; +template <> struct CompileTimeIntSqrt<36> { enum { Result = 6 }; }; +template <> struct CompileTimeIntSqrt<49> { enum { Result = 7 }; }; +template <> struct CompileTimeIntSqrt<64> { enum { Result = 8 }; }; +template <> struct CompileTimeIntSqrt<81> { enum { Result = 9 }; }; + } diff --git a/libuavcan/test/marshal/array.cpp b/libuavcan/test/marshal/array.cpp index be72a73b8e..dc7650b313 100644 --- a/libuavcan/test/marshal/array.cpp +++ b/libuavcan/test/marshal/array.cpp @@ -832,3 +832,240 @@ TEST(Array, MultidimensionalStreaming) uavcan::YamlStreamer::stream(std::cout, threedee, 0); std::cout << std::endl; } + + +TEST(Array, SquareMatrixPacking) +{ + Array, ArrayModeDynamic, 9> m3x3s; + Array, ArrayModeDynamic, 4> m2x2f; + Array, ArrayModeDynamic, 36> m6x6d; + + // NAN will be reduced to empty array + { + const double nans3x3[] = + { + NAN, NAN, NAN, + NAN, NAN, NAN, + NAN, NAN, NAN + }; + m3x3s.packSquareMatrix(nans3x3); + ASSERT_EQ(0, m3x3s.size()); + + // Empty array will be decoded as zero matrix + double nans3x3_out[9]; + m3x3s.unpackSquareMatrix(nans3x3_out); + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(0, nans3x3_out[i]); + } + } + { + std::vector empty; + m3x3s.packSquareMatrix(empty); + ASSERT_EQ(0, m3x3s.size()); + + empty.resize(9); + m3x3s.unpackSquareMatrix(empty); + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(0, empty.at(i)); + } + } + + // Scalar matrix will be reduced to a single value + { + std::vector scalar2x2(4); + scalar2x2[0] = scalar2x2[3] = 3.14; + m2x2f.packSquareMatrix(scalar2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(3.14, m2x2f[0]); + + m2x2f.unpackSquareMatrix(scalar2x2); + const float reference[] = + { + 3.14, 0, + 0, 3.14 + }; + ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference)); + } + { + const float scalar6x6[] = + { + -18, 0, 0, 0, 0, 0, + 0, -18, 0, 0, 0, 0, + 0, 0, -18, 0, 0, 0, + 0, 0, 0, -18, 0, 0, + 0, 0, 0, 0, -18, 0, + 0, 0, 0, 0, 0, -18 + }; + m6x6d.packSquareMatrix(scalar6x6); + ASSERT_EQ(1, m6x6d.size()); + ASSERT_FLOAT_EQ(-18, m6x6d[0]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6)); + } + + // Diagonal matrix will be reduced to an array of length Width + { + const float diagonal6x6[] = + { + 1, 0, 0, 0, 0, 0, + 0, -2, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, + 0, 0, 0, -4, 0, 0, + 0, 0, 0, 0, 5, 0, + 0, 0, 0, 0, 0, -6 + }; + m6x6d.packSquareMatrix(diagonal6x6); + ASSERT_EQ(6, m6x6d.size()); + ASSERT_FLOAT_EQ(1, m6x6d[0]); + ASSERT_FLOAT_EQ(-2, m6x6d[1]); + ASSERT_FLOAT_EQ(3, m6x6d[2]); + ASSERT_FLOAT_EQ(-4, m6x6d[3]); + ASSERT_FLOAT_EQ(5, m6x6d[4]); + ASSERT_FLOAT_EQ(-6, m6x6d[5]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6)); + } + + // A matrix filled with random values will not be compressed + { + std::vector full3x3(9); + for (int i = 0; i < 9; i++) + { + full3x3[i] = i; + } + m3x3s.packSquareMatrix(full3x3); + ASSERT_EQ(9, m3x3s.size()); + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(i, m3x3s[i]); + } + + long output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output)); + } + + // This will be represented as diagonal - NANs are exceptional + { + const double scalarnan3x3[] = + { + NAN, 0, 0, + 0, NAN, 0, + 0, 0, NAN + }; + m3x3s.packSquareMatrix(scalarnan3x3); + ASSERT_EQ(3, m3x3s.size()); + ASSERT_FALSE(m3x3s[0] == m3x3s[0]); // NAN + ASSERT_FALSE(m3x3s[1] == m3x3s[1]); // NAN + ASSERT_FALSE(m3x3s[2] == m3x3s[2]); // NAN + + float output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_FALSE(output[0] == output[0]); // NAN + ASSERT_EQ(0, output[1]); + ASSERT_EQ(0, output[2]); + ASSERT_EQ(0, output[3]); + ASSERT_FALSE(output[4] == output[4]); // NAN + ASSERT_EQ(0, output[5]); + ASSERT_EQ(0, output[6]); + ASSERT_EQ(0, output[7]); + ASSERT_FALSE(output[8] == output[8]); // NAN + } + + // This is a full matrix too (notice the NAN) + { + const float full2x2[] = + { + 1, NAN, + 0, -2 + }; + m2x2f.packSquareMatrix(full2x2); + ASSERT_EQ(4, m2x2f.size()); + ASSERT_FLOAT_EQ(1, m2x2f[0]); + ASSERT_FALSE(m2x2f[1] == m2x2f[1]); // NAN + ASSERT_FLOAT_EQ(0, m2x2f[2]); + ASSERT_FLOAT_EQ(-2, m2x2f[3]); + + float output[4]; + m2x2f.unpackSquareMatrix(output); + ASSERT_EQ(1, output[0]); + ASSERT_FALSE(output[1] == output[1]); // NAN + ASSERT_EQ(0, output[2]); + ASSERT_EQ(-2, output[3]); + } + + // Zero matrix will be represented as scalar matrix + { + const float zero2x2[] = + { + 0, 0, + 0, 0 + }; + m2x2f.packSquareMatrix(zero2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(0, m2x2f[0]); + } +} + + +TEST(Array, SquareMatrixPackingIntegers) +{ + Array, ArrayModeDynamic, 9> m3x3int; + { + const long scalar[] = + { + 42, 0, 0, + 0, 42, 0, + 0, 0, 42 + }; + m3x3int.packSquareMatrix(scalar); + ASSERT_EQ(1, m3x3int.size()); + ASSERT_EQ(42, m3x3int[0]); + + std::vector output(9); + m3x3int.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar)); + } + { + std::vector diagonal(9); + diagonal[0] = 6; + diagonal[4] = -57; + diagonal[8] = 1139; + m3x3int.packSquareMatrix(diagonal); + ASSERT_EQ(3, m3x3int.size()); + ASSERT_EQ(6, m3x3int[0]); + ASSERT_EQ(-57, m3x3int[1]); + ASSERT_EQ(1139, m3x3int[2]); + } + { + std::vector full(9); + for (int i = 0; i < 9; i++) + { + full[i] = i; + } + m3x3int.packSquareMatrix(full); + ASSERT_EQ(9, m3x3int.size()); + for (int i = 0; i < 9; i++) + { + ASSERT_EQ(i, m3x3int[i]); + } + } +} + +#if UAVCAN_EXCEPTIONS +TEST(Array, SquareMatrixPackingErrors) +{ + Array, ArrayModeDynamic, 9> m3x3s; + + std::vector ill_formed_row_major(8); + ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range); + + ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range); +} +#endif