From 3417ca7e491626b83ecec68f89e721dc30ee21f6 Mon Sep 17 00:00:00 2001
From: Erik Hofman <erik@ehofman.com>
Date: Thu, 2 Feb 2017 11:35:11 +0100
Subject: [PATCH] Add a linear-interpolation function

---
 simgear/math/SGVec2.hxx | 10 ++++++++++
 simgear/math/SGVec3.hxx | 10 ++++++++++
 simgear/math/SGVec4.hxx | 10 ++++++++++
 simgear/math/simd.hxx   | 15 ++++++++++-----
 4 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx
index e44344ec..21746a39 100644
--- a/simgear/math/SGVec2.hxx
+++ b/simgear/math/SGVec2.hxx
@@ -365,6 +365,16 @@ projection(const SGVec2<T>& u, const SGVec2<T>& d)
   else return d * (dot(u, d) / denom);
 }
 
+template<typename T>
+inline
+SGVec2<T>
+interpolate(T tau, const SGVec2<T>& v1, const SGVec2<T>& v2)
+{
+  SGVec2<T> r;
+  r.simd2() = simd4::interpolate(tau, v1.simd2(), v2.simd2());
+  return r;
+}
+
 #ifndef NDEBUG
 template<typename T>
 inline
diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx
index f57a1000..720a13a0 100644
--- a/simgear/math/SGVec3.hxx
+++ b/simgear/math/SGVec3.hxx
@@ -472,6 +472,16 @@ projection(const SGVec3<T>& u, const SGVec3<T>& d)
   else return d * (dot(u, d) / denom);
 }
 
+template<typename T>
+inline
+SGVec3<T>
+interpolate(T tau, const SGVec3<T>& v1, const SGVec3<T>& v2)
+{
+  SGVec3<T> r;
+  r.simd3() = simd4::interpolate(tau, v1.simd3(), v2.simd3());
+  return r;
+}
+
 #ifndef NDEBUG
 template<typename T>
 inline
diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx
index adf051c7..24cef2bb 100644
--- a/simgear/math/SGVec4.hxx
+++ b/simgear/math/SGVec4.hxx
@@ -391,6 +391,16 @@ projection(const SGVec4<T>& u, const SGVec4<T>& d)
   else return d * (dot(u, d) / denom);
 }
 
+template<typename T>
+inline
+SGVec4<T>
+interpolate(T tau, const SGVec4<T>& v1, const SGVec4<T>& v2)
+{
+  SGVec4<T> r;
+  r.simd4() = simd4::interpolate(tau, v1.simd4(), v2.simd4());
+  return r;
+}
+
 #ifndef NDEBUG
 template<typename T>
 inline
diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx
index 48b1d69d..da80aafa 100644
--- a/simgear/math/simd.hxx
+++ b/simgear/math/simd.hxx
@@ -76,6 +76,11 @@ inline T magnitude2(const simd4_t<T,N>& vi) {
     return mag2;
 }
 
+template<typename T, int N>
+inline simd4_t<T,N> interpolate(T tau, const simd4_t<T,N>& v1, const simd4_t<T,N>& v2) {
+    return (T(1)-tau)*v1 + tau*v2;
+}
+
 template<typename T, int N>
 inline T magnitude(const simd4_t<T,N>& v) {
     return std::sqrt(magnitude2(v));
@@ -105,11 +110,11 @@ inline T dot(const simd4_t<T,N>& v1, const simd4_t<T,N>& v2) {
 template<typename T>
 inline simd4_t<T,3> cross(const simd4_t<T,3>& v1, const simd4_t<T,3>& v2)
 {
-   simd4_t<T,3> d;
-   d[0] = v1[1]*v2[2] - v1[2]*v2[1];
-   d[1] = v1[2]*v2[0] - v1[0]*v2[2];
-   d[2] = v1[0]*v2[1] - v1[1]*v2[0];
-   return d;
+    simd4_t<T,3> d;
+    d[0] = v1[1]*v2[2] - v1[2]*v2[1];
+    d[1] = v1[2]*v2[0] - v1[0]*v2[2];
+    d[2] = v1[0]*v2[1] - v1[1]*v2[0];
+    return d;
 }
 
 } /* namespace simd4 */