library/compiler-builtins/libm/src/math/hypot.rs

/* SPDX-License-Identifier: MIT */
/* origin: core-math/src/binary64/hypot/hypot.c
 * Copyright (c) 2022 Alexei Sibidanov.
 * Ported to Rust in 2025, TG
 * Approximate CORE-MATH commit: 8ea8ea35c518
 */

//! Euclidian distance via the pythagorean theorem (`√(x2 + y2)`).
//!
//! Per IEEE 754-2019:
//!
//! - Domain: `[−∞, +∞] × [−∞, +∞]`
//! - `hypot(±0, ±0)` is +0
//! - `hypot(±∞, qNaN)` is +∞
//! - `hypot(qNaN, ±∞)` is +∞.
//! - May raise overflow or underflow

use super::sqrt;
#[allow(unused_imports)] // msrv compat
use super::support::Float;
use super::support::cold_path;

#[cfg_attr(assert_no_panic, no_panic::no_panic)]
pub fn hypot(x: f64, y: f64) -> f64 {
    return cr_hypot(x, y);
}

fn cr_hypot(mut x: f64, mut y: f64) -> f64 {
    let flag = get_flags();

    let xi = x.to_bits();
    let yi = y.to_bits();

    let emsk: u64 = 0x7ffu64 << 52;
    let mut ex: u64 = xi & emsk;
    let mut ey: u64 = yi & emsk;
    /* emsk corresponds to the upper bits of NaN and Inf (apart the sign bit) */
    x = x.abs();
    y = y.abs();
    if ex == emsk || ey == emsk {
        cold_path();

        /* Either x or y is NaN or Inf */
        let wx: u64 = xi << 1;
        let wy: u64 = yi << 1;
        let wm: u64 = emsk << 1;

        let one_inf = (wx == wm) ^ (wy == wm);
        let one_nan = x.is_nan() ^ y.is_nan();

        // let nqnn: i32 = (((wx >> 52) == 0xfff) ^ ((wy >> 52) == 0xfff)) as i32;
        // /* ninf is 1 when only one of x and y is +/-Inf
        // nqnn is 1 when only one of x and y is qNaN
        // IEEE 754 says that hypot(+/-Inf,qNaN)=hypot(qNaN,+/-Inf)=+Inf. */
        if one_inf && one_nan {
            return f64::INFINITY;
        }
        return x + y; /* inf, sNaN */
    }

    let u: f64 = x.max(y);
    let v: f64 = x.min(y);
    let mut xd: u64 = u.to_bits();
    let mut yd: u64 = v.to_bits();
    ey = yd;

    if ey >> 52 == 0 {
        cold_path();

        if yd == 0 {
            return f64::from_bits(xd);
        }

        ex = xd;

        if ex >> 52 == 0 {
            cold_path();

            if ex == 0 {
                return 0.0;
            }

            return as_hypot_denorm(ex, ey);
        }

        let nz: u32 = ey.leading_zeros();
        ey <<= nz - 11;
        ey &= u64::MAX >> 12;
        ey = ey.wrapping_sub(((nz as i64 - 12i64) << 52) as u64);
        let t = ey; // why did they do this?
        yd = t;
    }

    let de: u64 = xd.wrapping_sub(yd);
    if de > (27_u64 << 52) {
        cold_path();
        return hf64!("0x1p-27").fma(v, u);
    }

    let off: i64 = (0x3ff_i64 << 52) - (xd & emsk) as i64;
    xd = xd.wrapping_add(off as u64);
    yd = yd.wrapping_add(off as u64);
    x = f64::from_bits(xd);
    y = f64::from_bits(yd);
    let x2: f64 = x * x;
    let dx2: f64 = x.fma(x, -x2);
    let y2: f64 = y * y;
    let dy2: f64 = y.fma(y, -y2);
    let r2: f64 = x2 + y2;
    let ir2: f64 = 0.5 / r2;
    let dr2: f64 = ((x2 - r2) + y2) + (dx2 + dy2);
    let mut th: f64 = sqrt(r2);
    let rsqrt: f64 = th * ir2;
    let dz: f64 = dr2 - th.fma(th, -r2);
    let mut tl: f64 = rsqrt * dz;
    th = fasttwosum(th, tl, &mut tl);
    let mut thd: u64 = th.to_bits();
    let tld = tl.abs().to_bits();
    ex = thd;
    ey = tld;
    ex &= 0x7ff_u64 << 52;
    let aidr: u64 = ey.wrapping_add(0x3fe_u64 << 52).wrapping_sub(ex);
    let mid: u64 = (aidr.wrapping_sub(0x3c90000000000000).wrapping_add(16)) >> 5;
    if mid == 0 || !(0x39b0000000000000_u64..=0x3c9fffffffffff80_u64).contains(&aidr) {
        cold_path();
        thd = as_hypot_hard(x, y, flag).to_bits();
    }
    thd = thd.wrapping_sub(off as u64);
    if thd >= (0x7ff_u64 << 52) {
        cold_path();
        return as_hypot_overflow();
    }

    f64::from_bits(thd)
}

fn fasttwosum(x: f64, y: f64, e: &mut f64) -> f64 {
    let s: f64 = x + y;
    let z: f64 = s - x;
    *e = y - z;
    s
}

