From dbe4abefb89f01f1b602077ede45785564c9ab13 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A9r=C3=B4me=20Leclercq?= <lynix680@gmail.com>
Date: Sun, 20 Jun 2021 13:34:35 +0200
Subject: [PATCH] Math: Add PidController class

---
 include/Nazara/Math.hpp               |  1 +
 include/Nazara/Math/PidController.hpp | 31 +++++++++++++++++++++++++++
 include/Nazara/Math/PidController.inl | 28 ++++++++++++++++++++++++
 3 files changed, 60 insertions(+)
 create mode 100644 include/Nazara/Math/PidController.hpp
 create mode 100644 include/Nazara/Math/PidController.inl

diff --git a/include/Nazara/Math.hpp b/include/Nazara/Math.hpp
index e3d52540f..b4b965e07 100644
--- a/include/Nazara/Math.hpp
+++ b/include/Nazara/Math.hpp
@@ -40,6 +40,7 @@
 #include <Nazara/Math/Frustum.hpp>
 #include <Nazara/Math/Matrix4.hpp>
 #include <Nazara/Math/OrientedBox.hpp>
+#include <Nazara/Math/PidController.hpp>
 #include <Nazara/Math/Plane.hpp>
 #include <Nazara/Math/Quaternion.hpp>
 #include <Nazara/Math/Ray.hpp>
diff --git a/include/Nazara/Math/PidController.hpp b/include/Nazara/Math/PidController.hpp
new file mode 100644
index 000000000..96ac38958
--- /dev/null
+++ b/include/Nazara/Math/PidController.hpp
@@ -0,0 +1,31 @@
+// Copyright (C) 2021 Jérôme Leclercq
+// This file is part of the "Nazara Engine - Mathematics module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#pragma once
+
+#ifndef NAZARA_PIDCONTROLLER_HPP
+#define NAZARA_PIDCONTROLLER_HPP
+
+namespace Nz
+{
+	template<typename T>
+	class PidController
+	{
+		public:
+			PidController(float p, float i, float d);
+
+			T Update(const T& currentError, float elapsedTime);
+
+		private:
+			T m_lastError;
+			T m_integral;
+			float m_dFactor;
+			float m_iFactor;
+			float m_pFactor;
+	};
+}
+
+#include <Nazara/Math/PidController.inl>
+
+#endif
diff --git a/include/Nazara/Math/PidController.inl b/include/Nazara/Math/PidController.inl
new file mode 100644
index 000000000..f6478a4dd
--- /dev/null
+++ b/include/Nazara/Math/PidController.inl
@@ -0,0 +1,28 @@
+// Copyright (C) 2021 Jérôme Leclercq
+// This file is part of the "Nazara Engine - Mathematics module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/Math/PidController.hpp>
+
+namespace Nz
+{
+	template<typename T>
+	PidController<T>::PidController(float p, float i, float d) :
+	m_lastError(0),
+	m_integral(0),
+	m_dFactor(d),
+	m_iFactor(i),
+	m_pFactor(p)
+	{
+	}
+
+	template<typename T>
+	T PidController<T>::Update(const T& currentError, float elapsedTime)
+	{
+		m_integral += currentError * elapsedTime;
+		T deriv = (currentError - m_lastError) / elapsedTime;
+		m_lastError = currentError;
+
+		return currentError * m_pFactor + m_integral * m_iFactor + deriv * m_dFactor;
+	}
+}