Give vector math a (potential) boost. Next stoP; matrices

simgear/math/SGVec2.hxx

@@ -24,6 +24,8 @@
 #include <simgear/math/SGMisc.hxx>
 #include <simgear/math/SGMathFwd.hxx>
 
+#include "simd.hxx"
+
 /// 2D Vector Class
 template<typename T>
 class SGVec2 {
@@ -48,7 +50,7 @@ public:
   /// Constructor. Initialize by the content of a plain array,
   /// make sure it has at least 2 elements
   explicit SGVec2(const T* d)
-  { data()[0] = d[0]; data()[1] = d[1]; }
+  { simd4_t<T,2> r(d); _data = r; }
   template<typename S>
   explicit SGVec2(const SGVec2<S>& d)
   { data()[0] = d[0]; data()[1] = d[1]; }
@@ -82,25 +84,30 @@ public:
 
   /// Access raw data
   const T (&data(void) const)[2]
-  { return _data; }
+  { return _data.ptr(); }
   /// Access raw data
   T (&data(void))[2]
+  { return _data.ptr(); }
+  const simd4_t<T,2> (&simd2(void) const)
+  { return _data; }
+  /// Readonly raw storage interface
+  simd4_t<T,2> (&simd2(void))
   { return _data; }
 
   /// Inplace addition
   SGVec2& operator+=(const SGVec2& v)
-  { data()[0] += v(0); data()[1] += v(1); return *this; }
+  { _data += v.simd2(); return *this; }
   /// Inplace subtraction
   SGVec2& operator-=(const SGVec2& v)
-  { data()[0] -= v(0); data()[1] -= v(1); return *this; }
+  { _data -= v.simd2(); return *this; }
   /// Inplace scalar multiplication
   template<typename S>
   SGVec2& operator*=(S s)
-  { data()[0] *= s; data()[1] *= s; return *this; }
+  { _data *= s; return *this; }
   /// Inplace scalar multiplication by 1/s
   template<typename S>
   SGVec2& operator/=(S s)
-  { return operator*=(1/T(s)); }
+  { _data*=(1/T(s)); return *this; }
 
   /// Return an all zero vector
   static SGVec2 zeros(void)
@@ -112,7 +119,7 @@ public:
   { return SGVec2(0, 1); }
 
 private:
-  T _data[2];
+  simd4_t<T,2> _data;
 };
 
 /// Unary +, do nothing ...
@@ -126,36 +133,36 @@ operator+(const SGVec2<T>& v)
 template<typename T>
 inline
 SGVec2<T>
-operator-(const SGVec2<T>& v)
+operator-(SGVec2<T> v)
-{ return SGVec2<T>(-v(0), -v(1)); }
+{ v *= -1; return v; }
 
 /// Binary +
 template<typename T>
 inline
 SGVec2<T>
-operator+(const SGVec2<T>& v1, const SGVec2<T>& v2)
+operator+(SGVec2<T> v1, const SGVec2<T>& v2)
-{ return SGVec2<T>(v1(0)+v2(0), v1(1)+v2(1)); }
+{ v1.simd2() += v2.simd2(); return v1; }
 
 /// Binary -
 template<typename T>
 inline
 SGVec2<T>
