path: root/support/dktools/dk3mad.ctr

%%	options

copyright owner	=	Dirk Krause
copyright year	=	2014-xxxx
license		=	bsd



%%	header

/**	@file	dk3mad.h	Mathematical operations on double numbers.

Note: You should not use dk3ma_d_add_ok(), dk3ma_d_sub_ok(),
dk3ma_d_mul_ok(), dk3ma_d_div_ok() and dk3ma_d_div_ok()
for new programs.
Perform the calculations traditionally and use the
functions from the dk3mafpe module to check for floating point exceptions.

*/

#include <dk3conf.h>

#include <stdio.h>
#if DK3_HAVE_SYS_TYPES_H
#include <sys/types.h>
#endif
#if DK3_HAVE_STDINT
#include <stdint.h>
#endif
#if DK3_HAVE_INTTYPES_H
#include <inttypes.h>
#endif
#if DK3_HAVE_LIMITS_H
#include <limits.h>
#endif
#if DK3_HAVE_MATH_H
#include <math.h>
#endif
#if DK3_HAVE_FLOAT_H
#include <float.h>
#endif


#ifdef __cplusplus
extern "C" {
#endif

/**	Absolute value.
	@param	x	Original value.
	@return	Absolute value of x.
*/
double
dk3ma_d_abs(double x);

/**	Rounding to next integer.
	@param	x	Double value.
	@return	Value rounded to nearest integer.
*/
double
dk3ma_d_rint(double x);

/**	Addition.
	@param	a	Left operand.
	@param	b	Right operand.
	@param	ec	Pointer to error code variable, may be NULL.
	The variable may be set to DK3_ERROR_MATH_OVERFLOW when returning.
	@return	Summary of a and b.
*/
double
dk3ma_d_add_ok(double a, double b, int *ec);

/**	Substraction.
	@param	a	Left operand.
	@param	b	Right operand.
	@param	ec	Pointer to error code variable, may be NULL.
	The variable may be set to DK3_ERROR_MATH_OVERFLOW when returning.
	@return	Difference of a and b.
*/
double
dk3ma_d_sub_ok(double a, double b, int *ec);

/**	Multiplication.
	@param	a	Left operand.
	@param	b	Right operand.
	@param	ec	Pointer to error code variable, may be NULL.
	The variable may be set to DK3_ERROR_MATH_OVERFLOW when returning.
	@return	Product of a and b.
*/
double
dk3ma_d_mul_ok(double a, double b, int *ec);

/**	Division.
	@param	a	Left operand (nominator).
	@param	b	Right operand (denominator).
	@param	ec	Pointer to error code variable, may be NULL.
	The variable may be set to DK3_ERROR_MATH_OVERFLOW or
	DK3_ERROR_MATH_DIVZERO when returning.
	@return	Fraction of a and b.
*/
double
dk3ma_d_div_ok(double a, double b, int *ec);

/**	Square root.
	@param	x	Original value.
	@param	ec	Pointer to error code variable, may be NULL.
	The variable may be set to DK3_ERROR_MATH_OUT_OF_RANGE
	if the input is negative.
	@return	Square root of x.
*/
double
dk3ma_d_square_ok(double x, int *ec);

/**	Equality check.
	@param	a	Left operand.
	@param	b	Right operand.
	@param	epsilon	Maximum allowed difference between a and b.
	@return	1 for equal values, 0 for unequal values.
*/
int
dk3ma_d_equal(double a, double b, double epsilon);

/**	Arcus tangens for two lengths.
	@param	y	Y length (height).
	@param	x	X length (width).
	@return	Angle alpha for y/x = tan(alpha).
*/
double
dk3ma_d_atan2(double y, double x);

/**	Restrict number of digits following the decimal dot.
	@param	x	Original value.
	@param	n	Number of digits after decimal dot.
	@return	Rounded result.
*/
double
dk3ma_d_restrict_digits(double x, size_t n);

/**	Restrict number of digits following the decimal dot,
	round downwards.
	@param	x	Original value.
	@param	n	Number of digits after decimal dot.
	@return	Rounded result.
*/
double
dk3ma_d_restrict_digits_floor(double x, size_t n);

/**	Restrict number of digits following the decimal dot,
	round upwards.
	@param	x	Original value.
	@param	n	Number of digits after decimal dot.
	@return	Rounded result.
*/
double
dk3ma_d_restrict_digits_ceil(double x, size_t n);

#ifdef __cplusplus
}
#endif

