[ADD] Vector distance function

src/core/common/math/linearAlgebra/WMatrixFixed.h

@@ -1346,6 +1346,53 @@ ValueT length( const WMatrixFixed< ValueT, 1, Cols, ValueStoreT >& a )
     return sqrt( length2( a ) );
 }
 
+/**
+ * Calculates the <b>squared</b> distance between two vectors.
+ *
+ * \tparam ValueT Value type
+ * \tparam ValueStoreT Value store to use
+ * \tparam Rows number of rows in this vector, either this or Cols should be 1
+ * \tparam Cols number of cols in this vector, either this or Rows should be 1
+ * \param a the first vector
+ * \param b the second vector
+ *
+ * \return the squared distance between the two vectors
+ */
+template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows, size_t Cols >
+ValueT distance2( const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& a, const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& b )
+{
+    BOOST_STATIC_ASSERT( Rows == 1 || Cols == 1 );
+    ValueT r = ValueT();
+    ValueT t = ValueT();
+    for( size_t row = 0; row < Rows; ++row )
+    {
+        for( size_t col = 0; col < Cols; ++col )
+        {
+            t = a( row, col ) - b( row, col );
+            r += t*t;
+        }
+    }
+    return r;
+}
+
+/**
+ * Calculates the <b>squared</b> distance between two vectors.
+ *
+ * \tparam ValueT Value type
+ * \tparam ValueStoreT Value store to use
+ * \tparam Rows number of rows in this vector, either this or Cols should be 1
+ * \tparam Cols number of cols in this vector, either this or Rows should be 1
+ * \param a the first vector
+ * \param b the second vector
+ *
+ * \return the distance between the two vectors
+ */
+template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows, size_t Cols >
+ValueT distance( const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& a, const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& b )
+{
+    return sqrt( distance2( a, b ) );
+}
+
 /**
  * Normalizes the given vector.
  *

src/core/common/math/linearAlgebra/test/WMatrixFixed_test.h

@@ -682,6 +682,36 @@ public:
         TS_ASSERT_DELTA( length( transpose( vec ) ), sqrt( 3.0 ), 1e-10 );
     }
 
+    /**
+     * Test vector distance.
+     */
+    void testDistance()
+    {
+        WVector3d vec1;
+        vec1[ 0 ] = 0.0;
+        vec1[ 1 ] = 4.0;
+        vec1[ 2 ] = 3.0;
+
+        WVector3d vec2;
+        vec2[ 0 ] = 0.0;
+        vec2[ 1 ] = 0.0;
+        vec2[ 2 ] = 0.0;
+
+        TS_ASSERT_DELTA( distance( vec1, vec2 ), 5.0, 1e-10 );
+        TS_ASSERT_DELTA( distance( transpose( vec1 ), transpose( vec2 ) ), 5.0, 1e-10 );
+
+        vec1[ 0 ] = 0.0;
+        vec1[ 1 ] = 4.0;
+        vec1[ 2 ] = 3.0;
+
+        vec2[ 0 ] = 0.0;
+        vec2[ 1 ] = 1.0;
+        vec2[ 2 ] = 4.0;
+
+        TS_ASSERT_DELTA( distance( vec1, vec2 ), sqrt( 10.0 ), 1e-10 );
+        TS_ASSERT_DELTA( distance( transpose( vec1 ), transpose( vec2 ) ), sqrt( 10.0 ), 1e-10 );
+    }
+
     /**
      * Test vector normalization.
      */