-operator-(const SGVec2<T>& v1, const SGVec2<T>& v2)
+operator-(SGVec2<T> v1, const SGVec2<T>& v2)
-{ return SGVec2<T>(v1(0)-v2(0), v1(1)-v2(1)); }
+{ v1.simd2() -= v2.simd2(); return v1; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec2<T>
-operator*(S s, const SGVec2<T>& v)
+operator*(S s, SGVec2<T> v)
-{ return SGVec2<T>(s*v(0), s*v(1)); }
+{ v.simd2() *= s; return v; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec2<T>
-operator*(const SGVec2<T>& v, S s)
+operator*(SGVec2<T> v, S s)
-{ return SGVec2<T>(s*v(0), s*v(1)); }
+{ v.simd2() *= s; return v; }
 
 /// multiplication as a multiplicator, that is assume that the first vector
 /// represents a 2x2 diagonal matrix with the diagonal elements in the vector.
@@ -163,8 +170,8 @@ operator*(const SGVec2<T>& v, S s)
 template<typename T>
 inline
 SGVec2<T>
-mult(const SGVec2<T>& v1, const SGVec2<T>& v2)
+mult(SGVec2<T> v1, const SGVec2<T>& v2)
-{ return SGVec2<T>(v1(0)*v2(0), v1(1)*v2(1)); }
+{ v1.simd2() *= v2.simd2(); return v1; }
 
 /// component wise min
 template<typename T>
@@ -215,8 +222,8 @@ SGVec2<T> addClipOverflow(SGVec2<T> const& lhs, SGVec2<T> const& rhs)
 template<typename T>
 inline
 T
-dot(const SGVec2<T>& v1, const SGVec2<T>& v2)
+dot(SGVec2<T> v1, const SGVec2<T>& v2)
-{ return v1(0)*v2(0) + v1(1)*v2(1); }
+{ v1.simd2() *= v2.simd2(); return (v1(0)+v1(1)+v1(2)); }
 
 /// The euclidean norm of the vector, that is what most people call length
 template<typename T>
@@ -341,8 +348,8 @@ dist(const SGVec2<T>& v1, const SGVec2<T>& v2)
 template<typename T>
 inline
 T
-distSqr(const SGVec2<T>& v1, const SGVec2<T>& v2)
+distSqr(SGVec2<T> v1, const SGVec2<T>& v2)
-{ SGVec2<T> tmp = v1 - v2; return dot(tmp, tmp); }
+{ v1 -= v2; return dot(v1, v1); }
 
 // calculate the projection of u along the direction of d.
 template<typename T>

simgear/math/SGVec3.hxx

@@ -23,6 +23,8 @@
 #include <simgear/math/SGVec2.hxx>
 #include <simgear/math/SGGeodesy.hxx>
 
+#include "simd.hxx"
+
 /// 3D Vector Class
 template<typename T>
 class SGVec3 {
@@ -58,7 +60,7 @@ public:
   /// Constructor. Initialize by the content of a plain array,
   /// make sure it has at least 3 elements
   explicit SGVec3(const T* d)
-  { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
+  { simd4_t<T,3> r(d); _data = r; }
   template<typename S>
   explicit SGVec3(const SGVec3<S>& d)
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
@@ -100,25 +102,31 @@ public:
 
   /// Readonly raw storage interface
   const T (&data(void) const)[3]
-  { return _data; }
+  { return _data.ptr(); }
   /// Readonly raw storage interface
   T (&data(void))[3]
+  { return _data.ptr(); }
+  /// Readonly raw storage interface
+  const simd4_t<T,3> (&simd3(void) const)
+  { return _data; }
+  /// Readonly raw storage interface
+  simd4_t<T,3> (&simd3(void))
   { return _data; }
 
   /// Inplace addition
   SGVec3& operator+=(const SGVec3& v)
-  { data()[0] += v(0); data()[1] += v(1); data()[2] += v(2); return *this; }
+  { _data += v.simd3(); return *this; }
   /// Inplace subtraction
   SGVec3& operator-=(const SGVec3& v)
-  { data()[0] -= v(0); data()[1] -= v(1); data()[2] -= v(2); return *this; }
+  { _data -= v.simd3(); return *this; }
   /// Inplace scalar multiplication
   template<typename S>
   SGVec3& operator*=(S s)
-  { data()[0] *= s; data()[1] *= s; data()[2] *= s; return *this; }
+  { _data *= s; return *this; }
   /// Inplace scalar multiplication by 1/s
   template<typename S>
   SGVec3& operator/=(S s)
-  { return operator*=(1/T(s)); }
+  { _data*=(1/T(s)); return *this; }
 
   /// Return an all zero vector
   static SGVec3 zeros(void)
@@ -139,7 +147,8 @@ public:
   static SGVec3 fromGeoc(const SGGeoc& geoc);
 
 private:
-  T _data[3];
+  simd4_t<T,3> _data;
+  
 };
 
 template<>
@@ -193,36 +202,36 @@ operator+(const SGVec3<T>& v)
 template<typename T>
 inline
 SGVec3<T>
-operator-(const SGVec3<T>& v)
+operator-(SGVec3<T> v)
-{ return SGVec3<T>(-v(0), -v(1), -v(2)); }
+{ v *= -1; return v; }
 
 /// Binary +
 template<typename T>
 inline
 SGVec3<T>
-operator+(const SGVec3<T>& v1, const SGVec3<T>& v2)
+operator+(SGVec3<T> v1, const SGVec3<T>& v2)
-{ return SGVec3<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); }
+{ v1.simd3() += v2.simd3(); return v1; }
 
 /// Binary -
 template<typename T>
 inline
 SGVec3<T>
-operator-(const SGVec3<T>& v1, const SGVec3<T>& v2)
+operator-(SGVec3<T> v1, const SGVec3<T>& v2)
-{ return SGVec3<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); }
+{ v1.simd3() -= v2.simd3(); return v1; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec3<T>
-operator*(S s, const SGVec3<T>& v)
+operator*(S s, SGVec3<T> v)
-{ return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
+{ v.simd3() *= s; return v; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec3<T>
-operator*(const SGVec3<T>& v, S s)
+operator*(SGVec3<T> v, S s)
-{ return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
+{ v.simd3() *= s; return v; }
 
 /// multiplication as a multiplicator, that is assume that the first vector
 /// represents a 3x3 diagonal matrix with the diagonal elements in the vector.
@@ -230,8 +239,8 @@ operator*(const SGVec3<T>& v, S s)
 template<typename T>
 inline
 SGVec3<T>
-mult(const SGVec3<T>& v1, const SGVec3<T>& v2)
+mult(SGVec3<T> v1, const SGVec3<T>& v2)
-{ return SGVec3<T>(v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2)); }
+{ v1.simd3() *= v2.simd3(); return v1; }
 
 /// component wise min
 template<typename T>
@@ -307,8 +316,8 @@ SGVec3<T> addClipOverflow(SGVec3<T> const& lhs, SGVec3<T> const& rhs)
 template<typename T>
 inline
 T
-dot(const SGVec3<T>& v1, const SGVec3<T>& v2)
+dot(SGVec3<T> v1, const SGVec3<T>& v2)
-{ return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); }
+{ v1.simd3() *= v2.simd3(); return (v1(0)+v1(1)+v1(2)); }
 
 /// The euclidean norm of the vector, that is what most people call length
 template<typename T>
@@ -474,8 +483,8 @@ dist(const SGVec3<T>& v1, const SGVec3<T>& v2)
 template<typename T>
 inline
 T
-distSqr(const SGVec3<T>& v1, const SGVec3<T>& v2)
+distSqr(SGVec3<T> v1, const SGVec3<T>& v2)
-{ SGVec3<T> tmp = v1 - v2; return dot(tmp, tmp); }
+{ v1 -= v2; return dot(v1, v1); }
 
 // calculate the projection of u along the direction of d.
 template<typename T>

simgear/math/SGVec4.hxx

@@ -20,6 +20,8 @@
 
 #include <iosfwd>
 
+#include "simd.hxx"
+
 /// 4D Vector Class
 template<typename T>
 class SGVec4 {
@@ -44,7 +46,7 @@ public:
   /// Constructor. Initialize by the content of a plain array,
   /// make sure it has at least 3 elements
   explicit SGVec4(const T* d)
-  { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
+  { simd4_t<T,4> r(d); _data = r; }
   template<typename S>
   explicit SGVec4(const SGVec4<S>& d)
   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
@@ -92,25 +94,31 @@ public:
 
   /// Readonly raw storage interface
   const T (&data(void) const)[4]
-  { return _data; }
+  { return _data.ptr(); }
   /// Readonly raw storage interface
   T (&data(void))[4]
+  { return _data.ptr(); }
+  /// Readonly raw storage interface
+  const simd4_t<T,4> (&simd4(void) const)
+  { return _data; }
+  /// Readonly raw storage interface
+  simd4_t<T,4> (&simd4(void))
   { return _data; }
 
   /// Inplace addition
   SGVec4& operator+=(const SGVec4& v)
-  { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; }
+  { _data += v.simd4(); return *this; }
   /// Inplace subtraction
   SGVec4& operator-=(const SGVec4& v)
-  { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; }
+  { _data -= v.simd4(); return *this; }
   /// Inplace scalar multiplication
   template<typename S>
   SGVec4& operator*=(S s)
-  { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; }
+  { _data *= s; return *this; }
   /// Inplace scalar multiplication by 1/s
   template<typename S>
   SGVec4& operator/=(S s)
-  { return operator*=(1/T(s)); }
+  { _data*=(1/T(s)); return *this; }
 
   /// Return an all zero vector
   static SGVec4 zeros(void)
@@ -126,7 +134,7 @@ public:
   { return SGVec4(0, 0, 0, 1); }
 
 private:
-  T _data[4];
+  simd4_t<T,4> _data;
 };
 
 /// Unary +, do nothing ...
@@ -140,36 +148,36 @@ operator+(const SGVec4<T>& v)
 template<typename T>
 inline
 SGVec4<T>
-operator-(const SGVec4<T>& v)
+operator-(SGVec4<T> v)
-{ return SGVec4<T>(-v(0), -v(1), -v(2), -v(3)); }
+{ v *= -1; return v; }
 
 /// Binary +
 template<typename T>
 inline
 SGVec4<T>
-operator+(const SGVec4<T>& v1, const SGVec4<T>& v2)
+operator+(SGVec4<T> v1, const SGVec4<T>& v2)
-{ return SGVec4<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
+{ v1.simd4() += v2.simd4(); return v1; }
 
 /// Binary -
 template<typename T>
 inline
 SGVec4<T>
-operator-(const SGVec4<T>& v1, const SGVec4<T>& v2)
+operator-(SGVec4<T> v1, const SGVec4<T>& v2)
-{ return SGVec4<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
+{ v1.simd4() -= v2.simd4(); return v1; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec4<T>
-operator*(S s, const SGVec4<T>& v)
+operator*(S s, SGVec4<T> v)
-{ return SGVec4<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+{ v.simd4() *= s; return v; }
 
 /// Scalar multiplication
 template<typename S, typename T>
 inline
 SGVec4<T>
-operator*(const SGVec4<T>& v, S s)
+operator*(SGVec4<T> v, S s)
-{ return SGVec4<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+{ v.simd4() *= s; return v; }
 
 /// multiplication as a multiplicator, that is assume that the first vector
 /// represents a 4x4 diagonal matrix with the diagonal elements in the vector.
@@ -177,8 +185,8 @@ operator*(const SGVec4<T>& v, S s)
 template<typename T>
 inline
 SGVec4<T>
-mult(const SGVec4<T>& v1, const SGVec4<T>& v2)
+mult(SGVec4<T> v1, const SGVec4<T>& v2)
-{ return SGVec4<T>(v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2), v1(3)*v2(3)); }
+{ v1.simd4() *= v2.simd4(); return v1; }
 
 /// component wise min
 template<typename T>
@@ -261,8 +269,8 @@ SGVec4<T> addClipOverflow(SGVec4<T> const& lhs, SGVec4<T> const& rhs)
 template<typename T>
 inline
 T
-dot(const SGVec4<T>& v1, const SGVec4<T>& v2)
+dot(SGVec4<T> v1, const SGVec4<T>& v2)
-{ return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); }
+{ v1.simd4() *= v2.simd4(); return (v1(0)+v1(1)+v1(2)+v1(3)); }
 
 /// The euclidean norm of the vector, that is what most people call length
 template<typename T>
@@ -395,8 +403,8 @@ dist(const SGVec4<T>& v1, const SGVec4<T>& v2)
 template<typename T>
 inline
 T
-distSqr(const SGVec4<T>& v1, const SGVec4<T>& v2)
+distSqr(SGVec4<T> v1, const SGVec4<T>& v2)
-{ SGVec4<T> tmp = v1 - v2; return dot(tmp, tmp); }
+{ v1 -= v2; return dot(v1, v1); }
 
 // calculate the projection of u along the direction of d.
 template<typename T>