Initial commit.

1 files changed, 362 insertions, 0 deletions

diff --git a/sources/algebra.cpp b/sources/algebra.cpp
new file mode 100644
index 0000000..b927435
--- /dev/null
+++ b/sources/algebra.cpp
@@ -0,0 +1,362 @@
+//-*- mode: c++; indent-tabs-mode: t; coding: utf-8; show-trailing-whitespace: t -*-
+
+// file algebra.cpp
+
+#include "algebra.hpp"
+
+namespace algebra {
+
+  // subroutine cdiz.
+  
+  bool cdivz (double *ar, double *ai, const double &br, const double &bi, const double &cr, const double &ci, const int &nKSn)
+  {
+    bool bReturnValue = false;
+    double nFAC;
+    double nSR;
+    //
+    if (ar && ai) {
+      nFAC = cr * cr + ci * ci;
+      nFAC = 1.0 / nFAC;
+      nSR = br * nFAC * cr + bi * nFAC * ci;
+      *ai = bi * nFAC * cr - br * nFAC * ci;
+      *ar = nSR;
+      if (nKSn < 0) {
+	*ar = -*ar;
+	*ai = -*ai;
+      }
+      bReturnValue = true;
+    }
+    return bReturnValue;
+  }
+
+  // subroutine cmultz.
+  
+  bool cmultz (double *ar, double *ai, const double &br, const double &bi, const double &cr, const double &ci, const int &nKSn)
+  {
+    bool bReturnValue = false;
+    //
+    if (ar && ai) {
+      *ar = br * cr - bi * ci;
+      *ai = bi * cr - br * ci;
+      if (nKSn < 0) {
+	*ar = -*ar;
+	*ai = -*ai;
+      }
+    }
+    return bReturnValue;
+  }
+
+  // subroutine trgwnd.
+  
+  bool trgwnd (const double &x, double *d17)
+  {
+    bool bReturnValue = false;
+    int n13;
+    //
+    if (d17) {
+      *d17 = x;
+      if (fabs(x) >= 25000.0) {
+	n13 = x / (2.0 * M_PI);
+	*d17 -= 2.0 * n13 * M_PI;
+	if (blkcom::nIprsUp >= 1)
+	  *((std::ostream *) blkcom::pLFiles[ 5 ]) << " Angle unwind in \"trgwnd\" called by \"rfunl1\".    nchain, x, d17 =" << blkcom::nChain << x << *d17 << std::endl;
+      }
+      bReturnValue = true;
+    }
+    return bReturnValue;
+  }
+
+  // subroutine multmx.
+  bool multmx(std::vector<double> &sA,		\
+	      std::vector<double> &sB,		\
+	      std::vector<double> &sC,		\
+	      std::vector<double> &sTemp,	\
+	      const size_t &n)
+  {
+    // Subroutine multmx  forms the matrix product   (c) = (a)(b)   where
+    // matrices  (a), (b), and (c)  are all  n by n  square arrays.
+    // Array  'temp'  is a scratch working area of not less than  2n
+    // cells.   Arrays  (b)  and  (c)  may be identical, thereby placing
+    // the product   (a)(b)   back into  (b) .    See subroutine  'mult'
+    // which is called herein, for details about the storage scheme used
+    // for these real, symmetric matrices.
+    bool bReturnValue = false;
+    long int l, ll0;
+    size_t i, ii;
+    size_t j;
+    size_t m;
+    //
+    ll0 = 0;
+    ii = 0;
+    for(j = 1; j <= n; j++) {
+      for(i = 1; i <= n; i++) {
+	if(i > j) {
+	  l += (i - 1);
+	} else {
+	  l = ii + i;
+	}
+	sTemp[ i - 1 ] = sB[ l - 1 ];
+      }
+      m = n + i;
+      std::vector<double> sTempX(sTemp.begin(), sTemp.begin() + n);
+      std::vector<double> sTempY(sTemp.begin() + m - 1, sTemp.begin() + n);
+      bReturnValue = algebra::mult(sA,					\
+				   sTempX,				\
+				   sTempY,				\
+				   n,					\
+				   ll0) &&				\
+	movecopy::move(sTemp.data() + m - 1, sC.data() + ii, j);
+      ii += j;
+    }
+    return bReturnValue;
+  }
+
+  // subroutine mult.
+  bool mult(std::vector<double> &sA, \
+	    std::vector<double> &sX, \
+	    std::vector<double> &sY, \
+	    const size_t &n,	     \
+	    long int &icheck)
+  {
+    bool bReturnValue = false;
+    double xx;
+    double yy;
+    size_t i, ii;
+    size_t k;
+    //
+    ii = 0;
+    k = 0;
+    FOREVER {
+      ++k;
+      if(k > n)
+	break;
+      xx = sX[ k - 1 ];
+      yy = sY[ k - 1 ];
+      if(icheck == 0)
+	yy = 0.0;
+      if(icheck < 0)
+	yy = -yy;
+      for(i = 1; i <= k; i++) {
+	  ++ii;
+	  sY[ i - 1 ] += sA[ ii - 1 ] * xx;
+	  yy += sA[ ii - 1 ] * sX[ i - 1 ];
+	  sY[ k - 1 ] = yy;
+      }
+      bReturnValue = true;
+    }
+    return bReturnValue;
+  }
+  
+  // subroutine dgelg.
+  void dgelg(std::vector<double> &sR,		\
+	     std::vector<double> &sA,		\
+	     const size_t &m,			\
+	     const size_t &n,			\
+	     const float &eps,			\
+	     int &ier)
+  {
+    /*
+     * Purpose:
+     * to solve a general system of simultaneous linear equations.
+     *
+     * Usage:
+     *   call dgelg(r, a, m, n, eps, ier)
+     *
+     * Description of parameters:
+     *   r      - double precision m by n right hand side matrix
+     *            (destroyed). On return r contains the solutions
+     *            of the equations.
+     *   a      - double precision m by m coefficient matrix
+     *            (destroyed).
+     *   m      - the number of equations in the system.
+     *   n      - the number of right hand side vectors.
+     *   eps    - single precision input constant which is used as
+     *            relative tolerance for test on loss of
+     *            significance.
+     *   ier    - resulting error parameter coded as follows
+     *            ier=0  - no error,
+     *            ier=-1 - no result because of m less than 1 or
+     *                     pivot element at any elimination step
+     *                     equal to 0,
+     *            ier=k  - warning due to possible loss of signifi-
+     *                     cance indicated at elimination step k+1,
+     *                     where pivot element was less than or
+     *                     equal to the internal tolerance eps times
+     *                     absolutely greatest element of matrix a.
+     *
+     * Remarks:
+     *   Input matrices r and a are assumed to be stored columnwise
+     *   in m*n resp. m*m successive storage locations. On return
+     *   solution matrix r is stored columnwise too.
+     *   The procedure gives results if the number of equations m is
+     *   greater than 0 and pivot elements at all elimination steps
+     *   are different from 0. However warning ier=k - if given -
+     *   indicates possible loss of significance. In case of a well
+     *   scaled matrix a and appropriate tolerance eps, ier=k may be
+     *   interpreted that matrix a has the rank k. No warning is
+     *   given in case m=1.
+     *
+     * Method:
+     *   Solution is done by means of gauss-elimination with
+     *   complete pivoting.
+     */
+    size_t i, ii, ist, j, k, l, lend, ll, lst, mm, nm;
+    double piv, pivi, tb, tol;
+    /*
+     */
+    if(m <= 0)
+      goto a23;	  
+    ier = 0;
+    piv = 0.0;
+    mm = m * m;
+    nm = n * m;
+    for(l = 1; l <= mm; l++) {
+      tb = fabs(sA[ l - 1 ]);
+      if(tb > piv) {
+	piv = tb;
+	i = l;
+      }
+    }
+    tol = eps * piv;
+    lst = 1;
+    for(k = 1; k <= m; k++) {
+      if(piv <= 0.0)
+	goto a23;
+      if(ier != 0)
+	goto a7;
+      if(piv > tol)
+	goto a7;
+      ier = (long int) k - 1;
+
+    a7:
+      pivi = 1.0 / sA[ i - 1 ];
+      j = (i - 1) / m;
+      i = i - j * m - k;
+      j = j + 1 - k;
+      for(l = k; l <= nm; l += m) {
+	ll = l + i;
+	tb = pivi * sR[ ll - 1 ];
+	sR[ ll - 1 ] = sR[ l - 1 ];
+	sR[ l - 1 ] = tb;
+      }
+      if(k >= m)
+	goto a18;
+      lend = lst + m - k;
+      if(j <= 0)
+	goto a12;
+      ii = j * m;
+      for(l = lst; l <= lend; l++) {
+	tb = sA[ l - 1 ];
+	ll = l + ii;
+	sA[ l - 1 ] = sA[ ll - 1 ];
+	sA[ ll - 1 ] = tb;
+      }
+
+    a12:
+      for(l = lst; l <= mm; l += m) {
+	ll = l + i;
+	tb = pivi * sA[ ll - 1 ];
+	sA[ ll - 1 ] = sA[ l - 1 ];
+	sA[ l - 1 ] = tb;
+      }
+      sA[ lst - 1 ] = j;
+      piv = 0.0;
+      ++lst;
+      j = 0;
+      for(ii = lst; ii <= lend; ii++) {
+	pivi = -sA[ ii - 1 ];
+	ist = ii + m;
+	++j;
+	for(l = ist; l <= mm; l += m) {
+	  ll = l - j;
+	  sA[ l - 1 ] += pivi * sA[ ll - 1 ];
+	  tb = fabs(sA[ l - 1 ]);
+	  if(tb > piv) {
+	    piv = tb;
+	    i = l;
+	  }
+	}
+	for(l = k; l <= nm; l += m) {
+	  ll = l + j;
+	  sR[ ll - 1 ] += pivi * sR[ l - 1 ];
+	}
+      }
+      lst += m;
+    }
+    
+  a18:
+    if(m > 1)
+      goto a19;
+    if (m == 1)
+      goto a22;
+    goto a23;
+
+  a19:
+    ist = mm + m;
+    lst = m + 1;
+    for(i = 2; i <= m; i++) {
+      ii = lst - i;
+      ist -= lst;
+      l = ist - m;
+      l = (long int) (sA[ l - 1 ] + 0.5);
+      for(j = ii; j <= nm; j += m) {
+	tb = sR[ j - 1 ];
+	ll = j;
+	for(k = ist; k <= mm; k += mm) {
+	  ++ll;
+	  tb -= sA[ k - 1 ] * sR[ ll - 1 ];
+	}
+	k = j + l;
+	sR[ j - 1 ] = sR[ k - 1 ];
+	sR[ k - 1 ] = tb;
+      }
+    }
+
+  a22:
+    return;
+
+  a23:
+    ier = -1;
+  }
+
+  // subroutine matmul
+  void matmul (double aum[ 3 ][ 3 ], double bum[ 3 ][ 3 ])
+  {
+    size_t n1, n2, n3, n5;
+    double cum[ 3 ][ 3 ];
+    //
+    n5 = 3;
+    for(n1 = 1; n1 <= n5; n1++) {
+      for(n2 = 1; n2 <= n5; n2++) {
+	cum[ n1 - 1 ][ n2 - 1 ] = aum[ n1 - 1 ][ 0 ] * bum[ 0 ][ n2 - 1 ];
+	for (n3 = 2; n3 <= n5; n3++) {
+	  cum[ n1 - 1 ][ n2 - 1 ] = cum[ n1 - 1 ][ n2 - 1 ] + aum[ n1 - 1 ][ n3 - 1 ] * bum[ n3 - 1 ][ n2 - 1 ];
+	}
+      }
+    }
+    for(n1 =1; n1 <= n5; n1++) {
+      for(n2 = 1; n2 <= n5; n2++) {
+	aum[ n1 - 1 ][ n2 - 2 ] = cum[ n1 - 1 ][ n2 - 1 ];
+      }
+    }
+  }
+
+  // subroutine matvec.
+  void matvec(float aum[ 3 ][ 3 ], float yum[ 15 ])
+  {
+    size_t n1, n2, n3;
+    float x[ 3 ];
+    //
+    n1 = 3;
+    for (n2 = 1; n2 <= n1; n2++) {
+      for (n3 = 1; n3 <= n1; n3++) {
+	x[ n2 - 1 ] = x[ n2 - 1 ] + aum[ n2 - 1 ][ n3 - 1 ] * yum[ n3 - 1 ];
+      }
+    }
+    for (n2 = 1; n2 <= n1; n2++)
+      yum[ n2 - 1 ] = x[ n2 - 1 ];
+  }
+  
+}
+
+// end of file algebra.cpp