From bfc4b5336c40ad69bbf081b0fa0af0325a9e7c1c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 27 Apr 2015 10:12:50 +0300 Subject: [PATCH] Symmetric square matrix packing --- libuavcan/include/uavcan/marshal/array.hpp | 271 ++++++++++++++++----- libuavcan/test/marshal/array.cpp | 110 +++++++++ 2 files changed, 319 insertions(+), 62 deletions(-) diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index 67d5ee41d0..94f3060bd8 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -34,6 +34,138 @@ namespace uavcan enum ArrayMode { ArrayModeStatic, ArrayModeDynamic }; +/** + * Properties of a square matrix; assuming row-major representation. + */ +template +struct SquareMatrixTraits +{ + enum { NumElements = NumElements_ }; + + enum { NumRowsCols = CompileTimeIntSqrt::Result }; + + enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 }; + + static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); } + + static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; } +}; + +/** + * This class can be used to detect properties of square matrices. + * Element iterator is a random access forward constant iterator. + */ +template +class SquareMatrixAnalyzer : public SquareMatrixTraits +{ + typedef SquareMatrixTraits Traits; + + const ElementIterator first_; + +public: + enum PackingMode + { + PackingModeEmpty, + PackingModeScalar, + PackingModeDiagonal, + PackingModeSymmetric, + PackingModeFull + }; + + SquareMatrixAnalyzer(ElementIterator first_element_iterator) + : first_(first_element_iterator) + { + StaticAssert<(NumElements > 0)>::check(); + } + + ElementIterator accessElementAtRowCol(int row, int col) const + { + return first_ + Traits::computeElementIndexAtRowCol(row, col); + } + + bool areAllElementsNan() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!isNaN(*it)) + { + return false; + } + } + return true; + } + + bool isScalar() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_)) + { + return false; + } + } + return true; + } + + bool isDiagonal() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + } + return true; + } + + bool isSymmetric() const + { + for (int i = 0; i < Traits::NumRowsCols; ++i) + { + for (int k = 0; k < Traits::NumRowsCols; ++k) + { + // On diagonal comparison is pointless + if ((i != k) && + !areClose(*accessElementAtRowCol(i, k), + *accessElementAtRowCol(k, i))) + { + return false; + } + } + } + return true; + } + + PackingMode detectOptimalPackingMode() const + { + if (areAllElementsNan()) + { + return PackingModeEmpty; + } + if (isScalar()) + { + return PackingModeScalar; + } + if (isDiagonal()) + { + return PackingModeDiagonal; + } + if (isSymmetric()) + { + return PackingModeSymmetric; + } + return PackingModeFull; + } +}; + template class UAVCAN_EXPORT StaticArrayBase @@ -400,82 +532,74 @@ class UAVCAN_EXPORT Array : public ArrayImpl void packSquareMatrixImpl(const InputIter src_row_major) { StaticAssert::check(); - const unsigned 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); - const bool nan = isNaN(*it); - if (!nan) - { - all_nans = false; - } - if (!on_diagonal && !isCloseToZero(*it)) - { - scalar_matrix = false; // This matrix cannot be compressed. - diagonal_matrix = false; - break; - } - if (on_diagonal && !areClose(*it, *src_row_major)) - { - scalar_matrix = false; - } - } - } - /* - * Actual packing is performed here. - */ this->clear(); - if (!all_nans) + + typedef SquareMatrixAnalyzer Analyzer; + const Analyzer analyzer(src_row_major); + + switch (analyzer.detectOptimalPackingMode()) { - unsigned index = 0; - for (InputIter it = src_row_major; index < MaxSize; ++it, ++index) + case Analyzer::PackingModeEmpty: + { + break; // Nothing to insert + } + case Analyzer::PackingModeScalar: + { + this->push_back(ValueType(*src_row_major)); + break; + } + case Analyzer::PackingModeDiagonal: + { + for (int i = 0; i < Analyzer::NumRowsCols; i++) { - const bool on_diagonal = (index / Width) == (index % Width); - if (diagonal_matrix && !on_diagonal) + this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i))); + } + break; + } + case Analyzer::PackingModeSymmetric: + { + for (int row = 0; row < Analyzer::NumRowsCols; row++) + { + for (int col = row; col < Analyzer::NumRowsCols; col++) { - continue; - } - this->push_back(ValueType(*it)); - if (scalar_matrix) - { - break; + this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col))); } } + UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle); + break; + } + case Analyzer::PackingModeFull: + { + InputIter it = src_row_major; + for (unsigned index = 0; index < MaxSize; index++, it++) + { + this->push_back(ValueType(*it)); + } + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } } } template - void unpackSquareMatrixImpl(OutputIter it) const + void unpackSquareMatrixImpl(const OutputIter dst_row_major) const { StaticAssert::check(); - const unsigned 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) + typedef SquareMatrixTraits Traits; + + if (this->size() == Traits::NumRowsCols || this->size() == 1) // Scalar or diagonal { + OutputIter it = dst_row_major; for (unsigned index = 0; index < MaxSize; index++) { - const bool on_diagonal = (index / Width) == (index % Width); - if (on_diagonal) + if (Traits::isIndexOnDiagonal(index)) { - const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Width)); + const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols)); *it++ = ScalarType(this->at(source_index)); } else @@ -484,17 +608,40 @@ class UAVCAN_EXPORT Array : public ArrayImpl } } } - else if (this->size() == MaxSize) + else if (this->size() == Traits::NumElementsInTriangle) // Symmetric { + OutputIter it = dst_row_major; + SizeType source_index = 0; + for (int row = 0; row < Traits::NumRowsCols; row++) + { + for (int col = 0; col < Traits::NumRowsCols; col++) + { + if (col >= row) // Diagonal or upper-right triangle + { + *it++ = ScalarType(this->at(source_index)); + source_index++; + } + else // Lower-left triangle + { + // Transposing one element + *it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row)); + } + } + } + UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle); + } + else if (this->size() == MaxSize) // Full - no packing whatsoever + { + OutputIter it = dst_row_major; for (SizeType index = 0; index < MaxSize; index++) { *it++ = ScalarType(this->at(index)); } } - else + else // Everything else { // coverity[suspicious_sizeof : FALSE] - ::uavcan::fill_n(it, MaxSize, ScalarType(0)); + ::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0)); } } diff --git a/libuavcan/test/marshal/array.cpp b/libuavcan/test/marshal/array.cpp index f3dfebb3ad..d213c78b2a 100644 --- a/libuavcan/test/marshal/array.cpp +++ b/libuavcan/test/marshal/array.cpp @@ -1024,6 +1024,73 @@ TEST(Array, SquareMatrixPacking) ASSERT_EQ(1, m2x2f.size()); ASSERT_FLOAT_EQ(0, m2x2f[0]); } + + // Symmetric matrix will contain only upper-right triangle + { + const float sym2x2[] = + { + 1, 2, + 2, 1 + }; + m2x2f.packSquareMatrix(sym2x2); + ASSERT_EQ(3, m2x2f.size()); + + float sym2x2_out[4]; + m2x2f.unpackSquareMatrix(sym2x2_out); + ASSERT_FLOAT_EQ(1, sym2x2_out[0]); + ASSERT_FLOAT_EQ(2, sym2x2_out[1]); + ASSERT_FLOAT_EQ(2, sym2x2_out[2]); + ASSERT_FLOAT_EQ(1, sym2x2_out[3]); + } + { + const float sym3x3[] = + { + 1, 2, 3, + 2, 4, 5, + 3, 5, 6 + }; + m3x3s.packSquareMatrix(sym3x3); + ASSERT_EQ(6, m3x3s.size()); + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); + + float sym3x3_out[9]; + m3x3s.unpackSquareMatrix(sym3x3_out); + + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]); + } + } + { + const double sym6x6[] = + { + 1, 2, 3, 4, 5, 6, + 2, 7, 8, 9, 10, 11, + 3, 8, 12, 13, 14, 15, + 4, 9, 13, 16, 17, 18, + 5, 10, 14, 17, 19, 20, + 6, 11, 15, 18, 20, 21 + }; + m6x6d.packSquareMatrix(sym6x6); + ASSERT_EQ(21, m6x6d.size()); + for (uavcan::uint8_t i = 0; i < 21; i++) + { + ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]); + } + + double sym6x6_out[36]; + m6x6d.unpackSquareMatrix(sym6x6_out); + + for (int i = 0; i < 36; i++) + { + ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]); + } + } } @@ -1195,6 +1262,49 @@ TEST(Array, SquareMatrixPackingInPlace) const bool diagonal = (i == 0) || (i == 4) || (i == 8); ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]); } + + // Try again with symmetric matrix + /* + * Full matrix: + * 1 2 3 + * 2 4 5 + * 3 5 6 + * Compressed triangle: + * 1 2 3 + * 4 5 + * 6 + */ + m3x3s.clear(); + m3x3s.push_back(1); + m3x3s.push_back(2); + m3x3s.push_back(3); + m3x3s.push_back(4); + m3x3s.push_back(5); + m3x3s.push_back(6); + // Unpacking + ASSERT_EQ(6, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(2, m3x3s[3]); + ASSERT_EQ(4, m3x3s[4]); + ASSERT_EQ(5, m3x3s[5]); + ASSERT_EQ(3, m3x3s[6]); + ASSERT_EQ(5, m3x3s[7]); + ASSERT_EQ(6, m3x3s[8]); + // Packing back + m3x3s.packSquareMatrix(); + ASSERT_EQ(6, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); } TEST(Array, FuzzyComparison)