300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正

Header_Files/math_tc.h

@@ -0,0 +1,136 @@
+#ifndef MATH_H
+#define MATH_H
+
+#include <iostream>
+#include <stdexcept>
+
+using namespace std;
+
+template <class T>
+void ZZ_MinMax(T num[], int length, int &indexofMax, int &indexofMin)
+{
+
+    if (num == nullptr || length <= 0)
+    {
+        throw invalid_argument("Pay attention to the input array in minmax");
+        return;
+    }
+
+
+    int begin = 0;
+    if (length % 2 == 1)
+    {
+        indexofMax = indexofMin = 0;
+        begin = 1;
+    }
+    else
+    {
+        if (num[0] < num[1])
+        {
+            indexofMax = 1;
+            indexofMin = 0;
+        }
+        else {
+            indexofMax = 0;
+            indexofMin = 1;
+        }
+        begin = 2;
+    }
+    for (int i = begin; i != length; i += 2)
+    {
+        if (num[i] < num[i + 1])
+        {
+            if (num[indexofMax] < num[i + 1]) indexofMax = i + 1;
+            if (num[indexofMin] > num[i]) indexofMin = i;
+        }
+        else
+        {
+            if (num[indexofMax] < num[i]) indexofMax = i;
+            if (num[indexofMin] > num[i + 1]) indexofMin = i + 1;
+        }
+    }
+}
+
+
+
+template<typename T>
+void MinHeapify(T*arry, int size, int element)
+{
+    int lchild = element * 2 + 1, rchild = lchild + 1;//左右子树
+    while (rchild < size)//子树均在范围内
+    {
+        if (arry[element] <= arry[lchild] && arry[element] <= arry[rchild])//如果比左右子树都小，完成整理
+        {
+            return;
+        }
+        if (arry[lchild] <= arry[rchild])//如果左边最小
+        {
+            std::swap(arry[element], arry[lchild]);//把左面的提到上面
+            element = lchild;//循环时整理子树
+        }
+        else//否则右面最小
+        {
+            std::swap(arry[element], arry[rchild]);//同理
+            element = rchild;
+        }
+        lchild = element * 2 + 1;
+        rchild = lchild + 1;//重新计算子树位置
+    }
+    if (lchild < size&&arry[lchild] < arry[element])//只有左子树且子树小于自己
+    {
+        std::swap(arry[lchild], arry[element]);
+    }
+    return;
+}
+
+template<typename T>
+void MaxHeapify(T*arry, int size, int element)
+{
+    int lchild = element * 2 + 1, rchild = lchild + 1;//左右子树
+    while (rchild < size)//子树均在范围内
+    {
+        if (arry[element] >= arry[lchild] && arry[element] >= arry[rchild])//如果比左右子树都小，完成整理
+        {
+            return;
+        }
+        if (arry[lchild] >= arry[rchild])//如果左边最小
+        {
+            std::swap(arry[element], arry[lchild]);//把左面的提到上面
+            element = lchild;//循环时整理子树
+        }
+        else//否则右面最小
+        {
+            std::swap(arry[element], arry[rchild]);//同理
+            element = rchild;
+        }
+        lchild = element * 2 + 1;
+        rchild = lchild + 1;//重新计算子树位置
+    }
+    if (lchild<size&&arry[lchild]>arry[element])//只有左子树且子树小于自己
+    {
+        std::swap(arry[lchild], arry[element]);
+    }
+    return;
+}
+
+
+template<typename T>
+void HeapSort(T*arry, int size)
+{
+    int i;
+    for (i = size - 1; i >= 0; i--)//从子树开始整理树
+    {
+        MinHeapify(arry, size, i);
+    }
+    while (size > 0)//拆除树
+    {
+        std::swap(arry[size - 1], arry[0]);//将根（最小）与数组最末交换
+
+        size--;//树大小减小
+        MinHeapify(arry, size, 0);//整理树
+    }
+    return;
+}
+
+
+#endif // MATH_H