#pragma ident "MRC HGU $Id$"
/************************************************************************
* Project:      Mouse Atlas
* Title:        AlgMatrixSV.c
* Date:         March 1999
* Author:       Bill Hill
* Copyright:	1999 Medical Research Council, UK.
*		All rights reserved.
* Address:	MRC Human Genetics Unit,
*		Western General Hospital,
*		Edinburgh, EH4 2XU, UK.
* Purpose:	Provides functions for singular value decomposition.
*		decomposition for the MRC Human Genetics Unit
*		numerical algorithm library.
* $Revision$
* Maintenance:  Log changes below, with most recent at top of list.
************************************************************************/
#include <Alg.h>
#include <float.h>


static double	AlgMatrixSVPythag(double, double);

/************************************************************************
* Function:	AlgMatrixSVSolve					*
* Returns:	AlgError:		Error code.			*
* Purpose:	Solves the matrix equation A.x = b for x, where A is a	*
*		matrix with at least as many columns as rows.		*
*		On return the matrix A is overwritten by the matrix U	*
*		in the singular value decomposition:			*
*		  A = U.W.V'						*
* Global refs:	-							*
* Parameters:	double **aMat:		Matrix A.			*
*		int nM:			Number of rows in matrix A.	*
*		int nN:			Number of columns in matrix A.  *
*		double *bMat:		Column matrix b, overwritten	*
*					by matrix x on return.		*
*		double tol:		Tolerance for singular values,	*
*					1.0e-06 should be suitable as	*
*					a default value.		*
************************************************************************/
AlgError	AlgMatrixSVSolve(double **aMat, int nM, int nN,
				 double *bMat, double tol)
{
  int		cnt0;
  double	thresh,
  		wMax;
  double	*tDP0,
  		*wMat = NULL;
  double	**vMat = NULL;
  AlgError	errCode = ALG_ERR_NONE;

  if((aMat == NULL) || (bMat == NULL) || (nM <= 0) || (nM < nN))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if(((wMat = (double *)AlcCalloc(sizeof(double), nN)) == NULL) ||
          (AlcDouble2Malloc(&vMat, nM, nN) != ALC_ER_NONE))
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    errCode = AlgMatrixSVDecomp(aMat, nM, nN, wMat, vMat);
  }
  if(errCode == ALG_ERR_NONE)
  {
    /* Find maximum singular value. */
    wMax = 0.0;
    cnt0 = nN;
    tDP0 = wMat;
    while(cnt0-- > 0)
    {
      if(*tDP0 > wMax)
      {
        wMax = *tDP0;
      }
      ++tDP0;
    }
    /* Edit the singular values, replacing any less than tol * max singular
       value with 0.0. */
    cnt0 = nN;
    tDP0 = wMat;
    thresh = tol * wMax;
    while(cnt0-- > 0)
    {
      if(*tDP0 < thresh)
      {
	*tDP0 = 0.0;
      }
      ++tDP0;
    }
    errCode = AlgMatrixSVBackSub(aMat, nM, nN, wMat, vMat, bMat);
  }
  if(wMat)
  {
    AlcFree(wMat);
  }
  if(vMat)
  {
    AlcDouble2Free(vMat);
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVSolve FX %d\n",
	   (int )errCode));
  return(errCode);
}

