#![allow(clippy::extra_unused_type_parameters)]
use thermite::{
math::{TranscendentalMathWithPolicy as _, policy::Policy},
prelude::*,
register::FloatElement,
};
use crate::specialized::SpecializedSpecialMath;
pub const KIND_F: u8 = 1; pub const KIND_E: u8 = 2; pub const KIND_PI: u8 = 3; pub const KIND_D: u8 = 4;
macro_rules! sc {
($n:literal / $d:literal) => {
<E as FloatElement>::ConstRatio::<$n, $d>::VALUE
};
}
#[inline(always)]
pub fn agm_complete_ke<P, E, V>(k: V) -> (V, V)
where
P: Policy,
E: FloatElement,
V: FloatVector<Element = E>,
{
let mut a = V::ONE;
let b0 = k.one_minus_sq().sqrt();
let mut b = b0;
let mut c = k;
let mut sum = (c * c).scale(sc!(1 / 2));
let mut pow2 = V::ONE;
let thresh = V::SQRT_EPSILON; let mut iter = 0;
loop {
let an = (a + b).scale(sc!(1 / 2));
let bn = (a * b).sqrt();
c = (a - b).scale(sc!(1 / 2));
a = an;
b = bn;
sum = pow2.mul_adde(c * c, sum); pow2 = pow2 + pow2;
iter += 1;
if iter >= 24 || c.abs().cmp_le(a * thresh).all() {
break;
}
}
let k_int = V::FRAC_PI_2 / a;
let e_int = k_int.nmul_adde(sum, k_int);
if const { P::POLICY.check_overflow } {
let deg = b0.is_zero();
(deg.select(V::INFINITY, k_int), deg.select(V::ONE, e_int))
} else {
(k_int, e_int)
}
}
#[inline(always)]
fn two_or_more_zero<V: FloatVector>(x: V, y: V, z: V) -> V::Mask {
x.min(y).max(z.min(x.max(y))).is_zero()
}
pub trait EllipticConsts {
const CARLSON_THRESH: Self;
}
impl EllipticConsts for f32 {
const CARLSON_THRESH: f32 = 0.15637917816638947;
}
impl EllipticConsts for f64 {
const CARLSON_THRESH: f64 = 0.012674918778210762;
}
#[inline(always)]
fn carlson_thresh<V: FloatVector<Element: EllipticConsts>>() -> V {
V::splat(<V::Element as EllipticConsts>::CARLSON_THRESH)
}
#[inline(always)]
pub fn carlson_rf<P, E, V>(x: V, y: V, z: V) -> V
where
P: Policy,
E: FloatElement + EllipticConsts,
V: FloatVector<Element = E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let quarter = c!(1 / 4);
let thresh = carlson_thresh::<V>();
let mut xn = x;
let mut yn = y;
let mut zn = z;
let mut an = (x + y + z).scale(sc!(1 / 3));
let a0 = an;
let mut fmn = V::ONE; let q = (a0 - x).abs().max((a0 - y).abs()).max((a0 - z).abs()) / thresh;
let mut iter = 0;
loop {
let rx = xn.sqrt();
let ry = yn.sqrt();
let rz = zn.sqrt();
let lambda = rx.mul_adde(ry + rz, ry * rz); if const { V::HAS_TRUE_FMA } {
let lq = lambda * quarter;
an = an.mul_add(quarter, lq);
xn = xn.mul_add(quarter, lq);
yn = yn.mul_add(quarter, lq);
zn = zn.mul_add(quarter, lq);
} else {
an = (an + lambda) * quarter;
xn = (xn + lambda) * quarter;
yn = (yn + lambda) * quarter;
zn = (zn + lambda) * quarter;
}
fmn *= quarter;
iter += 1;
if iter >= 30 || (fmn * q).cmp_le(an).all() {
break;
}
}
let scale = fmn / an; let xd = (a0 - x) * scale;
let yd = (a0 - y) * scale;
let zd = -xd - yd;
let e2 = xd.mul_sube(yd, zd * zd); let e3 = xd * yd * zd;
let e2_2 = e2 * e2;
let lin = e2.mul_adde(c!(-1 / 10), V::ONE);
let lin = e3.mul_adde(c!(1 / 14), lin);
let quad = (e2 * e3).mul_adde(c!(-3 / 44), e2_2.scale(sc!(1 / 24)));
let quad = (e3 * e3).mul_adde(c!(3 / 104), quad);
let cub = (e2_2 * e3).mul_adde(c!(1 / 16), (e2_2 * e2).scale(sc!(-5 / 208)));
let poly = lin + (quad + cub);
let rf = poly / an.sqrt();
if const { P::POLICY.check_overflow } {
two_or_more_zero(x, y, z).select(V::INFINITY, rf)
} else {
rf
}
}
#[inline(always)]
pub fn carlson_rd<P, E, V>(x: V, y: V, z: V) -> V
where
P: Policy,
E: FloatElement + EllipticConsts,
V: FloatVector<Element = E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let quarter = c!(1 / 4);
let thresh = carlson_thresh::<V>();
let mut xn = x;
let mut yn = y;
let mut zn = z;
let mut an = ((x + y) + (z + z + z)).scale(sc!(1 / 5)); let a0 = an;
let mut sum = V::ZERO;
let mut fac = V::ONE; let q = (a0 - x).abs().max((a0 - y).abs()).max((a0 - z).abs()) / thresh;
let mut iter = 0;
loop {
let rx = xn.sqrt();
let ry = yn.sqrt();
let rz = zn.sqrt();
let lambda = rx.mul_adde(ry + rz, ry * rz); sum += fac / (rz * (zn + lambda));
if const { V::HAS_TRUE_FMA } {
let lq = lambda * quarter;
an = an.mul_add(quarter, lq);
xn = xn.mul_add(quarter, lq);
yn = yn.mul_add(quarter, lq);
zn = zn.mul_add(quarter, lq);
} else {
an = (an + lambda) * quarter;
xn = (xn + lambda) * quarter;
yn = (yn + lambda) * quarter;
zn = (zn + lambda) * quarter;
}
fac *= quarter;
iter += 1;
if iter >= 30 || (fac * q).cmp_le(an).all() {
break;
}
}
let scale = fac / an; let xd = (a0 - x) * scale;
let yd = (a0 - y) * scale;
let zd = (xd + yd).scale(sc!(-1 / 3));
let xy = xd * yd;
let zz = zd * zd;
let xy3 = xy.scale(sc!(3 / 1)); let e2 = zz.mul_adde(c!(-6 / 1), xy); let e3 = zz.mul_adde(c!(-8 / 1), xy3) * zd; let e4 = zz.mul_adde(c!(-3 / 1), xy3) * zz; let e5 = xy * (zz * zd);
let taylor = fac * rdj_poly::<E, V>(e2, e3, e4, e5) / (an * an.sqrt()); let rd = c!(3 / 1).mul_adde(sum, taylor);
if const { P::POLICY.check_overflow } {
two_or_more_zero(x, y, z).select(V::INFINITY, rd)
} else {
rd
}
}
#[inline(always)]
fn rdj_poly<E, V>(e2: V, e3: V, e4: V, e5: V) -> V
where
E: FloatElement,
V: FloatVector<Element = E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let e2_2 = e2 * e2;
let lin = e2.mul_adde(c!(-3 / 14), V::ONE);
let lin = e3.mul_adde(c!(1 / 6), lin);
let lin = e4.mul_adde(c!(-3 / 22), lin);
let lin = e5.mul_adde(c!(3 / 26), lin);
let quad = (e2 * e3).mul_adde(c!(-9 / 52), e2_2.scale(sc!(9 / 88)));
let quad = (e3 * e3).mul_adde(c!(3 / 40), quad);
let quad = (e2 * e4).mul_adde(c!(3 / 20), quad);
let cub = (e2_2 * e3).mul_adde(c!(45 / 272), (e2_2 * e2).scale(sc!(-1 / 16)));
let cub = (e3 * e4 + e2 * e5).mul_adde(c!(-9 / 68), cub);
lin + (quad + cub)
}
#[inline(always)]
pub fn carlson_rg<P, E, V>(x: V, y: V, z: V) -> V
where
P: Policy,
E: FloatElement + EllipticConsts,
V: FloatVector<Element = E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let lo = x.min(y).min(z);
let hi = x.max(y).max(z);
let mid = (x + y + z) - (lo + hi); let rf = carlson_rf::<P, E, V>(hi, lo, mid);
let rd = carlson_rd::<P, E, V>(hi, lo, mid);
let root = (hi * lo / mid).sqrt();
let prod = (hi - mid) * (lo - mid) * rd;
let rg = prod.mul_adde(c!(-1 / 3), mid.mul_adde(rf, root)).scale(sc!(1 / 2));
if const { P::POLICY.check_overflow } {
mid.is_zero().select(hi.sqrt().scale(sc!(1 / 2)), rg)
} else {
rg
}
}
#[inline(always)]
pub fn carlson_rc<P, E, V>(x: V, y: V) -> V
where
P: Policy,
E: FloatElement,
V: SpecializedSpecialMath<E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let d = y - x;
let absd = d.abs();
let (s, neg_arg, irx, scale) = if const { V::HAS_APPROX_RSQRT } {
let isad = absd.inverse_sqrt_p::<P>(); let irx = x.inverse_sqrt_p::<P>(); let iry = y.inverse_sqrt_p::<P>(); let sad = absd * isad; let rx = x * irx; (sad * irx, (rx + sad) * iry, irx, isad)
} else {
let sad = absd.sqrt(); let rx = x.sqrt();
let irx = rx.reciprocal_p::<P>(); (sad * irx, (rx + sad) / y.sqrt(), irx, sad)
};
let num = d.cmp_gt(V::ZERO).select(s.atan_p::<P>(), neg_arg.ln_p::<P>());
let closed = if const { V::HAS_APPROX_RSQRT } {
num * scale
} else {
num / scale
};
let t = d / x;
let mut res = closed;
let small = t.abs().cmp_lt(c!(1 / 128));
if const { P::POLICY.avoid_branching } || small.any() {
let series = t.poly_rev_p::<P, _>(&[
<E as FloatElement>::ConstRatio::<-1, 15>::VALUE,
<E as FloatElement>::ConstRatio::<1, 13>::VALUE,
<E as FloatElement>::ConstRatio::<-1, 11>::VALUE,
<E as FloatElement>::ConstRatio::<1, 9>::VALUE,
<E as FloatElement>::ConstRatio::<-1, 7>::VALUE,
<E as FloatElement>::ConstRatio::<1, 5>::VALUE,
<E as FloatElement>::ConstRatio::<-1, 3>::VALUE,
<E as FloatElement>::ConstRatio::<1, 1>::VALUE,
]) * irx;
res = small.select(series, res);
}
res
}
#[inline(always)]
pub fn carlson_rj<P, E, V>(x: V, y: V, z: V, p: V) -> V
where
P: Policy,
E: FloatElement + EllipticConsts,
V: SpecializedSpecialMath<E>,
{
macro_rules! c {
($n:literal / $d:literal) => {
V::splat(<E as FloatElement>::ConstRatio::<$n, $d>::VALUE)
};
}
let quarter = c!(1 / 4);
let thresh = carlson_thresh::<V>();
let lo = x.min(y).min(z);
let hi = x.max(y).max(z);
let mid = (x + y + z) - (lo + hi);
let neg = p.cmp_lt(V::ZERO);
let q = -p; let p_new = hi.mul_sube(lo + mid + q, lo * mid) / (hi + q);
let p_eff = neg.select(p_new, p);
let mut xn = lo;
let mut yn = mid;
let mut zn = hi;
let mut pn = p_eff;
let mut an = ((lo + mid + hi) + (p_eff + p_eff)).scale(sc!(1 / 5)); let a0 = an;
let mut rc_sum = V::ZERO;
let mut fmn = V::ONE; let qb = (a0 - lo)
.abs()
.max((a0 - mid).abs())
.max((a0 - hi).abs().max((a0 - p_eff).abs()))
/ thresh;
let mut iter = 0;
loop {
let rx = xn.sqrt();
let ry = yn.sqrt();
let rz = zn.sqrt();
let rp = pn.sqrt();
let dn = (rp + rx) * (rp + ry) * (rp + rz);
let inner = ry.mul_adde(rz, rx.mul_adde(ry + rz, pn)); let b = (rp * inner) / dn.scale(sc!(1 / 2));
rc_sum = (fmn / dn).mul_adde(carlson_rc::<P, E, V>(V::ONE, b), rc_sum);
let lambda = rx.mul_adde(ry + rz, ry * rz); if const { V::HAS_TRUE_FMA } {
let lq = lambda * quarter;
an = an.mul_add(quarter, lq);
xn = xn.mul_add(quarter, lq);
yn = yn.mul_add(quarter, lq);
zn = zn.mul_add(quarter, lq);
pn = pn.mul_add(quarter, lq);
} else {
an = (an + lambda) * quarter;
xn = (xn + lambda) * quarter;
yn = (yn + lambda) * quarter;
zn = (zn + lambda) * quarter;
pn = (pn + lambda) * quarter;
}
fmn *= quarter;
iter += 1;
if iter >= 30 || (fmn * qb).cmp_le(an).all() {
break;
}
}
let scale = fmn / an; let xd = (a0 - lo) * scale;
let yd = (a0 - mid) * scale;
let zd = (a0 - hi) * scale;
let pd = (xd + yd + zd).scale(sc!(-1 / 2));
let xyz = xd * yd * zd;
let pp = pd * pd;
let ppd = pp * pd; let sym = yd.mul_adde(zd, xd * (yd + zd)); let e2 = pp.mul_adde(c!(-3 / 1), sym); let pre3 = ppd.mul_adde(c!(4 / 1), xyz); let e3 = e2.mul_adde(pd + pd, pre3); let pre4 = ppd.mul_adde(c!(3 / 1), xyz + xyz) * pd; let e4 = e2.mul_adde(pp, pre4); let e5 = xyz * pp;
let taylor = fmn * rdj_poly::<E, V>(e2, e3, e4, e5) / (an * an.sqrt());
let rj = c!(6 / 1).mul_adde(rc_sum, taylor);
let out = if const { P::POLICY.avoid_branching } || neg.any() {
let rf = carlson_rf::<P, E, V>(lo, mid, hi);
let xy = lo * mid;
let xyz = xy * hi;
let pq = p_new * q;
let rc = carlson_rc::<P, E, V>(xy + pq, pq);
let root = (xyz / (xy + pq)).sqrt();
let val_neg = (p_new - hi).mul_adde(rj, root.mul_sube(rc, rf).scale(sc!(3 / 1))) / (hi + q);
neg.select(val_neg, rj)
} else {
rj
};
if const { P::POLICY.check_overflow } {
(mid.is_zero() | p.is_zero()).select(V::INFINITY, out)
} else {
out
}
}
#[inline(always)]
pub fn ellint_impl<P, E, V, const KIND: u8, const COMPLETE: bool>(phi: V, k: V, n: V) -> V
where
P: Policy,
E: FloatElement + EllipticConsts,
V: SpecializedSpecialMath<E>, {
const {
assert!(
KIND == KIND_F || KIND == KIND_E || KIND == KIND_D || KIND == KIND_PI,
"ellint_impl: KIND must be KIND_F, KIND_E, KIND_D, or KIND_PI"
);
}
if const { COMPLETE } {
if const { KIND == KIND_PI } {
let w = k.one_minus_sq(); let rf = carlson_rf::<P, E, V>(V::ZERO, w, V::ONE);
let rj = carlson_rj::<P, E, V>(V::ZERO, w, V::ONE, V::ONE - n);
n.scale(sc!(1 / 3)).mul_adde(rj, rf) } else {
let (kk, ee) = agm_complete_ke::<P, E, V>(k);
if const { KIND == KIND_F } {
kk
} else if const { KIND == KIND_E } {
ee
} else {
let d = (kk - ee) / (k * k);
if const { P::POLICY.check_overflow } {
k.is_zero().select(V::FRAC_PI_4, d)
} else {
d
}
}
}
} else {
let m = (phi * V::FRAC_1_PI).round();
let phi_red = m.nmul_adde(V::PI, phi);
let (s, cphi) = phi_red.sin_cos_p::<P>();
let c2 = cphi * cphi;
let k2 = k * k;
let w = k2.mul_adde(c2, k.one_minus_sq());
let rf = carlson_rf::<P, E, V>(c2, w, V::ONE);
let core = if const { KIND == KIND_F } {
s * rf
} else if const { KIND == KIND_PI } {
let pp = n.nmul_adde(s * s, V::ONE);
let rj = carlson_rj::<P, E, V>(c2, w, V::ONE, pp);
let s3 = s * s * s;
(n.scale(sc!(1 / 3)) * s3).mul_adde(rj, s * rf) } else {
let rd = carlson_rd::<P, E, V>(c2, w, V::ONE);
let s3 = s * s * s;
if const { KIND == KIND_E } {
(k2.scale(sc!(1 / 3)) * s3).nmul_adde(rd, s * rf)
} else {
s3.scale(sc!(1 / 3)) * rd
}
};
if const { P::POLICY.avoid_branching } || !m.is_zero().all() {
let complete = ellint_impl::<P, E, V, KIND, true>(phi, k, n);
m.is_zero().select(core, (m + m).mul_adde(complete, core))
} else {
core
}
}
}
pub trait CarlsonKind {
type Output;
fn eval<P: Policy>(self) -> Self::Output;
}
pub trait EllipticKind {
type Output;
fn eval<P: Policy>(self) -> Self::Output;
}
use thermite::math::scalar::Unwrap;
pub trait WrapTo {
type Wrapped: Unwrap<Unwrapped = Self>;
}
macro_rules! request_struct {
($(#[$meta:meta])* $name:ident { $($field:ident),* }) => {
#[derive(Debug, Clone, Copy)]
$(#[$meta])* pub struct $name<V> {
$(pub $field: V,)*
}
impl<V: Unwrap> Unwrap for $name<V> {
type Unwrapped = $name<<V as Unwrap>::Unwrapped>;
#[inline(always)]
fn wrap(value: Self::Unwrapped) -> Self {
$name { $($field: Unwrap::wrap(value.$field),)* }
}
#[inline(always)]
fn unwrap(self) -> Self::Unwrapped {
$name { $($field: self.$field.unwrap(),)* }
}
}
impl<E> WrapTo for $name<E>
where
E: FloatElement + thermite::register::FloatRegister<Storage = E>,
thermite::Vector<E>: Unwrap<Unwrapped = E>,
{
type Wrapped = $name<thermite::Vector<E>>;
}
};
}
macro_rules! decl_carlson {
($( $(#[$meta:meta])* struct $name:ident { $($field:ident),* } => $func:ident; )*) => {$(
request_struct! { $(#[$meta])* $name { $($field),* } }
impl<E, V> CarlsonKind for $name<V>
where
E: FloatElement + EllipticConsts,
V: FloatVector<Element = E> + SpecializedSpecialMath<E>,
{
type Output = V;
#[inline(always)]
fn eval<P: Policy>(self) -> V {
$func::<P, E, V>($(self.$field),*)
}
}
)*};
}
decl_carlson! {
struct CarlsonRf { x, y, z } => carlson_rf;
struct CarlsonRc { x, y } => carlson_rc;
struct CarlsonRd { x, y, z } => carlson_rd;
struct CarlsonRj { x, y, z, p } => carlson_rj;
struct CarlsonRg { x, y, z } => carlson_rg;
}
macro_rules! decl_ellint {
($( $(#[$meta:meta])* struct $name:ident { $($field:ident),* } = [$kind:expr, $complete:expr]($phi:ident, $k:ident, $n:ident); )*) => {$(
request_struct! { $(#[$meta])* $name { $($field),* } }
impl<E, V> EllipticKind for $name<V>
where
E: FloatElement + EllipticConsts,
V: FloatVector<Element = E> + SpecializedSpecialMath<E>,
{
type Output = V;
#[inline(always)]
fn eval<P: Policy>(self) -> V {
ellint_impl::<P, E, V, { $kind }, { $complete }>(self.$phi, self.$k, self.$n)
}
}
)*};
}
decl_ellint! {
struct EllintK { k } = [KIND_F, true](k, k, k);
struct EllintF { phi, k } = [KIND_F, false](phi, k, k);
struct EllintE { k } = [KIND_E, true](k, k, k);
struct EllintEInc { phi, k } = [KIND_E, false](phi, k, k);
struct EllintD { k } = [KIND_D, true](k, k, k);
struct EllintDInc { phi, k } = [KIND_D, false](phi, k, k);
struct EllintPi { n, k } = [KIND_PI, true](k, k, n);
struct EllintPiInc { n, phi, k } = [KIND_PI, false](phi, k, n);
}