fn as_hypot_overflow() -> f64 {
    let z: f64 = hf64!("0x1.fffffffffffffp1023");
    let f = z + z;
    if f > z {
        // errno = ERANGE
    }
    f
}

/// Here the square root is refined by Newton iterations: x^2+y^2 is exact
/// and fits in a 128-bit integer, so the approximation is squared (which
/// also fits in a 128-bit integer), compared and adjusted if necessary using
/// the exact value of x^2+y^2.
fn as_hypot_hard(x: f64, y: f64, flag: FExcept) -> f64 {
    let op: f64 = 1.0 + hf64!("0x1p-54");
    let om: f64 = 1.0 - hf64!("0x1p-54");
    let mut xi: u64 = x.to_bits();
    let yi: u64 = y.to_bits();
    let mut bm: u64 = (xi & (u64::MAX >> 12)) | 1u64 << 52;
    let mut lm: u64 = (yi & (u64::MAX >> 12)) | 1u64 << 52;
    let be: i32 = (xi >> 52) as i32;
    let le: i32 = (yi >> 52) as i32;
    let ri: u64 = sqrt(x * x + y * y).to_bits();
    let bs: i32 = 2;
    let mut rm: u64 = ri & (u64::MAX >> 12);
    let mut re: i32 = (ri >> 52) as i32 - 0x3ff;
    rm |= 1u64 << 52;

    for _ in 0..3 {
        if rm == 1u64 << 52 {
            rm = u64::MAX >> 11;
            re -= 1;
        } else {
            cold_path();
            rm -= 1;
        }
    }

    bm <<= bs;
    let mut m2: u64 = bm.wrapping_mul(bm);
    let de: i32 = be - le;
    let mut ls: i32 = bs - de;

    if ls >= 0 {
        lm <<= ls;
        m2 = m2.wrapping_add(lm.wrapping_mul(lm));
    } else {
        cold_path();
        let lm2: u128 = (lm as u128) * (lm as u128);
        ls *= 2;
        m2 = m2.wrapping_add((lm2 >> -ls) as u64);
        m2 |= ((lm2 << (128 + ls)) != 0) as u64;
    }

    let k: i32 = bs + re;
    let mut d: i64;

    loop {
        rm += 1 + (rm >= (1u64 << 53)) as u64;
        let tm: u64 = rm << k;
        let rm2: u64 = tm.wrapping_mul(tm);
        d = m2 as i64 - rm2 as i64;

        if d <= 0 {
            break;
        }
    }

    if d == 0 {
        set_flags(flag);
    } else if op == om {
        let tm: u64 = (rm << k) - (1 << (k - (rm <= (1u64 << 53)) as i32));
        d = m2 as i64 - (tm.wrapping_mul(tm)) as i64;

        if d == 0 {
            cold_path();
            rm -= rm & 1;
        } else {
            rm = rm.wrapping_add((d >> 63) as u64);
        }
    } else {
        cold_path();
        rm -= ((op == 1.0) as u64) << (rm > (1u64 << 53)) as u32;
    }

    if rm >= (1u64 << 53) {
        rm >>= 1;
        re += 1;
    }

    let e: u64 = (be - 1 + re) as u64;
    xi = (e << 52) + rm;

    f64::from_bits(xi)
}

fn as_hypot_denorm(mut a: u64, mut b: u64) -> f64 {
    let op: f64 = 1.0 + hf64!("0x1p-54");
    let om: f64 = 1.0 - hf64!("0x1p-54");
    let af: f64 = a as i64 as f64;
    let bf: f64 = b as i64 as f64;
    a <<= 1;
    b <<= 1;
    // Is this casting right?
    let mut rm: u64 = sqrt(af * af + bf * bf) as u64;
    let tm: u64 = rm << 1;
    let mut d: i64 = (a.wrapping_mul(a) as i64)
        .wrapping_sub(tm.wrapping_mul(tm) as i64)
        .wrapping_add(b.wrapping_mul(b) as i64);
    let sd: i64 = d >> 63;
    let um: i64 = ((rm as i64) ^ sd) - sd;
    let mut drm: i64 = sd + 1;
    let mut dd: i64 = (um << 3) + 4;
    let mut p_d: i64;
    rm -= drm as u64;
    drm += sd;
    loop {
        p_d = d;
        rm = rm.wrapping_add(drm as u64);
        d = d.wrapping_sub(dd);
        dd = dd.wrapping_add(8);
        if (d ^ p_d) <= 0 {
            cold_path();
            break;
        }
    }
    p_d = (sd & d) + (!sd & p_d);
    if p_d != 0 {
        if op == om {
            let sum: i64 = p_d
                .wrapping_sub(4u64.wrapping_mul(rm) as i64)
                .wrapping_sub(1);

            if sum != 0 {
                rm = rm.wrapping_add((sum >> 63).wrapping_add(1) as u64);
            } else {
                cold_path();
                rm += rm & 1;
            }
        } else {
            cold_path();
            rm += (op > 1.0) as u64;
        }
    } else {
        cold_path();
    }

    let xi: u64 = rm;
    f64::from_bits(xi)
}

type FExcept = u32;

fn set_flags(_flag: FExcept) {}

fn get_flags() -> FExcept {
    0
}