/************************************************************************
* Function:	AlgMatrixSVDecomp					*
* Returns:	AlgError:		Error code.			*
* Purpose:	Performs singular value decomposition of the given	*
*		matrix (A) and computes two additional matricies 	*
*		(U and V) such that:					*
*		  A = U.W.V'						*
*		where V' is the transpose of V.				*
*		The code for AlgMatrixSVDecomp was derived from:	*
*		Numerical Recipies function svdcmp(), EISPACK		*
*		subroutine SVD(), CERN subroutine SVD() and ACM		*
*		algorithm 358.						*
*		See AlgMatrixSVSolve() for a usage example.		*
* Global refs:	-							*
* Parameters:	double **aMat:		The given matrix A, and U on	*
*					return.				*
*		int nM:			Number of rows in matrix A.	*
*		int nN:			Number of columns in matrix A.	*
*		double *wMat:		The diagonal matrix of singular	*
*					values, returned as a vector.	*	
*		double **vMat:		The matrix V (not it's 		*
*					transpose).			*
************************************************************************/
AlgError	AlgMatrixSVDecomp(double **aMat, int nM, int nN,
				  double *wMat, double **vMat)
{
  int		cnt0,
  		flag,
  		idI,
		idJ,
		idK,
		idL,
		idM,
		its,
		nNL,
		nMI,
		nNM;
  double 	tD0,
  		c,
 		f,
		h,
		s,
		x,
		y,
		z,
		aNorm = 0.0,
		g = 0.0,
		scale = 0.0;
  double	*tDP0,
		*tDP1,
  		*tDVec = NULL;
  double	**tDPP0;
  const int	maxIts = 100;  /* Maximum iterations to find singular value. */
  AlgError	errCode = ALG_ERR_NONE;

  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVDecomp FE 0x%lx %d %d 0x%lx 0x%lx\n",
	   (unsigned long )aMat, nM, nN, (unsigned long )wMat,
	   (unsigned long )vMat));
  if((aMat == NULL) || (wMat == NULL) || (vMat == NULL) ||
     (nM <= 0) || (nM < nN))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((tDVec = (double *)AlcCalloc(sizeof(double), nN)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    /* Householder reduction to bidiagonal form */
    for(idI = 0; idI < nN; ++idI)
    {
      idL = idI + 1;
      nNL = nN - idL;
      nMI = nM - idI;
      tDVec[idI] = scale * g;
      g = s = scale = 0.0;
      if(idI < nM)
      {
        cnt0 = nMI;
	tDPP0 = aMat + idI;
	while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	{
	  tD0 = *(*tDPP0++ + idI);
	  scale += fabs(tD0);	/* scale += fabs(aMat[idK][idI]); */
	}
	if(scale > DBL_EPSILON)			   /* scale must always >= 0 */
	{
	  cnt0 = nMI;
	  tDPP0 = aMat + idI;
	  while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	  {
	    tDP0 = *tDPP0++ + idI;
	    *tDP0 /= scale;	/* aMat[idK][idI] /= scale; */
	    s += *tDP0 * *tDP0;	/* s += aMat[idK][idI] * aMat[idK][idI]; */
	  }
	  f = aMat[idI][idI];
	  g = (f > 0.0)? -(sqrt(s)): sqrt(s);
	  h = (f * g) - s;
	  aMat[idI][idI] = f - g;
	  if(idI != (nN - 1))
	  {
	    for(idJ = idL; idJ < nN; ++idJ)
	    {
	      s = 0.0;
	      cnt0 = nMI;
	      tDPP0 = aMat + idI;
	      while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	      {
	        s += *(*tDPP0 + idI) * *(*tDPP0 + idJ);
		++tDPP0;	/* s += aMat[idK][idI] * aMat[idK][idJ]; */
	      }
	      f = s / h;
	      cnt0 = nMI;
	      tDPP0 = aMat + idI;
	      while(cnt0-- > 0) /* for(idK = idI; idK < nM; ++idK) */
	      {
	        *(*tDPP0 + idJ) += f * *(*tDPP0 + idI);
		++tDPP0;	/* aMat[idK][idJ] += f * aMat[idK][idI]; */
	      }
	    }
	  }
	  cnt0 = nMI;
	  tDPP0 = aMat + idI;
	  while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	  {
	    *(*tDPP0++ + idI) *= scale; /* aMat[idK][idI] *= scale; */
	  }
	}
      }
      wMat[idI] = scale * g;
      g = s = scale = 0.0;
      if((idI < nM) && (idI != (nN - 1)))
      {
	cnt0 = nNL;
	tDP0 = *(aMat + idI) + idL;
	while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	{
	  scale += fabs(*tDP0++); /* scale += fabs(aMat[idI][idK]); */
	}
	if(scale > DBL_EPSILON)				/* scale always >= 0 */
	{
	  cnt0 = nNL;
	  tDP0 = *(aMat + idI) + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP0 /= scale;	/* aMat[idI][idK] /= scale; */
	    tD0 = *tDP0++;
	    s += tD0 * tD0;	/* s += aMat[idI][idK] * aMat[idI][idK]; */
	  }
	  f = aMat[idI][idL];
	  g = (f > 0.0)? -(sqrt(s)): sqrt(s);
	  h = (f * g) - s;
	  aMat[idI][idL] = f - g;
	  cnt0 = nNL;
	  tDP0 = *(aMat + idI) + idL;
	  tDP1 = tDVec + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP1++ = *tDP0++ / h; /* tDVec[idK] = aMat[idI][idK] / h; */
	  }
	  if(idI != (nM - 1))
	  {
	    for(idJ = idL; idJ < nM; ++idJ)
	    {
	      s = 0.0;
	      cnt0 = nNL;
	      tDP0 = *(aMat + idI) + idL;
	      tDP1 = *(aMat + idJ) + idL;
	      while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	      {
	        s += *tDP1++ * *tDP0++; /* s += aMat[idJ][idK] *
					        aMat[idI][idK]; */
	      }
	      cnt0 = nNL;
	      tDP0 = tDVec + idL; 
	      tDP1 = *(aMat + idJ) + idL;
	      while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	      {
	        *tDP1++ += s * *tDP0++; /* aMat[idJ][idK] += s * tDVec[idK]; */
	      }
	    }
	  }
	  cnt0 = nNL;
	  tDP0 = *(aMat + idI) + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP0++ *= scale;	/* aMat[idI][idK] *= scale; */
	  }
	}
      }
      if((tD0 = fabs(wMat[idI]) + fabs(tDVec[idI])) > aNorm)
      {
        aNorm = tD0;
      }
    }
    /* Accumulate right-hand transformations. */
    for(idI = nN - 1; idI >= 0; --idI)
    {
      if(idI < (nN - 1))
      {
	nNL = nN - idL;
	if(fabs(g) > DBL_EPSILON)
	{
	  cnt0 = nNL;
	  tDP0 = *(aMat + idI) + idL;
	  tD0 = *tDP0;
	  tDPP0 = vMat + idL;
	  while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	  {
	    			/* vMat[idJ][idI] = (aMat[idI][idJ] /
				 		     aMat[idI][idL]) /g */
	    *(*tDPP0++ + idI) = (*tDP0++ / tD0) / g; /* Double division to try
	    						and avoid underflow */
	  }
	  for(idJ = idL; idJ < nN; ++idJ)
	  {
	    s = 0.0;
	    cnt0 = nNL;
	    tDP0 = *(aMat + idI) + idL;
	    tDPP0 = vMat + idL;
	    while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	    {
	      s += *tDP0++ * *(*tDPP0++ + idJ); /* s += aMat[idI][idK] *
	      						vMat[idK][idJ]; */
	    }
	    cnt0 = nNL;
	    tDPP0 = vMat + idL;
	    while(cnt0-- > 0)	
	    {
	      tDP0 = *tDPP0++;
	      *(tDP0 + idJ) += s * *(tDP0 + idI); /* vMat[idK][idJ] += s *
	      						     vMat[idK][idI]; */
	    }
	  }
	}
	cnt0 = nNL;
	tDP0 = *(vMat + idI) + idL;
	tDPP0 = vMat + idL;
	while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	{
	  *tDP0++ = 0.0;	   /* vMat[idI][idJ] = 0.0 */
	  *(*tDPP0++ + idI) = 0.0; /* vMat[idJ][idI] = 0.0; */
	}
      }
      vMat[idI][idI] = 1.0;
      g = tDVec[idI];
      idL = idI;
    }
    /* Accumulate left-hand transformations. */
    for(idI = nN - 1; idI >= 0; --idI)
    {
      idL = idI + 1;
      nNL = nN - idL;
      nMI = nM - idI;
      g = wMat[idI];
      if(idI < (nN - 1))
      {
	cnt0 = nNL;
	tDP0 = *(aMat + idI) + idL;
	while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	{
	  *tDP0++ = 0.0;	/* aMat[idI][idJ] = 0.0; */
	}
      }
      if(fabs(g) > DBL_EPSILON)
      {
	g = 1.0 / g;
	if(idI != (nN - 1))
	{
	  for(idJ = idL; idJ < nN; ++idJ)
	  {
	    s = 0.0;
	    cnt0 = nMI - 1;
	    tDPP0 = aMat + idL;
	    while(cnt0-- > 0)	/* for(idK = idL; idK < nM; ++idK) */
	    {
	      tDP0 = *tDPP0++;
	      s +=  *(tDP0 + idI) * *(tDP0 + idJ); /* s += aMat[idK][idI] * 
	      						   aMat[idK][idJ]; */
	    }
	    f = (s / aMat[idI][idI]) * g;
	    cnt0 = nMI;
	    tDPP0 = aMat + idI;
	    while(cnt0-- > 0)   /* for(idK = idI; idK < nM; ++idK) */
	    {
	      tDP0 = *tDPP0++;
	      *(tDP0 + idJ) += f * *(tDP0 + idI);
	    }
	  }
	}
	cnt0 = nMI;
	tDPP0 = aMat + idI;
	while(cnt0-- > 0)   /* for(idJ = idI; idJ < nM; ++idJ) */
	{
	  *(*tDPP0++ + idI) *= g; /* aMat[idJ][idI] *= g; */
	}
      } 
      else
      {
	cnt0 = nMI;
	tDPP0 = aMat + idI;
	while(cnt0-- > 0)	/* for(idJ = idI; idJ < nM; ++idJ) */
	{
	  *(*tDPP0++ + idI) = 0.0; /* aMat[idJ][idI] = 0.0; */
	}
      }
      aMat[idI][idI] += 1.0;
    }
    /* Diagonalize the bidiagonal form. */
    for(idK = nN - 1; (idK >= 0) && (errCode == ALG_ERR_NONE); --idK)
    {
      for(its = 1; its <= maxIts; ++its)
      {
	flag = 1;
	for(idL = idK; idL >= 0; --idL)
	{
	  nNM = idL - 1;
	  if(fabs((tDVec[idL]) + aNorm) == aNorm)
	  {
	    flag = 0;
	    break;
	  }
	  if((fabs(wMat[nNM]) + aNorm) == aNorm)
	  {
	    break;
	  }
	}
	if(flag)
	{
	  c = 0.0;
	  s = 1.0;
	  for(idI = idL; idI <= idK; ++idI)
	  {
	    f = s * tDVec[idI];
	    if(fabs(f) + aNorm != aNorm)
	    {
	      g = wMat[idI];
	      h = AlgMatrixSVPythag(f, g);
	      wMat[idI] = h;
	      h = 1.0 / h;
	      c = g * h;
	      s = (-f * h);
	      cnt0 = nM;
	      tDPP0 = aMat;
	      while(cnt0-- > 0)	/* for(idJ = 0; idJ < nM; ++idJ) */
	      {
	        tDP0 = *tDPP0 + nNM;
		tDP1 = *tDPP0++ + idI;
		y = *tDP0;	/* y = aMat[idJ][nNM]; */
		z = *tDP1;	/* z = aMat[idJ][idI]; */
		*tDP0 = (y * c) + (z * s); /* aMat[idJ][nNM] = (y * c) +
							       (z * s); */
		*tDP1 = (z * c) - (y * s); /* aMat[idJ][idI] = (z * c) -
							       (y * s); */
	      }
	    }
	  }
	}
	/* Test for convergence. */
	z = wMat[idK];
	if(idL == idK)
	{
	  if(z < 0.0)
	  {
	    wMat[idK] = -z;
	    cnt0 = nN;
	    tDPP0 = vMat;
	    while(cnt0-- > 0)	/* for(idJ = 0; idJ < nN; ++idJ) */
	    {
	      tDP0 = *tDPP0++ + idK;
	      *tDP0 = -*tDP0; /* vMat[idJ][idK] = -vMat[idJ][idK]; */
	    }
	  }
	  break;
	}
	if(its >= maxIts)
	{
	  errCode = ALG_ERR_SINGULAR;
	}
	else
	{
	  x = wMat[idL];
	  nNM = idK - 1;
	  y = wMat[nNM];
	  g = tDVec[nNM];
	  h = tDVec[idK];
	  f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
	  g = AlgMatrixSVPythag(f, 1.0);
	  f = (((x - z) * (x + z)) + 
	      (h * ((y / (f + ((f > 0)? g: -g))) - h))) / x;
	  c = s = 1.0;
	  for(idJ = idL; idJ <= nNM; ++idJ)
	  {
	    idI = idJ + 1;
	    g = tDVec[idI];
	    y = wMat[idI];
	    h = s * g;
	    g = c * g;
	    z = AlgMatrixSVPythag(f, h);
	    tDVec[idJ] = z;
	    c = f / z;
	    s = h / z;
	    f = (x * c) + (g * s);
	    g = (g * c) - (x * s);
	    h = y * s;
	    y = y * c;
	    cnt0 = nN;
	    tDPP0 = vMat;
	    while(cnt0-- > 0)	/* for(idM = 0; idM < nN; ++idM) */
	    {
	      tDP0 = *tDPP0 + idJ;
	      tDP1 = *tDPP0++ + idI;
	      x = *tDP0; 	/* x = vMat[idM][idJ]; */
	      z = *tDP1;	/* z = vMat[idM][idI]; */
	      *tDP0 = (x * c) + (z * s); /* vMat[idM][idJ] = (x * c) +
	      						     (z * s); */
	      *tDP1 = (z * c) - (x * s); /* vMat[idM][idI] = (z * c) -
	      						     (x * s); */
	    }
	    z = AlgMatrixSVPythag(f, h);
	    wMat[idJ] = z;
	    if(z > DBL_EPSILON)			       /* Can only be >= 0.0 */
	    {
	      z = 1.0 / z;
	      c = f * z;
	      s = h * z;
	    }
	    f = (c * g) + (s * y);
	    x = (c * y) - (s * g);
	    cnt0 = nM;
	    tDPP0 = aMat;
	    while(cnt0-- > 0)   /* for(idM = 0; idM < nM; ++idM) */
	    {
	      tDP0 = *tDPP0 + idJ;
	      tDP1 = *tDPP0++ + idI;
	      y = *tDP0;	/* y = aMat[idM][idJ]; */
	      z = *tDP1;	/* z = aMat[idM][idI]; */
	      *tDP0 = (y * c) + (z * s); /* aMat[idM][idJ] = (y * c) +
	      						     (z * s); */
	      *tDP1 = (z * c) - (y * s); /* aMat[idM][idI] = (z * c) -
	      						     (y * s); */
	    }
	  }
	  tDVec[idL] = 0.0;
	  tDVec[idK] = f;
	  wMat[idK] = x;
        }
      }
    }
    AlcFree(tDVec);
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVDecomp FX %d\n",
	   (int )errCode));
  return(errCode);
}

