From 4372701dab5958253325baf4dbcbdafb5ec5f147 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 4 Jul 2015 17:24:55 +0200
Subject: [PATCH] EKF: Fix entirely unnecessary C++11 dependency

---
 .../estimator_utilities.cpp                     | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 284a099023..527420ba0b 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -38,7 +38,6 @@
  */
 
 #include "estimator_utilities.h"
-#include <algorithm>
 
 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
@@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...)
 void ekf_debug(const char *fmt, ...) { while(0){} }
 #endif
 
+/* we don't want to pull in the standard lib just to swap two floats */
+void swap_var(float &d1, float &d2);
+
 float Vector3f::length(void) const
 {
     return sqrt(x*x + y*y + z*z);
@@ -108,9 +110,9 @@ void Mat3f::identity() {
 Mat3f Mat3f::transpose() const
 {
     Mat3f ret = *this;
-    std::swap(ret.x.y, ret.y.x);
-    std::swap(ret.x.z, ret.z.x);
-    std::swap(ret.y.z, ret.z.y);
+    swap_var(ret.x.y, ret.y.x);
+    swap_var(ret.x.z, ret.z.x);
+    swap_var(ret.y.z, ret.z.y);
     return ret;
 }
 
@@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar)
     vecOut.z = vec.z / scalar;
     return vecOut;
 }
+
+void swap_var(float &d1, float &d2)
+{
+    float tmp = d1;
+    d1 = d2;
+    d2 = tmp;
+}