%%	module

#include "dk3ma.h"
#include "dk3const.h"



$!trace-include



#if (DK3_SIZEOF_DOUBLE == 8) && (DK3_HAVE_IEEE_754_DOUBLE)
/**	Type to construct double value from binary (hex) data.
*/
typedef union {
  unsigned char c[sizeof(double)];	/**< Bytes to specify hex. */
  double d; 				/**< Double value to retrieve. */
} __dk3ma_max_double_t;

/**	Maximum double value.
*/
__dk3ma_max_double_t const __dk3ma_max_double = {
#if DK3_WORDS_BIGENDIAN
  { 0x7F, 0xEF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }
#else
  { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xEF, 0x7F }
#endif
};
/**	Maximum double value for internal use.
*/
#define	dk3ma_i_max_double	__dk3ma_max_double.d
#else
#ifndef DK3_MAX_DOUBLE
#define	DK3_MAX_DOUBLE 1.7e308
#endif
/**	Maximum double value for internal use.
*/
#define	dk3ma_i_max_double	DK3_MAX_DOUBLE
#endif



double
dk3ma_d_abs(double x)
{
#if DK3_HAVE_FABS
  return (fabs(x));
#else
  return ((0.0 <= x) ? x : (0.0 - x));
#endif
}



double
dk3ma_d_rint(double x)
{
#if DK3_HAVE_RINT
  return (rint(x));
#else
  return (floor(x + 0.5));
#endif
}



double
dk3ma_d_add_ok(double a, double b, int *ec)
{
  $? "+ dk3ma_d_add_ok %lg %lg", a, b
  if (NULL != ec) {
    if ((0.0 < a) && (0.0 < b)) {
      if ((dk3ma_i_max_double - a) < b) {
        $? "! overflow"
        *ec = DK3_ERROR_MATH_OVERFLOW;
      }
    } else {
      if ((0.0 > a) && (0.0 > b)) {
        if (   (   (-1.0 * dk3ma_i_max_double) - a )   > b   ) {
	  $? "! overflow"
	  *ec = DK3_ERROR_MATH_OVERFLOW;
	}
      }
    }
  } $? "- dk3ma_d_add_ok %lg", (a + b)
  return (a + b);
}



double
dk3ma_d_sub_ok(double a, double b, int *ec)
{
  $? "+ dk3ma_d_sub_ok %lg %lg", a, b
  if (NULL != ec) {
    if ((0.0 < a) && (0.0 > b)) {
      if(  (dk3ma_i_max_double + b)  <  a  ) {
        $? "! overflow"
        *ec = DK3_ERROR_MATH_OVERFLOW;
      }
    } else {
      if ((0.0 > a) && (0.0 < b)) {
        if (  (  (-1.0 * dk3ma_i_max_double)  +  b  )   >   a  ) {
	  $? "! overflow"
	  *ec = DK3_ERROR_MATH_OVERFLOW;
	}
      }
    }
  } $? "- dk3ma_d_sub_ok %lg", (a - b) 
  return (a - b);
}



double
dk3ma_d_mul_ok(double a, double b, int *ec)
{
  $? "+ dk3ma_d_mul_ok %lg %lg", a, b
  if (ec) {
    if (dk3ma_d_abs(a) > 1.0) {
      if (  (dk3ma_i_max_double / dk3ma_d_abs(a))  <  dk3ma_d_abs(b)  ) {
        $? "! overflow"
        *ec = DK3_ERROR_MATH_OVERFLOW;
      }
    }
  } $? "- dk3ma_d_mul_ok %lg", (a * b)
  return (a * b);
}