/************************************************************************
* Function:	AlgMatrixSVBackSub					*
* Returns:	AlgError:		Error code.			*
* Purpose:	Solves the set of of linear equations A.x = b where	*
*		A is input as its singular value decomposition in	*
*		the three matricies U, W and V, as returned by		*
*		AlgMatrixSVDecomp().					*
*		The code for AlgMatrixSVBackSub was derived from:	*
*		Numerical Recipies function svbksb().			*
* Global refs:	-							*
* Parameters:	double **uMat:		Given matrix U.			*
*		int nM:			Number of rows in matrix U and	*
*					number of elements in matrix B.	*
*		int nN:			Number of columns in matricies	*
*					U and V, also the number of	*
*					elements in matricies W and x.	*
*		double *wMat:		The diagonal matrix of singular	*
*					values, returned as a vector.	*	
*		double **vMat:		The matrix V (not it's 		*
*					transpose).			*
*		double *bMat:		Column matrix b, overwritten by	*
*					column matrix x on return.	*
************************************************************************/
AlgError	AlgMatrixSVBackSub(double **uMat, int nM, int nN,
				   double *wMat, double **vMat,
				   double *bMat)
{
  int		cnt0,
  		idJ;
  double	s;
  double	*tDP0,
		*tDP1,
  		*tDVec = NULL;
  double	**tDPP0;
  AlgError	errCode = ALG_ERR_NONE;

  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVBackSub FE 0x%lx %d %d 0x%lx 0x%lx 0x%lx\n",
	   (unsigned long )uMat, nM, nN,
	   (unsigned long )wMat, (unsigned long)vMat,
	   (unsigned long )bMat));

  if((uMat == NULL) || (wMat == NULL) || (vMat == NULL) ||
     (bMat == NULL) || (vMat == NULL) || (nM <= 0) || (nM < nN))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((tDVec = (double *)AlcCalloc(sizeof(double), nN)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    for(idJ = 0; idJ < nN; ++idJ) 
    {
      s = 0.0;
      if(fabs(wMat[idJ]) > DBL_EPSILON)
      {
	cnt0 = nM;
	tDPP0 = uMat;
	tDP0 = bMat;
	while(cnt0-- > 0)	/* for(idI = 0; idI < nM; ++idI) */
	{
	  s += *(*tDPP0++ + idJ) * *tDP0++; /* s += uMat[idI][idJ] *
	  					    bMat[idI]; */
	}
	s /= wMat[idJ];
      }
      tDVec[idJ] = s;
    }
    for(idJ = 0; idJ < nN; ++idJ) 
    {
      s = 0.0;
      cnt0 = nN;
      tDP0 = *(vMat + idJ);
      tDP1 = tDVec;
      while(cnt0-- > 0)		/* for(idI = 0; idI < nN; ++idI) */
      {
        s += *tDP0++ * *tDP1++; /* s += vMat[idJ][idI] * tDVec[idI]; */
      }
      bMat[idJ] = s;
    }
    AlcFree(tDVec);
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVBackSub FX %d\n",
	   (int )errCode));
  return(errCode);
}

/************************************************************************
* Function:	AlgMatrixSVPythag					*
* Returns:	double:			Square root of sum of squares.	*
* Purpose:	Computes sqrt(size0^2 + size1^2) without underflow or	*
*		overflow.						*
* Global refs:	-							*
* Parameters:	double side0:		Length of first side.		*
*		double side1:		Length of second side1.		*
************************************************************************/
static double	AlgMatrixSVPythag(double sd0, double sd1)
{
  double	tD0,
		abs0,
		abs1,
		hyp = 0.0;

  if((abs0 = fabs(sd0)) > (abs1 = fabs(sd1)))
  {
    tD0 = abs1 / abs0;
    hyp = abs0 * sqrt(1.0 + (tD0 * tD0));
  }
  else if(abs0 > DBL_EPSILON)
  {
    tD0 = abs0 / abs1;
    hyp = abs1 * sqrt(1.0 + (tD0 * tD0));
  }
  return(hyp);
}