Implement vpMatrix::insert() using memcpy suggestion for the general …

…case. Fix lagadic#165 issue.
s-trinh · Mar 30, 2017 · f9c9d22 · f9c9d22
1 parent 2937cc3
commit f9c9d22
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 11 deletions.
diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h
@@ -273,13 +273,13 @@ class vpArray2D
     */
   friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
   {
-    if (A.data == NULL)
+    if (A.data == NULL || A.size() == 0)
       return s;
     std::ios_base::fmtflags original_flags = s.flags();
 
     s.precision(10) ;
     for (unsigned int i=0;i<A.getRows();i++) {
-      for (unsigned int j=0;j<A.getCols() -1;j++){
+      for (unsigned int j=0;j<A.getCols()-1;j++){
         s << A[i][j] << "  ";
       }
       // We don't add "  " after the last row element

diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h
@@ -224,7 +224,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D<double>
                         "Cannot resize a row vector to a (%dx%d) dimension vector that has more than one row",
                         nrows, ncols));
     vpArray2D<double>::resize(nrows, ncols, flagNullify);
-  };
+  }
 
   void stack(const double &d);
   void stack(const vpRowVector &v);

diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp
@@ -2922,12 +2922,10 @@ void vpMatrix::insert(const vpMatrix&A, const unsigned int r,
   if( (r + A.getRows() ) <= rowNum && (c + A.getCols() ) <= colNum ){
     if (A.colNum == colNum && data != NULL && A.data != NULL && A.size() > 0) {
       memcpy(data+r*colNum, A.data, sizeof(double)*A.size());
-    } else {
+    } else if(data != NULL && A.data != NULL && A.colNum > 0) {
       // recopy matrix A in the current one, does not call static function to avoid initialisation and recopy of matrix
       for(unsigned int i=r; i<(r+A.getRows()); i++){
-        for(unsigned int j=c; j<(c+A.getCols()); j++){
-          (*this)[i][j] = A[i-r][j-c];
-        }
+        memcpy(data+i*colNum+c, A.data+(i-r)*A.colNum, sizeof(double)*A.colNum);
       }
     }
   }

diff --git a/modules/core/test/math/testColVector.cpp b/modules/core/test/math/testColVector.cpp
@@ -457,10 +457,10 @@ int main()
     v1.insert(0, v2);
     v3.insert(0, v2);
 
-//    std::cout << "Insert empty vectors:" << std::endl;
-//    std::cout << "v1: " << v1.t() << std::endl;
-//    std::cout << "v2: " << v2.t() << std::endl;
-//    std::cout << "v3: " << v3.t() << std::endl;
+    std::cout << "Insert empty vectors:" << std::endl;
+    std::cout << "v1: " << v1.t() << std::endl;
+    std::cout << "v2: " << v2.t() << std::endl;
+    std::cout << "v3: " << v3.t() << std::endl;
   }
 
 

diff --git a/modules/core/test/math/testMatrix.cpp b/modules/core/test/math/testMatrix.cpp
@@ -360,6 +360,51 @@ main()
       std::cout << "m3:\n" << m3 << std::endl;
     }
 
+    {
+      vpMatrix m1(11,9), m2(3,4);
+      for (unsigned int i = 0; i < m2.getRows(); i++) {
+        for (unsigned int j = 0; j < m2.getCols(); j++) {
+          m2[i][j] = getRandomValues(-100.0, 100.0);
+        }
+      }
+
+      unsigned int offset_i = 4, offset_j = 3;
+      m1.insert(m2, offset_i, offset_j);
+
+      for (unsigned int i = 0; i < m2.getRows(); i++) {
+        for (unsigned int j = 0; j < m2.getCols(); j++) {
+          if ( !vpMath::equal(m1[i+offset_i][j+offset_j], m2[i][j], std::numeric_limits<double>::epsilon()) ) {
+            std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+            return EXIT_FAILURE;
+          }
+        }
+      }
+
+      offset_i = 4, offset_j = 5;
+      m1.insert(m2, offset_i, offset_j);
+
+      for (unsigned int i = 0; i < m2.getRows(); i++) {
+        for (unsigned int j = 0; j < m2.getCols(); j++) {
+          if ( !vpMath::equal(m1[i+offset_i][j+offset_j], m2[i][j], std::numeric_limits<double>::epsilon()) ) {
+            std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+            return EXIT_FAILURE;
+          }
+        }
+      }
+
+      offset_i = 8, offset_j = 5;
+      m1.insert(m2, offset_i, offset_j);
+
+      for (unsigned int i = 0; i < m2.getRows(); i++) {
+        for (unsigned int j = 0; j < m2.getCols(); j++) {
+          if ( !vpMath::equal(m1[i+offset_i][j+offset_j], m2[i][j], std::numeric_limits<double>::epsilon()) ) {
+            std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+            return EXIT_FAILURE;
+          }
+        }
+      }
+    }
+
     std::cout << "All tests succeed" << std::endl;
     return EXIT_SUCCESS;
   }