Обновлена IRS

Source/INS/util/Record.cpp

@@ -0,0 +1,47 @@
+#include "Record.h"
+#include <math.h>
+
+template class Record<float>;
+template class Record<Vector2>;
+template class Record<Vector3>;
+template class Record<Quaternion>;
+
+template <class T>
+Record<T>::Record(T* RecordBuffer, float Frequency, float Period)
+{
+  Init(RecordBuffer, Frequency, Period);
+}
+
+template <class T>
+void Record<T>::Init(T* RecordBuffer, float Frequency, float Period)
+{
+  Freq = Frequency;
+  Buffer = RecordBuffer;
+  Count = (float)(Frequency * Period + 1.0f);
+}
+
+template <class T>
+T Record<T>::Past(float Time) const
+{
+  unsigned long move = (float)(Time * Freq);
+  if (move >= Count) move = Count - 1;
+  
+  if (Index >= move) move = Index - move;
+  else move = Index + (Count - move);
+
+  return Buffer[move];
+}
+
+template <class T>
+void Record<T>::Add(const T& Value)
+{
+  Index++;
+  if (Index >= Count) Index = 0;
+  Buffer[Index] = Value;
+}
+
+template <class T>
+void Record<T>::Mix(const T& Shift)
+{
+  for (unsigned int a = 0; a < Count; a++) Buffer[a] += Shift;
+}

Source/INS/util/Record.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#ifndef RECORD_H
+#define RECORD_H
+
+#include "Vector.h"
+#include "Quaternion.h"
+
+template <typename T>
+class Record
+{
+public:
+  Record() {};
+  Record(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+  
+private:
+  float Freq;
+  unsigned long Count;
+  T* Buffer;
+  T Shift;
+  unsigned long Index = 0;
+
+public:
+  void Init(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+
+  T Past(float Time) const;
+
+  void Add(const T& Value);
+
+  void Mix(const T& Shift);
+};
+
+template <typename T>
+class Average
+{
+private:
+  T Value;
+  float Count = 0;
+
+public:
+  void Set(const T& Val) { Value += Val; Count++; }
+  T Get() { if (Count == 0.0f) return T(); T ave = Value / Count; Count = 0; Value = 0.0f; return ave; }
+};
+
+#endif