use super::FixedVector;
use super::FixedMatrix;
use super::kernels;
use super::kernels::Orient;
use crate::fixed_point::fixed_num::{Compute, Fixed, FixedNum};
use crate::fixed_point::width::{Storage, WideInt};
use crate::fixed_point::core_types::errors::OverflowDetected;
fn exact_signed_sum<S: Storage, const F: u32>(
plus: &[(S, S)],
minus: &[(S, S)],
) -> Result<Compute<Fixed<S, F>>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let dot = |t: &[(S, S)]| {
let (a, b): (Vec<S>, Vec<S>) = t.iter().copied().unzip();
kernels::exact_dot::<Fixed<S, F>>(&a, &b)
};
Fixed::<S, F>::compute_checked_add(dot(plus)?, Fixed::<S, F>::compute_negate(dot(minus)?))
}
#[inline]
fn lift_to_triple<S: Storage, const F: u32>(c: Compute<Fixed<S, F>>) -> Orient<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
S::widen_product(c, S::widen(Fixed::one().raw()))
}
fn quotient<S: Storage, const F: u32>(num: Compute<Fixed<S, F>>, den: Compute<Fixed<S, F>>) -> Result<Fixed<S, F>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
if Fixed::<S, F>::compute_is_zero(&den) {
return Err(OverflowDetected::DomainError);
}
Ok(Fixed::from_raw(kernels::divide_to_storage_nearest::<Fixed<S, F>>(lift_to_triple(num), den)?))
}
#[inline]
fn orient_unit<S: Storage, const F: u32>() -> Orient<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
<Orient<Fixed<S, F>> as WideInt>::one()
}
#[inline]
fn orient_divmod<S: Storage, const F: u32>(n: Orient<Fixed<S, F>>, d: Orient<Fixed<S, F>>) -> (Orient<Fixed<S, F>>, Orient<Fixed<S, F>>)
where
Fixed<S, F>: FixedNum<Storage = S>,
{
n.divmod(d)
}
fn orient_mul<S: Storage, const F: u32>(x: Orient<Fixed<S, F>>, y: Orient<Fixed<S, F>>) -> Result<Orient<Fixed<S, F>>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let magnitude = |v: Orient<Fixed<S, F>>| if WideInt::is_negative(v) { -v } else { v };
if WideInt::bit_length(magnitude(x)) + WideInt::bit_length(magnitude(y)) > <Orient<Fixed<S, F>> as WideInt>::BITS - 1 {
return Err(OverflowDetected::TierOverflow);
}
Ok(x.mul_exact(y))
}
fn ratio_to_storage<S: Storage, const F: u32>(n: Orient<Fixed<S, F>>, d: Orient<Fixed<S, F>>) -> Result<S, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let zero = <Orient<Fixed<S, F>> as WideInt>::zero();
if d == zero {
return Err(OverflowDetected::DivisionByZero);
}
let negative = WideInt::is_negative(n) != WideInt::is_negative(d) && n != zero;
let na = if WideInt::is_negative(n) { -n } else { n };
let da = if WideInt::is_negative(d) { -d } else { d };
let (mut q, mut r) = orient_divmod(na, da);
if WideInt::bit_length(q) > <Orient<Fixed<S, F>> as WideInt>::BITS / 4 {
return Err(OverflowDetected::TierOverflow);
}
let unit = orient_unit::<S, F>();
for _ in 0..3 * F {
let rest = da - r;
if r >= rest {
q = q + q + unit;
r = r - rest;
} else {
q = q + q;
r = r + r;
}
}
let floor = if negative { -(if r != zero { q + unit } else { q }) } else { q };
kernels::narrow_triple_nearest::<Fixed<S, F>>(floor)
}
pub fn to_homogeneous<S: Storage, const F: u32>(v: &FixedVector<Fixed<S, F>>) -> FixedVector<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = v.len();
let mut h = FixedVector::<Fixed<S, F>>::new(n + 1);
for i in 0..n {
h[i] = v[i];
}
h[n] = Fixed::one();
h
}
pub fn from_homogeneous<S: Storage, const F: u32>(h: &FixedVector<Fixed<S, F>>) -> Result<FixedVector<Fixed<S, F>>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = h.len();
if n == 0 { return Err(OverflowDetected::DomainError); }
let w = h[n - 1];
if w.is_zero() {
return Err(OverflowDetected::DomainError);
}
let mut v = FixedVector::<Fixed<S, F>>::new(n - 1);
for i in 0..n - 1 {
v[i] = h[i] / w;
}
Ok(v)
}
pub fn is_at_infinity<S: Storage, const F: u32>(h: &FixedVector<Fixed<S, F>>, tol: Fixed<S, F>) -> bool
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = h.len();
if n == 0 { return true; }
h[n - 1].abs() < tol
}
pub fn projective_transform<S: Storage, const F: u32>(
h_matrix: &FixedMatrix<Fixed<S, F>>,
point: &FixedVector<Fixed<S, F>>,
) -> Result<FixedVector<Fixed<S, F>>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = point.len();
assert!(h_matrix.rows() == n + 1 && h_matrix.cols() == n + 1, "projective_transform: H must be (n+1)x(n+1)");
let hp: Vec<S> = to_homogeneous(point).iter().map(|x| x.raw()).collect();
let row = |i: usize| {
let h: Vec<S> = (0..=n).map(|j| h_matrix.get(i, j).raw()).collect();
kernels::exact_dot::<Fixed<S, F>>(&h, &hp)
};
let w = row(n)?;
if Fixed::<S, F>::compute_is_zero(&w) {
return Err(OverflowDetected::DomainError);
}
let mut v = FixedVector::<Fixed<S, F>>::new(n);
for i in 0..n {
v[i] = quotient(row(i)?, w)?;
}
Ok(v)
}
pub fn projective_transform_homogeneous<S: Storage, const F: u32>(
h_matrix: &FixedMatrix<Fixed<S, F>>,
point: &FixedVector<Fixed<S, F>>,
) -> FixedVector<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
h_matrix.mul_vector(point)
}
pub fn compose_projective<S: Storage, const F: u32>(
h1: &FixedMatrix<Fixed<S, F>>,
h2: &FixedMatrix<Fixed<S, F>>,
) -> FixedMatrix<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
h1 * h2
}
pub fn cross_ratio_1d<S: Storage, const F: u32>(
a: Fixed<S, F>,
b: Fixed<S, F>,
c: Fixed<S, F>,
d: Fixed<S, F>,
) -> Result<Fixed<S, F>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let (a, b, c, d) = (S::widen(a.raw()), S::widen(b.raw()), S::widen(c.raw()), S::widen(d.raw()));
let numer = S::widen_product(Fixed::<S, F>::compute_subtract(a, c), Fixed::<S, F>::compute_subtract(b, d));
let denom = S::widen_product(Fixed::<S, F>::compute_subtract(a, d), Fixed::<S, F>::compute_subtract(b, c));
if denom == <Orient<Fixed<S, F>> as WideInt>::zero() {
return Err(OverflowDetected::DomainError);
}
Ok(Fixed::from_raw(ratio_to_storage(numer, denom)?))
}
pub fn cross_ratio<S: Storage, const F: u32>(
a: &FixedVector<Fixed<S, F>>,
b: &FixedVector<Fixed<S, F>>,
c: &FixedVector<Fixed<S, F>>,
d: &FixedVector<Fixed<S, F>>,
) -> Result<Fixed<S, F>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = a.len();
assert!(b.len() == n && c.len() == n && d.len() == n, "cross_ratio: dimension mismatch");
let diff = |x: &FixedVector<Fixed<S, F>>| -> Vec<Compute<Fixed<S, F>>> {
(0..n).map(|i| Fixed::<S, F>::compute_subtract(S::widen(x[i].raw()), S::widen(a[i].raw()))).collect()
};
let dir = diff(b);
let project = |x: &FixedVector<Fixed<S, F>>| kernels::exact_dot_compute::<Fixed<S, F>>(&diff(x), &dir);
let tb = project(b)?;
if tb == <Orient<Fixed<S, F>> as WideInt>::zero() {
return Err(OverflowDetected::DomainError);
}
let (tc, td) = (project(c)?, project(d)?);
let numer = orient_mul(-tc, tb.add_exact(-td)?)?;
let denom = orient_mul(-td, tb.add_exact(-tc)?)?;
if denom == <Orient<Fixed<S, F>> as WideInt>::zero() {
return Err(OverflowDetected::DomainError);
}
Ok(Fixed::from_raw(ratio_to_storage(numer, denom)?))
}
pub fn stereo_project<S: Storage, const F: u32>(p: &FixedVector<Fixed<S, F>>) -> Result<FixedVector<Fixed<S, F>>, OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let n = p.len();
if n < 2 { return Err(OverflowDetected::DomainError); }
let pn = p[n - 1];
let denom = Fixed::one() - pn;
if denom.is_zero() {
return Err(OverflowDetected::DomainError); }
let mut x = FixedVector::<Fixed<S, F>>::new(n - 1);
for i in 0..n - 1 {
x[i] = p[i] / denom;
}
Ok(x)
}
pub fn stereo_unproject<S: Storage, const F: u32>(x: &FixedVector<Fixed<S, F>>) -> FixedVector<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
const OVERFLOW: &str = "stereo_unproject: |x|^2 exceeds the compute tier";
let n = x.len();
let one = Fixed::<S, F>::make_compute_int(1);
let x_raw: Vec<S> = (0..n).map(|i| x[i].raw()).collect();
let x_sq = kernels::exact_dot::<Fixed<S, F>>(&x_raw, &x_raw).expect(OVERFLOW);
let denom = Fixed::<S, F>::compute_add(one, x_sq);
let mut p = FixedVector::<Fixed<S, F>>::new(n + 1);
for i in 0..n {
let numer = S::widen_product(Fixed::<S, F>::make_compute_int(2), S::widen(x[i].raw()));
p[i] = Fixed::from_raw(kernels::divide_to_storage_nearest::<Fixed<S, F>>(numer, denom).expect(OVERFLOW));
}
p[n] = quotient(Fixed::<S, F>::compute_subtract(x_sq, one), denom).expect(OVERFLOW);
p
}
#[derive(Clone, Copy, Debug)]
pub struct Moebius<T> {
pub a: T,
pub b: T,
pub c: T,
pub d: T,
}
impl<S: Storage, const F: u32> Moebius<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
pub fn new(a: Fixed<S, F>, b: Fixed<S, F>, c: Fixed<S, F>, d: Fixed<S, F>) -> Self {
Self { a, b, c, d }
}
pub fn identity() -> Self {
Self {
a: Fixed::one(),
b: Fixed::ZERO,
c: Fixed::ZERO,
d: Fixed::one(),
}
}
pub fn apply(&self, x: Fixed<S, F>) -> Result<Fixed<S, F>, OverflowDetected> {
let one = Fixed::one().raw();
let x = x.raw();
let numer = exact_signed_sum(&[(self.a.raw(), x), (self.b.raw(), one)], &[])?;
let denom = exact_signed_sum(&[(self.c.raw(), x), (self.d.raw(), one)], &[])?;
quotient(numer, denom)
}
pub fn compose(&self, other: &Moebius<Fixed<S, F>>) -> Moebius<Fixed<S, F>> {
let pair = |p: Fixed<S, F>, q: Fixed<S, F>, r: Fixed<S, F>, s: Fixed<S, F>| {
let sum = exact_signed_sum(&[(p.raw(), q.raw()), (r.raw(), s.raw())], &[])
.expect("Moebius::compose: coefficient exceeds the compute tier");
Fixed::from_raw(kernels::round_to_storage::<Fixed<S, F>>(sum))
};
Moebius {
a: pair(self.a, other.a, self.b, other.c),
b: pair(self.a, other.b, self.b, other.d),
c: pair(self.c, other.a, self.d, other.c),
d: pair(self.c, other.b, self.d, other.d),
}
}
pub fn inverse(&self) -> Moebius<Fixed<S, F>> {
Moebius {
a: self.d,
b: -self.b,
c: -self.c,
d: self.a,
}
}
pub fn determinant(&self) -> Fixed<S, F> {
let det = exact_signed_sum(&[(self.a.raw(), self.d.raw())], &[(self.b.raw(), self.c.raw())])
.expect("Moebius::determinant exceeds the compute tier");
Fixed::from_raw(kernels::round_to_storage::<Fixed<S, F>>(det))
}
pub fn to_matrix(&self) -> FixedMatrix<Fixed<S, F>> {
FixedMatrix::<Fixed<S, F>>::from_slice(2, 2, &[self.a, self.b, self.c, self.d])
}
}
#[derive(Clone, Copy, Debug)]
pub struct MoebiusComplex<T> {
pub a: (T, T), pub b: (T, T),
pub c: (T, T),
pub d: (T, T),
}
type Complex<S, const F: u32> = (Fixed<S, F>, Fixed<S, F>);
fn complex_mul_add_exact<S: Storage, const F: u32>(p: Complex<S, F>, q: Complex<S, F>, r: Complex<S, F>, s: Complex<S, F>) -> Result<(Compute<Fixed<S, F>>, Compute<Fixed<S, F>>), OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let re = exact_signed_sum(
&[(p.0.raw(), q.0.raw()), (r.0.raw(), s.0.raw())],
&[(p.1.raw(), q.1.raw()), (r.1.raw(), s.1.raw())],
)?;
let im = exact_signed_sum(
&[(p.0.raw(), q.1.raw()), (p.1.raw(), q.0.raw()), (r.0.raw(), s.1.raw()), (r.1.raw(), s.0.raw())],
&[],
)?;
Ok((re, im))
}
fn complex_div<S: Storage, const F: u32>(
a: (Compute<Fixed<S, F>>, Compute<Fixed<S, F>>),
b: (Compute<Fixed<S, F>>, Compute<Fixed<S, F>>),
) -> Result<(Fixed<S, F>, Fixed<S, F>), OverflowDetected>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
let denom = S::widen_product(b.0, b.0).add_exact(S::widen_product(b.1, b.1))?;
if denom == <Orient<Fixed<S, F>> as WideInt>::zero() {
return Err(OverflowDetected::DomainError);
}
let re = S::widen_product(a.0, b.0).add_exact(S::widen_product(a.1, b.1))?;
let im = S::widen_product(a.1, b.0).add_exact(-S::widen_product(a.0, b.1))?;
Ok((
Fixed::from_raw(ratio_to_storage(re, denom)?),
Fixed::from_raw(ratio_to_storage(im, denom)?),
))
}
impl<S: Storage, const F: u32> MoebiusComplex<Fixed<S, F>>
where
Fixed<S, F>: FixedNum<Storage = S>,
{
pub fn new(
a: (Fixed<S, F>, Fixed<S, F>),
b: (Fixed<S, F>, Fixed<S, F>),
c: (Fixed<S, F>, Fixed<S, F>),
d: (Fixed<S, F>, Fixed<S, F>),
) -> Self {
Self { a, b, c, d }
}
pub fn apply(
&self,
z: (Fixed<S, F>, Fixed<S, F>),
) -> Result<(Fixed<S, F>, Fixed<S, F>), OverflowDetected> {
let one = (Fixed::one(), Fixed::ZERO);
let numer = complex_mul_add_exact(self.a, z, self.b, one)?;
let denom = complex_mul_add_exact(self.c, z, self.d, one)?;
complex_div(numer, denom)
}
pub fn compose(&self, other: &MoebiusComplex<Fixed<S, F>>) -> MoebiusComplex<Fixed<S, F>> {
let term = |p: Complex<S, F>, q: Complex<S, F>, r: Complex<S, F>, s: Complex<S, F>| {
let (re, im) = complex_mul_add_exact(p, q, r, s)
.expect("MoebiusComplex::compose: coefficient exceeds the compute tier");
(Fixed::from_raw(kernels::round_to_storage::<Fixed<S, F>>(re)), Fixed::from_raw(kernels::round_to_storage::<Fixed<S, F>>(im)))
};
MoebiusComplex {
a: term(self.a, other.a, self.b, other.c),
b: term(self.a, other.b, self.b, other.d),
c: term(self.c, other.a, self.d, other.c),
d: term(self.c, other.b, self.d, other.d),
}
}
pub fn inverse(&self) -> MoebiusComplex<Fixed<S, F>> {
MoebiusComplex {
a: self.d,
b: (-self.b.0, -self.b.1),
c: (-self.c.0, -self.c.1),
d: self.a,
}
}
}