Line data    Source code

       1             : #include "dist.h"
       2             : #include <tgmath.h>
       3             : #include "mathext.h"
       4             : #define swap(T, x, y) \
       5             :   {                   \
       6             :     T tmp = (x);      \
       7             :     x = (y);          \
       8             :     y = tmp;          \
       9             :   }
      10             : 
      11             : #define map1(i) (i) - 1
      12             : #define x(i) x[ map1(i) ]
      13             : #define y(i) y[ map1(i) ]
      14             : 
      15             : // Computes either the Euclidean distance between two N-dimensional
      16             : // vectors X and Y or the Euclidean norm of one such vector X.
      17             : //
      18             : // Call the routine
      19             : // either
      20             : //    with KVEC = 2 and two vectors X and Y of dimension N stored
      21             : //    with storage-increments INCX and INCY, respectively.
      22             : // or
      23             : //    with KVEC = 1 and one vector X of dimension N stored
      24             : //    with storage-increment INCX. In this case the second array
      25             : //    is not referenced and can be a dummy array or simply the
      26             : //    array X again.
      27             : //
      28             : // If N .LE. 0 then zero is returned, if N .GE. 1 then the
      29             : // storage increments cannot be zero.
      30             : //
      31             : // This code is inspired by the LAPACK function DNRM2 written by
      32             : // Sven Hammarling.
      33             : //
      34             : // Variables in the calling sequence
      35             : // ---------------------------------
      36             : //    N     I    IN   Dimension of the vectors X and Y
      37             : //    X     D    IN   The first vector of dimension N
      38             : //    INCX  I    IN   Storage increment of X
      39             : //    Y     D    IN   The second vector of dimension N
      40             : //    INYY  D    IN   Storage increment of Y
      41             : //    KVEC  I    IN   Number of vectors
      42             : //                    KVEC = ANY INTEGER OTHER THAN 2
      43             : //                              Only one vector, namely X, is given,
      44             : //                              the Euclidean norm of X is computed
      45             : //                              and the Y array is not referenced
      46             : //                              This is the default
      47             : //                    KVEC = 2  Two vectors X and Y  are given,
      48             : //                              the Euclidean distance between
      49             : //                              X and Y is computed
      50             : //
      51          19 : double ddist2(int n, double *x, int incx, double *y, int incy, int kvec)
      52             : {
      53          19 :   const double zer = 0.0;
      54          19 :   int ix = 1, iy = 1;
      55          19 :   double diff = 0.0, dx = 0.0, dy = 0.0;
      56          19 :   double scale = 0.0, sum = 0.0;
      57             : 
      58          19 :   if (kvec != 2) kvec = 1;
      59             : 
      60             :   // Define the inner product to be zero whenever the dimension is not positive
      61             :   // or a storage incrementor (incx/incy) is zero
      62          19 :   if (n <= 0) return zer;
      63          16 :   if (kvec == 1 && incx == 0) return zer;
      64          15 :   if (kvec == 2 && incx == 0 && incy == 0) return zer;
      65             : 
      66             :   // Initialize incrementors
      67             : 
      68          14 :   ix = 1;
      69          14 :   if (incx < 0) ix = (-n + 1) * incx + 1;
      70          14 :   iy = 1;
      71          14 :   if (incy < 0) iy = (-n + 1) * incy + 1;
      72             : 
      73          14 :   sum = zer;
      74          14 :   scale = zer;
      75         145 :   for (int j = 1; j <= n; j++)
      76             :   {
      77         131 :     dx = x(ix);
      78         131 :     ix += incx;
      79         131 :     if (kvec == 1)  // One vector case
      80             :     {
      81          72 :       diff = dx;
      82             :     }
      83             :     else  // Two vector case
      84             :     {
      85          59 :       dy = y(iy);
      86          59 :       iy += incy;
      87          59 :       diff = dx - dy;
      88             :     }
      89             : 
      90             :     // Scaled squares (less than or equal to one) of the nonzero terms.
      91         131 :     add_next_element_squared(diff, &sum, &scale);
      92             :   }
      93          14 :   return scale * sqrt(sum);
      94             : }
      95             : 
      96             : // Computes the Euclidean norm of X.
      97             : //
      98             : // If N .LE. 0 then zero is returned, if N .GE. 1 then the
      99             : // storage increments cannot be zero.
     100             : //
     101             : // This code is inspired by the LAPACK function DNRM2 written by
     102             : // Sven Hammarling.
     103             : //
     104             : // Variables in the calling sequence
     105             : // ---------------------------------
     106             : //    N     I    IN   Dimension of the vector X
     107             : //    X     D    IN   Vector of dimension N
     108             : //    INCX  I    IN   Storage spacing between elements of X
     109             : //
     110         236 : double dnrm2(int n, double *x, int incx)
     111             : {
     112         236 :   const double zer = 0.0;
     113         236 :   int ix = 1;
     114         236 :   double dx = 0.0;
     115         236 :   double scale = 0.0, sum = 0.0;
     116             : 
     117             :   // Define the norm2 to be zero whenever the dimension is not positive
     118             :   // or a storage incrementor is zero
     119         236 :   if (n < 1 || incx == 0) return zer;
     120             : 
     121             :   // Initialize incrementors
     122         232 :   if (incx < 0) ix = (-n + 1) * incx + 1;
     123             : 
     124        1308 :   for (int j = 1; j <= n; j++)
     125             :   {
     126        1076 :     dx = x(ix);
     127        1076 :     ix += incx;
     128             :     // Scaled squares (less than or equal to one) of the nonzero terms.
     129        1076 :     add_next_element_squared(dx, &sum, &scale);
     130             :   }
     131         232 :   return scale * sqrt(sum);
     132             : }
     133             : 
     134        1207 : void add_next_element_squared(double xi, double *sum, double *scale)
     135             : {
     136        1207 :   double bar = *scale;
     137        1207 :   double ssq = *sum;
     138        1207 :   const double zer = 0.0, one = 1.0;
     139        1207 :   double tmp = 0.0, absxi = 0.0;
     140             : 
     141             :   // If xi==0, then the remaining statements do not permit a division by zero
     142             :   // because bar >= 0 implies that bar < 0 is always false.
     143             :   // However a quick return prohibits a wasted multiplication and sum.
     144        1207 :   if (xi == zer) return;
     145             : 
     146        1046 :   absxi = fabs(xi);
     147        1046 :   if (bar < absxi)
     148             :   {
     149         594 :     tmp = bar / absxi;
     150         594 :     ssq = one + ssq * tmp * tmp;
     151         594 :     bar = absxi;
     152             :   }
     153             :   else
     154             :   {
     155         452 :     tmp = absxi / bar;
     156         452 :     ssq += tmp * tmp;
     157             :   }
     158             : 
     159        1046 :   *scale = bar;
     160        1046 :   *sum = ssq;
     161             : }