double
dk3ma_d_div_ok(double a, double b, int *ec)
{
#if DK3_HAVE_FPCLASSIFY && defined(FP_ZERO)
  if (FP_ZERO == fpclassify(b)) {
    if (NULL != ec) { *ec = DK3_ERROR_MATH_DIVZERO; }
    if (FP_ZERO == fpclassify(a)) {
      return (NAN);
    } else {
      if (0.0 <= a) {
        return (INFINITY);
      } else {
        return (-INFINITY);
      }
    }
  } else {
    if (dk3ma_d_abs(b) >= 1.0) {
      return (a / b);
    } else {
      if ((dk3ma_i_max_double * dk3ma_d_abs(b)) < dk3ma_d_abs(a)) {
        if (ec) {			$? "= ! division overflow"
	  *ec = DK3_ERROR_MATH_OVERFLOW;
	}
	if (((0.0 <= a) && (0.0 <= b)) || ((0.0 > a) && (0.0 > b))) {
	  return (INFINITY);
	} else {
	  return (-INFINITY);
	}
      } else {
        return (a / b);
      }
    }
  }
#else
#if DK3_ON_WINDOWS
  switch(_fpclass(b)) {
    case _FPCLASS_NZ:
    case _FPCLASS_PZ: {
      if (ec) {					$? "= ! division zero"
        *ec = DK3_ERROR_MATH_DIVZERO;
      }
      switch(_fpclass(a)) {
        case _FPCLASS_NZ:
	case _FPCLASS_PZ: {
#if defined(NAN)
	  return (NAN);
#else
	  return (dk3ma_i_max_double);
#endif
	} break;
	default: {
	  if (0.0 <= a) {
	    return (HUGE_VAL);
	  } else {
	    return (-HUGE_VAL);
	  }
	} break;
      }
    } break;
    default: {
      if (dk3ma_d_abs(b) >= 1.0) {
        return (a / b);
      } else {
        if ((dk3ma_i_max_double * dk3ma_d_abs(b)) < dk3ma_d_abs(a)) {
          if (ec) {				$? "= ! division overflow"
	    *ec = DK3_ERROR_MATH_OVERFLOW;
	  }
	  if (((0.0 <= a) && (0.0 <= b)) || ((0.0 > a) && (0.0 > b))) {
	    return (HUGE_VAL);
	  } else {
	    return (-HUGE_VAL);
	  }
	} else {
	  return (a / b);
	}
      }
    } break;
  }
#else
  if (dk3ma_d_abs(b) >= 1.0) {
    return (a / b);
  } else {
    if ((dk3ma_i_max_double * dk3ma_d_abs(b)) < dk3ma_d_abs(a)) {
      if (ec) {				$? "= ! division overflow"
        *ec = DK3_ERROR_MATH_OVERFLOW;
      }
      if (((0.0 <= a) && (0.0 <= b)) || ((0.0 > a) && (0.0 > b))) {
#ifdef INFINITY
	return (INFINITY);
#else
#ifdef HUGE_VAL
	return (HUGE_VAL);
#else
	return (dk3ma_i_max_double);
#endif
#endif
      } else {
#ifdef INFINITY
	return (-INFINITY);
#else
#ifdef HUGE_VAL
	return (-HUGE_VAL);
#else
	return (-1.0 * dk3ma_i_max_double);
#endif
#endif
      }
    } else {
      return (a / b);
    }
  }
#endif
#endif
}



