[utils] added array utilities

include/gnss_utils/utils/utils.h

@@ -7,6 +7,8 @@
 #include <string>
 
 #define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
+#define ARRAY2D_NROWS(arr) sizeof(arr) / sizeof(arr[0])
+#define ARRAY2D_NCOLS(arr) sizeof(arr[0]) / sizeof(arr[0][0])
 #define GNSSUTILS_MSG "--GnssUtils--"
 
 extern "C" {
@@ -22,15 +24,101 @@ void printArray(std::string _name, double* _array, int size);
 void printArray(std::string _name, float* _array, int size);
 
 template <typename T>
-bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
+inline bool addToArray(const T& new_element, T*& array, int& n, int& nmax)
 {
-  if (size1 != size2)
+  // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
+  // "inspired" from RTKLIB rinex.c
+  T* array_ref;
+  if (nmax <= n)
+  {
+    // std::cout << "addToArray: nmax <= n\n";
+    nmax += 1024;
+    if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
+    {
+      printf("addToArray malloc error: n=%d\n", nmax);
+      free(array);
+      array = NULL;
+      n = nmax = 0;
+      return false;
+    }
+    // std::cout << "addToArray: assigning reallocated array\n";
+    array = array_ref;
+  }
+  // std::cout << "addToArray: adding element " << n << "\n";
+  array[n++] = new_element;
+  // std::cout << "addToArray: added!\n";
+  return true;
+}
+
+template <typename T>
+inline bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
+{
+  // std::cout << "copyArray: " << n_in << " elements\n";
+  if (array_in == NULL)
     return false;
 
-  for (int ii = 0; ii < size1; ++ii)
+  // std::cout << "copyArray: array in not null\n";
+
+  for (int i = 0; i < n_in; i++)
   {
-    if (array1[ii] != array2[ii])
+    // std::cout << "copyArray: adding element " << i << "\n";
+    if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
+    {
+      // std::cout << "copyArray: failed to add..\n";
       return false;
+    }
+    // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+  }
+
+  // std::cout << "copyArray: all copied\n";
+
+  return true;
+}
+
+template <typename T>
+void freeArray(T*& array, int& n, int& nmax)
+{
+  if (array != NULL)
+    free(array);
+  array = NULL;
+  n = nmax = 0;
+}
+
+template <typename T>
+bool equalArray(const T* array_1, const T* array_2, const int& n1, const int& n2)
+{
+  if (n1 != n2)
+    return false;
+
+  for (int i = 0; i < n1; ++i)
+  {
+    if (array_1[i] != array_2[i])
+      return false;
+  }
+
+  return true;
+}
+
+template <typename T>
+bool equalArray2d(const T*   array_1,
+                  const T*   array_2,
+                  const int& nrow_1,
+                  const int& ncol_1,
+                  const int& nrow_2,
+                  const int& ncol_2)
+{
+  if (nrow_1 != nrow_2)
+    return false;
+  if (ncol_1 != ncol_2)
+    return false;
+
+  for (int i = 0; i < nrow_1; ++i)
+  {
+    for (int j = 0; j < ncol_1; ++j)
+    {
+      if (*((array_1 + i * ncol_1) + j) != *((array_2 + i * ncol_1) + j))
+        return false;
+    }
   }
 
   return true;
@@ -40,6 +128,8 @@ bool equalTime(const gtime_t& time1, const gtime_t& time2);
 bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
 bool equalObservations(const obs_t& obs1, const obs_t& obs2);
 
+bool equalNavigations(const nav_t& nav_1, const nav_t& nav_2);
+
 }  // namespace GnssUtils
 
-#endif // INCLUDE_GNSS_UTILS_UTILS_UTILS_H_
+#endif  // INCLUDE_GNSS_UTILS_UTILS_UTILS_H_
\ No newline at end of file

src/utils/utils.cpp

@@ -90,11 +90,11 @@ bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
     return false;
   if (obs1.freq != obs2.freq)
     return false;
-  if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
+  if (!equalArray<double>(obs1.L, obs2.L, ARRAY_SIZE(obs1.L), ARRAY_SIZE(obs2.L)))
     return false;
-  if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
+  if (!equalArray<double>(obs1.P, obs2.P, ARRAY_SIZE(obs1.P), ARRAY_SIZE(obs2.P)))
     return false;
-  if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
+  if (!equalArray<float>(obs1.D, obs2.D, ARRAY_SIZE(obs1.D), ARRAY_SIZE(obs2.D)))
     return false;
 
   return true;
@@ -117,4 +117,78 @@ bool equalObservations(const obs_t& obs1, const obs_t& obs2)
 
   return true;
 }
+
+bool equalNavigation(const nav_t& nav1, const nav_t& nav2)
+{
+  if (nav1.n != nav2.n)
+    return false;
+  if (nav1.nmax != nav2.nmax)
+    return false;
+  if (nav1.ng != nav2.ng)
+    return false;
+  if (nav1.ngmax != nav2.ngmax)
+    return false;
+  if (nav1.ns != nav2.ns)
+    return false;
+  if (nav1.nsmax != nav2.nsmax)
+    return false;
+  if (nav1.ne != nav2.ne)
+    return false;
+  if (nav1.nemax != nav2.nemax)
+    return false;
+  if (nav1.nc != nav2.nc)
+    return false;
+  if (nav1.ncmax != nav2.ncmax)
+    return false;
+  if (nav1.na != nav2.na)
+    return false;
+  if (nav1.namax != nav2.namax)
+    return false;
+  if (nav1.nt != nav2.nt)
+    return false;
+  if (nav1.ntmax != nav2.ntmax)
+    return false;
+  if (nav1.nf != nav2.nf)
+    return false;
+  if (nav1.nfmax != nav2.nfmax)
+    return false;
+
+  // eph
+  // geph
+  // seph
+  // peph
+  // pclk
+  // alm
+  // tec
+  // fcb
+  // erp
+  if (!equalArray<double>(nav1.utc_gps, nav2.utc_gps, ARRAY_SIZE(nav1.utc_gps), ARRAY_SIZE(nav2.utc_gps)))
+    return false;
+  if (!equalArray<double>(nav1.utc_glo, nav2.utc_glo, ARRAY_SIZE(nav1.utc_glo), ARRAY_SIZE(nav2.utc_glo)))
+    return false;
+  if (!equalArray<double>(nav1.utc_gal, nav2.utc_gal, ARRAY_SIZE(nav1.utc_gal), ARRAY_SIZE(nav2.utc_gal)))
+    return false;
+  if (!equalArray<double>(nav1.utc_qzs, nav2.utc_qzs, ARRAY_SIZE(nav1.utc_qzs), ARRAY_SIZE(nav2.utc_qzs)))
+    return false;
+  if (!equalArray<double>(nav1.utc_cmp, nav2.utc_cmp, ARRAY_SIZE(nav1.utc_cmp), ARRAY_SIZE(nav2.utc_cmp)))
+    return false;
+  if (!equalArray<double>(nav1.utc_irn, nav2.utc_irn, ARRAY_SIZE(nav1.utc_irn), ARRAY_SIZE(nav2.utc_irn)))
+    return false;
+  if (!equalArray<double>(nav1.utc_sbs, nav2.utc_sbs, ARRAY_SIZE(nav1.utc_sbs), ARRAY_SIZE(nav2.utc_sbs)))
+    return false;
+  if (!equalArray<double>(nav1.ion_gps, nav2.ion_gps, ARRAY_SIZE(nav1.ion_gps), ARRAY_SIZE(nav2.ion_gps)))
+    return false;
+  if (!equalArray<double>(nav1.ion_gal, nav2.ion_gal, ARRAY_SIZE(nav1.ion_gal), ARRAY_SIZE(nav2.ion_gal)))
+    return false;
+  if (!equalArray<double>(nav1.ion_qzs, nav2.ion_qzs, ARRAY_SIZE(nav1.ion_qzs), ARRAY_SIZE(nav2.ion_qzs)))
+    return false;
+  if (!equalArray<double>(nav1.ion_cmp, nav2.ion_cmp, ARRAY_SIZE(nav1.ion_cmp), ARRAY_SIZE(nav2.ion_cmp)))
+    return false;
+  if (!equalArray<double>(nav1.ion_irn, nav2.ion_irn, ARRAY_SIZE(nav1.ion_irn), ARRAY_SIZE(nav2.ion_irn)))
+    return false;
+  
+  if (nav1.leaps != nav2.leaps)
+    return false;
+  
+}
 }  // namespace GnssUtils