From 154dcd105ceb9291e92b75bac16776e2dd471586 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Thu, 3 Jul 2014 12:56:33 +0400
Subject: [PATCH] In-place matrix packing

---
 libuavcan/include/uavcan/marshal/array.hpp  | 45 +++++++++++++
 libuavcan/include/uavcan/util/templates.hpp |  6 ++
 libuavcan/test/marshal/array.cpp            | 72 +++++++++++++++++++++
 3 files changed, 123 insertions(+)

diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp
index f98536f6d1..ca8cca9ee0 100644
--- a/libuavcan/include/uavcan/marshal/array.hpp
+++ b/libuavcan/include/uavcan/marshal/array.hpp
@@ -648,6 +648,36 @@ public:
         packSquareMatrixImpl<const ScalarType*>(src_row_major);
     }
 
+    /**
+     * Fills this array as a packed square matrix in place.
+     */
+    void packSquareMatrix()
+    {
+        if (this->size() == MaxSize)
+        {
+            ValueType matrix[MaxSize];
+            for (unsigned i = 0; i < MaxSize; i++)
+            {
+                matrix[i] = this->at(i);
+            }
+            packSquareMatrix(matrix);
+        }
+        else if (this->size() == 0)
+        {
+            ; // Nothing to do - leave the matrix empty
+        }
+        else
+        {
+#if UAVCAN_EXCEPTIONS
+            throw std::out_of_range("uavcan::Array::packSquareMatrix()");
+#else
+            assert(0);
+            this->clear();
+#endif
+        }
+
+    }
+
     /**
      * Fills this array as a packed square matrix from any container that implements the methods begin() and size().
      */
@@ -683,6 +713,21 @@ public:
         unpackSquareMatrixImpl<ScalarType*>(dst_row_major);
     }
 
+    /**
+     * Reconstructs full matrix in place.
+     */
+    void unpackSquareMatrix()
+    {
+        ValueType matrix[MaxSize];
+        unpackSquareMatrix(matrix);
+
+        this->clear();
+        for (unsigned i = 0; i < MaxSize; i++)
+        {
+            this->push_back(matrix[i]);
+        }
+    }
+
     /**
      * Reconstructs full matrix, result will be saved into container that implements begin() and size().
      */
diff --git a/libuavcan/include/uavcan/util/templates.hpp b/libuavcan/include/uavcan/util/templates.hpp
index 4295c08d16..a2967e6c6d 100644
--- a/libuavcan/include/uavcan/util/templates.hpp
+++ b/libuavcan/include/uavcan/util/templates.hpp
@@ -147,6 +147,12 @@ template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<36> { enum { Result = 6 }; }
 template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<49> { enum { Result = 7 }; };
 template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<64> { enum { Result = 8 }; };
 template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<81> { enum { Result = 9 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<100> { enum { Result = 10 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<121> { enum { Result = 11 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<144> { enum { Result = 12 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<169> { enum { Result = 13 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<196> { enum { Result = 14 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<256> { enum { Result = 16 }; };
 
 /**
  * Replacement for std::copy(..)
diff --git a/libuavcan/test/marshal/array.cpp b/libuavcan/test/marshal/array.cpp
index dc7650b313..6b38f86c99 100644
--- a/libuavcan/test/marshal/array.cpp
+++ b/libuavcan/test/marshal/array.cpp
@@ -1069,3 +1069,75 @@ TEST(Array, SquareMatrixPackingErrors)
     ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range);
 }
 #endif
+
+TEST(Array, SquareMatrixPackingInPlace)
+{
+    Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
+
+    // Will do nothing - matrix is empty
+    m3x3s.packSquareMatrix();
+    ASSERT_TRUE(m3x3s.empty());
+
+    // Will fill with zeros - matrix is empty
+    m3x3s.unpackSquareMatrix();
+    ASSERT_EQ(9, m3x3s.size());
+    for (unsigned i = 0; i < 9; i++)
+    {
+        ASSERT_EQ(0, m3x3s[i]);
+    }
+
+    // Fill an unpackaple matrix
+    m3x3s.clear();
+    m3x3s.push_back(11);
+    m3x3s.push_back(12);
+    m3x3s.push_back(13);
+
+#if UAVCAN_EXCEPTIONS
+    // Shall throw - matrix is ill-formed
+    ASSERT_THROW(m3x3s.packSquareMatrix(), std::out_of_range);
+#endif
+
+    m3x3s.push_back(21);
+    m3x3s.push_back(22);
+    m3x3s.push_back(23);
+    m3x3s.push_back(31);
+    m3x3s.push_back(32);
+    m3x3s.push_back(33);
+
+    // Will pack/unpack successfully
+    ASSERT_EQ(9, m3x3s.size());
+    m3x3s.packSquareMatrix();
+    ASSERT_EQ(9, m3x3s.size());
+    m3x3s.unpackSquareMatrix();
+
+    // Make sure it was unpacked properly
+    ASSERT_EQ(11, m3x3s[0]);
+    ASSERT_EQ(12, m3x3s[1]);
+    ASSERT_EQ(13, m3x3s[2]);
+    ASSERT_EQ(21, m3x3s[3]);
+    ASSERT_EQ(22, m3x3s[4]);
+    ASSERT_EQ(23, m3x3s[5]);
+    ASSERT_EQ(31, m3x3s[6]);
+    ASSERT_EQ(32, m3x3s[7]);
+    ASSERT_EQ(33, m3x3s[8]);
+
+    // Try again with a scalar matrix
+    m3x3s.clear();
+    for (unsigned i = 0; i < 9; i++)
+    {
+        const bool diagonal = (i == 0) || (i == 4) || (i == 8);
+        m3x3s.push_back(diagonal ? 123 : 0);
+    }
+
+    ASSERT_EQ(9, m3x3s.size());
+    m3x3s.packSquareMatrix();
+    ASSERT_EQ(1, m3x3s.size());
+    m3x3s.unpackSquareMatrix();
+    ASSERT_EQ(9, m3x3s.size());
+
+    for (unsigned i = 0; i < 9; i++)
+    {
+        const bool diagonal = (i == 0) || (i == 4) || (i == 8);
+        ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]);
+    }
+}