double
dk3ma_d_square_ok(double x, int *ec)
{
#if 0
  double back = -1.0;
  $? "+ dk3ma_d_square_ok"
  if (0.0 <= x) {
    back = sqrt(x);
  } else {
    if (NULL != ec) {				$? "! out of range"
      *ec = DK3_ERROR_MATH_OUT_OF_RANGE;
    }
  } $? "- dk3ma_d_square_ok %lg", back
  return back;
#else
  return (dk3ma_d_mul_ok(x, x, ec));
#endif
}



int
dk3ma_d_equal(double a, double b, double epsilon)
{
  int	back	= 0;
  int	ec	= 0;
  if (dk3ma_d_abs(dk3ma_d_sub_ok(a, b, &ec)) < epsilon) {
    if (0 == ec) {
      back = 1;
    }
  }
  return back;
}



double
dk3ma_d_atan2(double y, double x)
{
#if DK3_HAVE_ATAN2
  double		 back;
  $? "+ dk3ma_atan2_ok y=%lg x=%lg", y, x
  back = atan2(y, x);	$? ". back = %lg", back
  while (back < 0.0) { back += (2.0 * M_PI); }
  while (back > (2.0 * M_PI)) { back -= (2.0 * M_PI); }
  $? "- dk3ma_atan2_ok (atan2) %lg", back
  return back;
#else
  double		 back = -5.0 * M_PI;
  double		 v;
  $? "+ dk3ma_atan2_ok y=%lg x=%lg", y, x
  v = dk3ma_d_div_ok(y, x, &mec);
  if (mec) {			$? "! division error"
    if(y < 0.0) {		$? ". negative y"
      back = 1.5 * M_PI;
    } else {			$? ". positive y"
      back = 0.5 * M_PI;
    }
  } else {			$? ". use atan"
    back = atan(v);
    if (x < 0.0) {		$? ". add pi"
      back += M_PI;
    }
  }	$? ". back = %lg", back
  while (back < 0.0) { back += (2.0 * M_PI); }
  while (back > (2.0 * M_PI)) { back -= (2.0 * M_PI); }
  $? "- dk3ma_atan2_ok (atan) %lg", back
  return back;
#endif
}



double
dk3ma_d_restrict_digits(double x, size_t n)
{
  double	back;
  double	newval;
  size_t	i;
  size_t	mult;
  int		ec	= 0;

  back = x;
  mult = 0;
  /*	Multiplications
  */
  for (i = 0; ((0 == ec) && (i < n)); i++) {
    newval = dk3ma_d_mul_ok(back, 10.0, &ec);
    if (0 == ec) {
      back = newval;
      mult++;
    }
  }
  /*	Rounding
  */
  back = dk3ma_d_rint(back);
  /*	Divisions
  */
  for (i = 0; i < mult; i++) {
    back = back / 10.0;
  }
  return back;
}



double
dk3ma_d_restrict_digits_ceil(double x, size_t n)
{
  double	back;
  double	newval;
  size_t	i;
  size_t	mult;
  int		ec	= 0;

  back = x;
  mult = 0;
  /*	Multiplications
  */
  for (i = 0; ((0 == ec) && (i < n)); i++) {
    newval = dk3ma_d_mul_ok(back, 10.0, &ec);
    if (0 == ec) {
      back = newval;
      mult++;
    }
  }
  /*	Rounding
  */
  back = ceil(back);
  /*	Divisions
  */
  for (i = 0; i < mult; i++) {
    back = back / 10.0;
  }
  return back;
}



double
dk3ma_d_restrict_digits_floor(double x, size_t n)
{
  double	back;
  double	newval;
  size_t	i;
  size_t	mult;
  int		ec	= 0;

  back = x;
  mult = 0;
  /*	Multiplications
  */
  for (i = 0; ((0 == ec) && (i < n)); i++) {
    newval = dk3ma_d_mul_ok(back, 10.0, &ec);
    if (0 == ec) {
      back = newval;
      mult++;
    }
  }
  /*	Rounding
  */
  back = floor(back);
  /*	Divisions
  */
  for (i = 0; i < mult; i++) {
    back = back / 10.0;
  }
  return back;
}