lapack/experimental/xlapy3.cpp

#include <algorithm>
#include <cmath>

extern "C" {
float slapy3_(float* x, float* y, float* z);
double dlapy3_(double* x, double* y, double* z);
}

// Compute sqrt(x*x + y*y + z*z) taking care not to cause unnecessary overflow.
template <typename T>
T xlapy3(const T x, const T y, const T z) {
  const T xabs = std::abs(x);
  const T yabs = std::abs(y);
  const T zabs = std::abs(z);
  const T w = std::fmax(std::fmax(xabs, yabs), zabs);

  if (w == T(0)) {
    // W can be zero for max(0,nan,0) adding all three entries together will
    // make sure NaN will not disappear.
    return (xabs + yabs + zabs);
  }

  const T tx = xabs / w;
  const T ty = yabs / w;
  const T tz = zabs / w;
  return w * std::sqrt(tx * tx + ty * ty + tz * tz);
}

float slapy3_(float* x, float* y, float* z) { return xlapy3(*x, *y, *z); }
double dlapy3_(double* x, double* y, double* z) { return xlapy3(*x, *y, *z); }